diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py index e6b7276..90904ec 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -20,10 +20,11 @@ """ import rospy -from sensor_msgs.msg import Range +from sensor_msgs.msg import Range, Imu from ros_arduino_msgs.msg import * from ros_arduino_msgs.srv import * -from math import pow +from math import pow, radians +from tf.transformations import quaternion_from_euler LOW = 0 HIGH = 1 @@ -38,6 +39,7 @@ class MessageType: FLOAT = 3 INT = 4 BOOL = 5 + IMU = 6 class Sensor(object): def __init__(self, device, name, pin=None, rate=0, direction="input", frame_id="base_link", **kwargs): @@ -58,25 +60,22 @@ class Sensor(object): now = rospy.Time.now() if now > self.t_next: if self.direction == "input": - try: - self.value = self.read_value() - except: - return + self.value = self.read_value() else: - try: - self.ack = self.write_value() - except: - return + self.ack = self.write_value() # For range sensors, assign the value to the range message field if self.message_type == MessageType.RANGE: self.msg.range = self.value - else: + elif self.message_type != MessageType.IMU: self.msg.value = self.value # Add a timestamp and publish the message self.msg.header.stamp = rospy.Time.now() - self.pub.publish(self.msg) + try: + self.pub.publish(self.msg) + except: + rospy.logerr("Invalid value read from sensor: " + str(self.name)) self.t_next = now + self.t_delta @@ -286,6 +285,64 @@ class GP2D12(IRSensor): return distance +class IMU(Sensor): + def __init__(self,*args, **kwargs): + super(IMU, self).__init__(*args, **kwargs) + + self.message_type = MessageType.IMU + + self.rpy = None + + self.msg = Imu() + self.msg.header.frame_id = self.frame_id + + self.msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] + self.msg.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] + self.msg.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e6, 0, 0, 0, 1e6] + + # Create the publisher + if self.rate != 0: + self.pub = rospy.Publisher("~sensor/" + self.name, Imu, queue_size=5) + + def read_value(self): + ''' + IMU data is assumed to be returned in the following order: + + [ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch] + + where a stands for accelerometer, g for gyroscope and m for magnetometer. + The last value uh stands for "unified heading" that some IMU's compute + from both gyroscope and compass data. + + ''' + data = self.device.get_imu_data() + + try: + ax, ay, az, gx, gy, gz, mx, my, mz, roll, pitch, ch = data + except: + rospy.logerr("Invalid value read from sensor: " + str(self.name)) + return None + + roll = radians(roll) + pitch = -radians(pitch) + + self.msg.linear_acceleration.x = radians(ax) + self.msg.linear_acceleration.y = radians(ay) + self.msg.linear_acceleration.z = radians(az) + + self.msg.angular_velocity.x = radians(gx) + self.msg.angular_velocity.y = radians(gy) + self.msg.angular_velocity.z = radians(gz) + + if ch != -999: + yaw = -radians(uh) + else: + yaw = -radians(mz) + + (self.msg.orientation.x, self.msg.orientation.y, self.msg.orientation.z, self.msg.orientation.w) = quaternion_from_euler(roll, pitch, yaw) + + return data + class PololuMotorCurrent(AnalogFloatSensor): def __init__(self, *args, **kwargs): super(PololuMotorCurrent, self).__init__(*args, **kwargs) 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 aed4949..38f3278 100755 --- a/ros_arduino_python/src/ros_arduino_python/base_controller.py +++ b/ros_arduino_python/src/ros_arduino_python/base_controller.py @@ -39,6 +39,8 @@ class BaseController: self.timeout = rospy.get_param("~base_controller_timeout", 1.0) self.odom_linear_scale_correction = rospy.get_param("~odom_linear_scale_correction", 1.0) 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 pid_params = dict() @@ -118,6 +120,7 @@ class BaseController: 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)) def poll(self): now = rospy.Time.now() @@ -166,13 +169,14 @@ class BaseController: quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. - self.odomBroadcaster.sendTransform( - (self.x, self.y, 0), - (quaternion.x, quaternion.y, quaternion.z, quaternion.w), - rospy.Time.now(), - self.base_frame, - "odom" - ) + if self.publish_odom_base_transform: + self.odomBroadcaster.sendTransform( + (self.x, self.y, 0), + (quaternion.x, quaternion.y, quaternion.z, quaternion.w), + rospy.Time.now(), + self.base_frame, + "odom" + ) odom = Odometry() odom.header.frame_id = "odom" @@ -198,9 +202,9 @@ class BaseController: odom.pose.covariance[7] = 0.1 odom.pose.covariance[35] = 0.05 if self.use_imu_heading else 0.2 - odom.pose.covariance[14] = float_info.max # set a non-zero covariance on unused - odom.pose.covariance[21] = float_info.max # dimensions (z, pitch and roll); this - odom.pose.covariance[28] = float_info.max # is a requirement of robot_pose_ekf + 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 self.odomPub.publish(odom)