Use depth_image_proc to produce correct pointclouds

Disable realsense gazebo plugin pointcloud simulation
The plugin doesn't actually register the depth data to the RGB frame, which causes the color information to misalign with depth boundaries, especially when the object is close to the camera
depth_image_proc also produces the intermediate "aligned_depth_to_color" topics like the real camera now
See pal-robotics/realsense_gazebo_plugin#31

launch/include/spawn_stretch.xml

@@ -62,6 +62,28 @@
     <param name="initial_mode" value="Default"/>
   </node>
 
+  <group ns="camera" if="$(arg realsense)">
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />
+
+    <node pkg="nodelet" type="nodelet" name="nodelet1"
+          args="load depth_image_proc/register nodelet_manager">
+      <remap from="rgb/camera_info" to="color/camera_info"/>
+      <remap from="depth/camera_info" to="depth/camera_info"/>
+      <remap from="depth/image_rect" to="depth/image_raw"/>
+      <remap from="depth_registered/camera_info" to="aligned_depth_to_color/camera_info"/>
+      <remap from="depth_registered/image_rect" to="aligned_depth_to_color/image_raw"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet2"
+          args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager">
+      <remap from="rgb/image_rect_color" to="color/image_raw"/>
+      <remap from="rgb/camera_info" to="color/camera_info"/>
+      <remap from="depth_registered/camera_info" to="aligned_depth_to_color/camera_info"/>
+      <remap from="depth_registered/image_rect" to="aligned_depth_to_color/image_raw"/>
+      <remap from="depth_registered/points" to="depth/color/points"/>
+    </node>
+  </group>
+
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
   <!--include file="$(find stretch_moveit_config)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>

robots/forky.gazebo.xacro

@@ -394,7 +394,7 @@
           <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
           <rangeMinDepth>0.1</rangeMinDepth>
           <rangeMaxDepth>10</rangeMaxDepth>
-          <pointCloud>1</pointCloud>
+          <pointCloud>0</pointCloud>
           <pointCloudTopicName>depth/color/points</pointCloudTopicName>
           <pointCloudCutoff>0.15</pointCloudCutoff>
           <pointCloudCutoffMax>10</pointCloudCutoffMax>

scripts/follow_joint_trajectory_server

@@ -351,6 +351,8 @@ class FollowJointTrajectoryServer(object):
             # print(feedback_msg)
             self._as.publish_feedback(feedback_msg)
         self.base.stopped = False
+        # Manually reset this bit
+        # self._as.preempt_request = False
         rospy.loginfo(f"{goal.trajectory.joint_names} {goal.trajectory.points[0].positions} {goal.trajectory.points[0].velocities}")
 
         modified = goal.trajectory