Fix depth img encoding

launch/env.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="sim" default="true" />
+  <arg name="sim" />
   <arg name="launch_rviz" default="false" />
 
   <!-- Load parameters -->

scripts/bt_sim_node.py

@@ -317,7 +317,7 @@ class CameraPlugin(Plugin):
         if self.cam_noise:
             depth = apply_noise(depth)
 
-        msg = self.cv_bridge.cv2_to_imgmsg(depth)
+        msg = self.cv_bridge.cv2_to_imgmsg((1000 * depth).astype(np.uint16))
         msg.header.stamp = stamp
         self.depth_pub.publish(msg)
 

src/active_grasp/controller.py

@@ -112,7 +112,7 @@ class GraspController:
     def get_state(self):
         q, _ = self.arm.get_state()
         msg = copy.deepcopy(self.latest_depth_msg)
-        img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
+        img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
         pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
         return img, pose, q