Merge pull request #111 from hello-robot/feature/separated_launch_files2

Separate drivers into stretch.launch
hello-robot · Sep 14, 2023 · 5e709b1 · 5e709b1
2 parents c126b4c + d8685e8
commit 5e709b1
Show file tree

Hide file tree

Showing 14 changed files with 715 additions and 110 deletions.
diff --git a/stretch_core/README.md b/stretch_core/README.md
@@ -8,7 +8,7 @@
 
 *detect_aruco_markers* : node that detects and estimates the pose of ArUco markers, including the markers on the robot's body
 
-*d435i_** : various nodes to help use the Stretch RE1's 3D camera
+*d435i_* : various nodes to help use the Stretch RE1's 3D camera
 
 *keyboard_teleop* : node that provides a keyboard interface to control the robot's joints
 

diff --git a/stretch_core/launch/d435i_basic.launch b/stretch_core/launch/d435i_basic.launch
@@ -8,62 +8,49 @@
 
   <!-- REALSENSE D435i -->
   <include file="$(find realsense2_camera)/launch/rs_camera.launch">
-    <!-- "The D435i depth camera generates and transmits the gyro and
-         accelerometer samples independently, as the inertial sensors
-         exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for
-         accelerometer)."
-         https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/
-         https://github.com/intel-ros/realsense
-         -->
-    <arg name="accel_fps" value="63"/>
-    <arg name="gyro_fps" value="200"/>
-    <arg name="depth_fps" value="15"/>
-    <arg name="enable_infra1" value="false"/>
-    <arg name="enable_infra2" value="false"/>
-    <arg name="enable_accel" value="true"/>
-    <arg name="enable_color" value="true"/>
-    <arg name="enable_depth" value="true"/>
-
-    <arg name="depth_width" value="$(arg depth_width)"/>
-    <arg name="depth_height" value="$(arg depth_height)"/>
-    <arg name="color_width" value="$(arg color_width)"/>
-    <arg name="color_height" value="$(arg color_height)"/>
-
-    <arg name="color_fps" value="15"/>
+    <arg name="enable_confidence" value="false"/>
+    <arg name="enable_infra1"     value="false"/>
+    <arg name="enable_infra2"     value="false"/>
+    <arg name="enable_infra"      value="false"/>
+    <arg name="enable_color"      value="true"/>
+    <arg name="enable_depth"      value="true"/>
+    <arg name="enable_accel"      value="true"/>
+    <arg name="enable_gyro"       value="true"/>
+
+    <arg name="color_fps"         value="15"/>
+    <arg name="depth_fps"         value="15"/>
+    <arg name="accel_fps"         value="63"/>
+    <arg name="gyro_fps"          value="200"/>
+
+    <arg name="color_width"       value="$(arg color_width)"/>
+    <arg name="color_height"      value="$(arg color_height)"/>
+    <arg name="depth_width"       value="$(arg depth_width)"/>
+    <arg name="depth_height"      value="$(arg depth_height)"/>
 
     <!-- publish depth streams aligned to other streams -->
     <arg name="align_depth" value="true"/>
 
     <!-- publish an RGBD point cloud -->
     <arg name="filters" value="pointcloud"/>
-
-    <!-- "tf_prefix: By default all frame's ids have the same prefix -
-         camera_. This allows changing it per camera."
-         https://github.com/intel-ros/realsense -->
+    <arg name="enable_pointcloud" value="true"/>
 
-    <!-- "enable_sync: gathers closest frames of different sensors,
+    <!-- enable_sync: gathers closest frames of different sensors,
          infra red, color and depth, to be sent with the same
          timetag. This happens automatically when such filters as
-         pointcloud are enabled."
-         https://github.com/intel-ros/realsense -->
+         pointcloud are enabled. -->
     <arg name="enable_sync" value="true"/>
-
-    <!-- "You can have a full depth to pointcloud, coloring the
-         regions beyond the texture with zeros..." -->
+
     <!-- Set to true in order to make use of the full field of view of
          the depth image instead of being restricted to the field of
          view associated with the narrower RGB camera. Note that
          points out of the RGB camera's field of view will have their
          colors set to 0,0,0. -->
     <arg name="allow_no_texture_points" value="true"/>
 
-    <!-- "initial_reset: On occasions the device was not closed
+    <!-- initial_reset: On occasions the device was not closed
          properly and due to firmware issues needs to reset. If set to
-         true, the device will reset prior to usage."
-         https://github.com/intel-ros/realsense -->
+         true, the device will reset prior to usage. -->
     <arg name="initial_reset" value="$(arg rs_initial_reset)"/>
-
-    <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
 
     <!-- configure the depth image to the high accuracy visual preset -->
     <arg name="json_file_path" value="$(arg rs_initial_preset)" />

diff --git a/stretch_core/launch/d435i_high_resolution.launch b/stretch_core/launch/d435i_high_resolution.launch
@@ -1,16 +1,14 @@
 <launch>
