Suport Imu and battery

README.md

@@ -12,6 +12,12 @@ This is tested on Kinetic/Ubuntu16.04 and Android only.
  * (Android)
  * (Cozmo SDK 0.10)
 
+### Note
+
+Cozmo SDK will become idle mode if the message is not send to cozmo for a few minutes.
+You need send some messages repeatedly if you don't want to use idle mode.
+I mean that when the camera has been stopped, try to send some command like /cmd_vel.
+
 ## Super hack to run rospy from python3
 
 This is not recommended as usual, but it seems difficult to run rospy from python3 normally. (It requires full recompile of all ROS packages.)
@@ -25,19 +31,19 @@ sudo pip3 install rospkg catkin_pkg
 
 ## TODO
 
-* camera stops after a few minutes.
 * difficult to use tf2 on python3, it has tf2.so. How can we use?
 transform_stamped_to_tf2.py will convert the transform message to tf2 message.
 * use trajectory_msgs to command head angle and lift height.
-* publish battery state
 
 ## Pub/Sub
 
 ### Publish
 
  * /image (sensor_msgs/Image) : camera image from cozmo. This is gray scale, but the format is rgb8.
  * /joint_states (sensor_msgs/JointState) : This contains head angle and lift height
- * /transforms (geometry_msgs/TransformStamped) : poses of visialbe cubes and cozmo
+ * /transforms (geometry_msgs/TransformStamped) : poses of visialbe cubes and cozmo. if you need /tf, use transform_stamped_to_tf2.py
+ * /imu (sensor_msgs/Imu) : Imu mounted on cozmo head
+ * /battery (sensor_msgs/BatteryState) : battery voltage and charging status
 
 ### Subscribe
 

nodes/cozmo_driver.py

@@ -14,6 +14,8 @@
 from std_msgs.msg import Float64
 from std_msgs.msg import ColorRGBA
 from sensor_msgs.msg import Image
+from sensor_msgs.msg import BatteryState
+from sensor_msgs.msg import Imu
 from sensor_msgs.msg import JointState
 
 
@@ -24,30 +26,35 @@ def __init__(self, coz):
         self._cozmo = coz
         self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback)
         self._say_sub = rospy.Subscriber('say', String, self._say_callback)
-        self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head)
-        self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift)
+        self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head,
+                                          queue_size=1)
+        self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift,
+                                          queue_size=1)
         # TODO: better to use array (length=5)
         self._backpack_led_sub = rospy.Subscriber(
-            'backpack_led', ColorRGBA, self._set_backpack_led)
+            'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1)
         self._image_pub = rospy.Publisher('image', Image, queue_size=100)
         self._joint_state_pub = rospy.Publisher('joint_states', JointState,
                                                 queue_size=100)
+        self._imu_pub = rospy.Publisher('imu', Imu, queue_size=100)
+        self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=100)
         self._tf_br = rospy.Publisher('transforms', TransformStamped,
                                       queue_size=100)
 
     def _move_head(self, cmd):
         action = self._cozmo.set_head_angle(radians(cmd.data), duration=0.1,
                                             in_parallel=True)
-        # action.wait_for_completed()
+        action.wait_for_completed()
 
     def _move_lift(self, cmd):
-        action = self._cozmo.set_lift_height(height=cmd.data * 10, duration=0.1,
+        action = self._cozmo.set_lift_height(height=cmd.data * 10, duration=0.2,
                                              in_parallel=True)
-        # action.wait_for_completed()
+        action.wait_for_completed()
 
     def _set_backpack_led(self, msg):
         light = cozmo.lights.Light(cozmo.lights.Color(
-            rgba=[int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]])
+            rgba=[int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]),
+                                   on_period_ms=1)
         self._cozmo.set_all_backpack_lights(light)
 
     def _twist_callback(self, cmd):
@@ -57,7 +64,7 @@ def _twist_callback(self, cmd):
         self._cozmo.drive_wheels(lv, rv)
 
     def _say_callback(self, msg):
-        self._cozmo.say_text(msg.data)
+        self._cozmo.say_text(msg.data).wait_for_completed()
 
     def _publish_objects(self):
         def convert_pose_to_tf_msg(pose):
@@ -75,8 +82,8 @@ def convert_pose_to_ros_msg(pose, name):
             # TODO: use TransformStamped with tf2
             transform_msg = TransformStamped()
             transform_msg.header.stamp = rospy.Time.now()
-            transform_msg.header.frame_id = name
-            transform_msg.child_frame_id = 'map'
+            transform_msg.header.frame_id = 'map'
+            transform_msg.child_frame_id = name
             transform_msg.transform = convert_pose_to_tf_msg(pose)
             return transform_msg
 
@@ -112,12 +119,41 @@ def _publish_joint_state(self):
         js.effort = [0.0, 0.0]
         self._joint_state_pub.publish(js)
 
-    def spin(self):
-        r = rospy.Rate(10)
+    def _publish_imu(self):
+        imu = Imu()
+        imu.header.stamp = rospy.Time.now()
+        imu.header.frame_id = 'cozmo'
+        imu.orientation.w = self._cozmo.pose.rotation.q0
+        imu.orientation.x = self._cozmo.pose.rotation.q1
+        imu.orientation.y = self._cozmo.pose.rotation.q2
+        imu.orientation.z = self._cozmo.pose.rotation.q3
+        imu.angular_velocity.x = self._cozmo.gyro.x
+        imu.angular_velocity.y = self._cozmo.gyro.y
+        imu.angular_velocity.z = self._cozmo.gyro.z
+        imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
+        imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
+        imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
+        self._imu_pub.publish(imu)
+
+    def _publish_battery(self):
+        battery = BatteryState()
+        battery.header.stamp = rospy.Time.now()
+        battery.voltage = self._cozmo.battery_voltage
+        battery.present = True
+        if self._cozmo.is_on_charger: # is_charging always return False
+            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
+        else:
+            battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
+        self._battery_pub.publish(battery)
+
+    def spin(self, spin_hz=10.0):
+        r = rospy.Rate(spin_hz)
         while not rospy.is_shutdown():
             self._publish_image()
             self._publish_objects()
             self._publish_joint_state()
+            self._publish_imu()
+            self._publish_battery()
             r.sleep()
         self._cozmo.stop_all_motors()