diff --git a/ros_arduino_python/src/ros_arduino_python/base_controller.py b/ros_arduino_python/src/ros_arduino_python/base_controller.py index 43bd46d..0fa0c12 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -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 +