yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Added diagnostics and default covariance values to base controller module

Browse Source
This commit is contained in:
Patrick Goebel 2016-09-12 06:06:53 -07:00
parent e7bfde4b80
commit 9d0ed30c9c
1 changed files with 46 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
ros_arduino_python/src/ros_arduino_python/base_controller.py
View File

@ -22,18 +22,20 @@
"""
import rospy
import os
import sys, os
from math import sin, cos, pi
from geometry_msgs.msg import Quaternion, Twist, Pose
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster
from sys import float_info
from ros_arduino_python.diagnostics import DiagnosticsUpdater
""" Class to receive Twist commands and publish Odometry data """
class BaseController:
def __init__(self, arduino, base_frame):
def __init__(self, arduino, base_frame, name='base_controller'):
self.arduino = arduino
self.name = name
self.base_frame = base_frame
self.rate = float(rospy.get_param("~base_controller_rate", 10))
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
@ -41,7 +43,9 @@ class BaseController:
self.odom_angular_scale_correction = rospy.get_param("~odom_angular_scale_correction", 1.0)
self.use_imu_heading = rospy.get_param("~use_imu_heading", False)
self.publish_odom_base_transform = rospy.get_param("~publish_odom_base_transform", True)
self.stopped = False
self.current_speed = Twist()
pid_params = dict()
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
@ -55,19 +59,28 @@ class BaseController:
self.accel_limit = rospy.get_param('~accel_limit', 1.0)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
# Default error threshold (percent) before getting a diagnostics warning
self.base_diagnotics_error_threshold = rospy.get_param("~base_diagnotics_error_threshold", 10)
# Diagnostics update rate
self.base_diagnotics_rate = rospy.get_param("~base_diagnotics_rate", 1.0)
# Create the diagnostics updater for the Arduino device
self.diagnostics = DiagnosticsUpdater(self, self.name, self.base_diagnotics_error_threshold, self.base_diagnotics_rate)
# Set up PID parameters and check for missing values
self.setup_pid(pid_params)
# How many encoder ticks are there per meter?
self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
# What is the maximum acceleration we will tolerate when changing wheel speeds?
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
# Track how often we get a bad encoder count (if any)
self.bad_encoder_count = 0
now = rospy.Time.now()
self.then = now # time for determining dx/dy
self.t_delta = rospy.Duration(1.0 / self.rate)
@ -87,14 +100,14 @@ class BaseController:
# Subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
# Clear any old odometry info
self.arduino.reset_encoders()
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
@ -119,16 +132,22 @@ class BaseController:
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko))
if self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko):
rospy.loginfo("PID parameters update to: Kp=%d, Kd=%d, Ki=%d, Ko=%d" %(self.Kp, self.Kd, self.Ki, self.Ko))
else:
rospy.logerr("Updating PID parameters failed!")
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
self.diagnostics.reads += 1
self.diagnostics.total_reads += 1
left_enc, right_enc = self.arduino.get_encoder_counts()
self.diagnostics.freq_diag.tick()
except:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
@ -190,8 +209,12 @@ class BaseController:
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.current_speed = Twist()
self.current_speed.linear.x = vxy
self.current_speed.angular.z = vth
"""
Covariance values taken from Kobuki node odometry.hpp at:
Covariance values taken from Kobuki node odometry.cpp at:
https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
@ -200,11 +223,15 @@ class BaseController:
"""
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2
if self.use_imu_heading:
#odom.pose.covariance[35] = 0.2
odom.pose.covariance[35] = 0.05
else:
odom.pose.covariance[35] = 0.05
odom.pose.covariance[14] = 1e6 # set a non-zero covariance on unused
odom.pose.covariance[21] = 1e6 # dimensions (z, pitch and roll); this
odom.pose.covariance[28] = 1e6 # is a requirement of robot_pose_ekf
odom.pose.covariance[14] = sys.float_info.max # set a non-zero covariance on unused
odom.pose.covariance[21] = sys.float_info.max # dimensions (z, pitch and roll); this
odom.pose.covariance[28] = sys.float_info.max # is a requirement of robot_pose_ekf
self.odomPub.publish(odom)
@ -266,4 +293,5 @@ class BaseController:
self.x = 0.0
self.y = 0.0
self.th = 0.0