Added initial IMU support to arduino_sensors and base_controller

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)

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)