-  
-  <!-- REALSENSE D435i -->
+
+  <!-- D435i Base -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	HIGHEST RESOLUTION, but also has the highest minimum depth
-	(280mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->
-    <arg name="depth_width" value="1280"/>
+    <!-- HIGHEST RESOLUTION, but also has the highest minimum depth
+         (280mm Min-Z) below which objects generate bad noise, such as
+         when the arm and gripper are raised close to the camera. -->
+    <arg name="depth_width"  value="1280"/>
     <arg name="depth_height" value="720"/>
-    <arg name="color_width" value="1280"/>
+    <arg name="color_width"  value="1280"/>
     <arg name="color_height" value="720"/>
   </include>
-  
+
 </launch>
diff --git a/stretch_core/launch/d435i_low_resolution.launch b/stretch_core/launch/d435i_low_resolution.launch
@@ -1,17 +1,14 @@
 <launch>
 
-  <!-- REALSENSE D435i -->
+  <!-- D435i Base -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	LOWEST RESOLUTION, but also has the lowest minimum depth
-	(105mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->    
-    <arg name="depth_width" value="424"/>
+    <!-- LOWEST RESOLUTION, but also has the lowest minimum depth
+         (105mm Min-Z) below which objects generate bad noise, such as
+         when the arm and gripper are raised close to the camera. -->
+    <arg name="depth_width"  value="424"/>
     <arg name="depth_height" value="240"/>
-    <arg name="color_width" value="424"/>
+    <arg name="color_width"  value="424"/>
     <arg name="color_height" value="240"/>
-
   </include>
 
 </launch>
diff --git a/stretch_core/launch/stretch.launch b/stretch_core/launch/stretch.launch
@@ -0,0 +1,26 @@
+<launch>
+
+  <arg name="lidar_odom" default="true"  doc="whether the odom TF is estimated with lidar odometry fused, or just wheel odometry" />
+  <arg name="respeaker"  default="true"  doc="whether to launch the respeaker microphone array/speaker drivers" />
+  <arg name="rviz"       default="false" doc="whether to show Rviz" />
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true" unless="$(arg lidar_odom)" />
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true" />
+
+  <!-- D435i CAMERA -->
+  <include file="$(find stretch_core)/launch/stretch_realsense.launch" pass_all_args="true" />
+
+  <!-- LASER RANGE FINDER + SCAN MATCHER ODOMETRY -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" if="$(arg lidar_odom)" />
+  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" if="$(arg lidar_odom)" />
+
+  <!-- RESPEAKER MICROPHONE ARRAY -->
+  <group ns="stretch">
+    <include file="$(find respeaker_ros)/launch/respeaker.launch" if="$(arg respeaker)" />
+  </group>
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_core)/rviz/stretch.rviz" if="$(arg rviz)" />
+
+</launch>
diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch
@@ -41,7 +41,9 @@
     <param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
   </node>
 
+  <!--
   <node name="aggregator_node" pkg="diagnostic_aggregator" type="aggregator_node">
     <rosparam command="load" file="$(find stretch_core)/config/diagnostics.yaml" />
   </node>
+  -->
 </launch>
diff --git a/stretch_core/launch/stretch_realsense.launch b/stretch_core/launch/stretch_realsense.launch
@@ -0,0 +1,24 @@
+<launch>
+
+  <arg name="resolution"          default="high"  doc="resolution of the color/depth images ('low' or 'high')" />
+  <arg name="publish_upright_img" default="false" doc="whether to publish a rotated upright color image" />
+  <arg name="publish_frustum_viz" default="false" doc="whether to publish frustum field of view visualizers" />
+
+  <!-- D435i DRIVER -->
+  <include file="$(find stretch_core)/launch/d435i_$(arg resolution)_resolution.launch" />
+
+  <!-- VISUAL PRESET CONFIGURATION -->
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" />
+
+  <!-- UPRIGHT ROTATED CAMERA VIEW -->
+  <node name="upright_rotater" pkg="image_rotate" type="image_rotate" if="$(arg publish_upright_img)">
+    <remap from="image" to="/camera/color/image_raw" />
+    <remap from="rotated/image" to="/camera/color/upright_image_raw" />
+    <param name="target_frame_id" type="str" value="" />
+    <param name="target_x" type="double" value="-1.5708" />
+  </node>
+
+  <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
+  <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" if="$(arg publish_frustum_viz)" />
+
+</launch>
diff --git a/stretch_core/nodes/d435i_accel_correction b/stretch_core/nodes/d435i_accel_correction
@@ -8,11 +8,11 @@ class D435iAccelCorrectionNode:
     def __init__(self):
         self.num_samples_to_skip = 4
         self.sample_count = 0
-        
+
     def accel_callback(self, accel):
         self.accel = accel
         self.sample_count += 1
-        if (self.sample_count % self.num_samples_to_skip) == 0: 
+        if (self.sample_count % self.num_samples_to_skip) == 0:
             # This can avoid issues with the D435i's time stamps being too
             # far ahead for TF.
             self.accel.header.stamp = rospy.Time.now()
@@ -26,15 +26,15 @@ class D435iAccelCorrectionNode:
 
             self.corrected_accel_pub.publish(self.accel)
 
-            
+
     def main(self):
         rospy.init_node('D435iAccelCorrectionNode')
         self.node_name = rospy.get_name()
         rospy.loginfo("{0} started".format(self.node_name))
 
         self.topic_name = '/camera/accel/sample'
         self.accel_subscriber = rospy.Subscriber(self.topic_name, Imu, self.accel_callback)
-        
+
         self.corrected_accel_pub = rospy.Publisher('/camera/accel/sample_corrected', Imu, queue_size=1)
 
 

diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure
@@ -10,21 +10,21 @@ class D435iConfigureNode:
         self.rate = 1.0
         self.visual_preset = None
         self.mode_lock = threading.Lock()
-        
+
     def turn_on_default_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 1
             self.locked_mode_name = 'Default'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def turn_on_high_accuracy_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 3
             self.locked_mode_name = 'High Accuracy'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def default_mode_service_callback(self, request):
         self.turn_on_default_mode()
         return TriggerResponse(
@@ -58,9 +58,9 @@ class D435iConfigureNode:
         while not rospy.is_shutdown():
             rate.sleep()
 
-            
+
 if __name__ == '__main__':
-    try: 
+    try:
         node = D435iConfigureNode()
         node.main()
     except KeyboardInterrupt:

diff --git a/stretch_core/nodes/d435i_frustum_visualizer b/stretch_core/nodes/d435i_frustum_visualizer
@@ -10,16 +10,16 @@ from visualization_msgs.msg import MarkerArray
 from geometry_msgs.msg import Transform, Pose, Vector3, Quaternion, Point
 from sensor_msgs.msg import CameraInfo
 
-    
+
 class FrustumNode:
     def __init__(self):
         self.color_count = 0
         self.depth_count = 0
         self.camera_types = ['depth', 'color']
         # only publish a single frustum for every N camera info
         # messages received
-        self.skip_publishing = 5 
-        
+        self.skip_publishing = 5
+
         # minimum-Z depth for D435i
         # 1280x720 0.28  m
         # 848x480  0.195 m
@@ -43,12 +43,12 @@ class FrustumNode:
         if (self.depth_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='depth')
         self.depth_count = self.depth_count + 1
-        
+
     def color_camera_info_callback(self, camera_info):
         if (self.color_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='color')
         self.color_count = self.color_count + 1
-        
+
     def camera_info_callback(self, camera_info, camera_type=None):
         if camera_type not in self.camera_types:
             print('WARNING: FrustumNode camera_info_callback camera_type = {0} unrecognized. Valid types = {1}.'.format(camera_type, self.camera_types))
@@ -101,28 +101,28 @@ class FrustumNode:
         marker.type = marker.TRIANGLE_LIST
         marker.action = marker.ADD
         marker.lifetime = rospy.Duration(0.5) #0.2
-        marker.text = 'D435i_frustum'
         marker.header.frame_id = frame_id
         marker.header.stamp = timestamp
         marker.id = 0
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
-        if camera_type == 'depth': 
+        marker.pose.orientation.w = 1.0
+        if camera_type == 'depth':
             # frustum color and transparency
             # gray
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 1.0
             marker.color.a = 0.4
-        elif camera_type == 'color': 
+        elif camera_type == 'color':
             # frustum color and transparency
             # yellow
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             marker.color.a = 0.4
-            
+
         def adjacent_corners(c0, c1):
             # return True if the corners are adjacent to one another and False otherwise
             faces0 = c0[1]
@@ -170,23 +170,23 @@ class FrustumNode:
 
         points = []
         quad_ids = ['near', 'far', 'top', 'bottom', 'left', 'right']
-        for s in quad_ids: 
+        for s in quad_ids:
             quad_xyz = corners_to_quad(frustum_corners_xyz, s)
             triangles = quad_to_triangles(quad_xyz)
             points.extend(triangles)
         marker.points = points
-        
+
         if camera_type == 'depth':
             self.depth_frustum_marker_pub.publish(marker)
         elif camera_type == 'color':
             self.color_frustum_marker_pub.publish(marker)
 
-        
+
     def main(self):
         rospy.init_node('FrustumNode')
         self.node_name = rospy.get_name()
         rospy.loginfo("{0} started".format(self.node_name))
-        
+
         self.color_camera_topic = '/camera/color/camera_info'
         self.depth_camera_topic = '/camera/depth/camera_info'
         self.depth_camera_info_subscriber = rospy.Subscriber(self.depth_camera_topic, CameraInfo, self.depth_camera_info_callback)

diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
@@ -543,7 +543,7 @@ class StretchDriverNode:
         self.last_twist_time = rospy.get_time()
 
         # start action server for joint trajectories
-        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True)
+        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', False)
         self.joint_trajectory_action = JointTrajectoryAction(self)
         self.joint_trajectory_action.server.start()
         self.diagnostics = StretchDiagnostics(self, self.robot)