Skip to content

Commit

Permalink
[jsk_fetch_startup] Make realsense nodes respawn (#82)
Browse files Browse the repository at this point in the history
* [jsk_fetch_startup] copy nodelet.launch.xml from realsense2_camera

* [jsk_fetch_startup] make realsense nodelet be launched as a standalone node

* [jsk_fetch_startup] change required arg of realsense_nodelet.launch.xml to respawn arg

* [jsk_fetch_startup] update fetch_reansense_bringup.launch so that it uses realsense_nodelet.launch.xml in jsk_fetch_startup instead of nodelet.launch.xml in realsense2_camera

* [jsk_fetch_startup] fix arguments for realsense bringup launch
  • Loading branch information
sktometometo authored and knorth55 committed Sep 19, 2021
1 parent 1c1bf3e commit a3b4082
Show file tree
Hide file tree
Showing 3 changed files with 230 additions and 16 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,206 @@
<launch>
<!--
This launch file is copied from realsense2_camera in realsense_ros https://github.com/IntelRealSense/realsense-ros and then modified
And it is originally distributed under Apache-2.0 License
-->
<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default=""/>
<arg name="tf_prefix" default=""/>
<arg name="json_file_path" default=""/>
<arg name="rosbag_filename" default=""/>
<arg name="respawn" default="false"/>
<arg name="output" default="screen"/> <!-- [ screen | log ]-->

<arg name="fisheye_width" default="0"/>
<arg name="fisheye_height" default="0"/>
<arg name="enable_fisheye" default="false"/>
<arg name="enable_fisheye1" default="false"/>
<arg name="enable_fisheye2" default="false"/>

<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="enable_depth" default="true"/>

<arg name="confidence_width" default="640"/>
<arg name="confidence_height" default="480"/>
<arg name="enable_confidence" default="true"/>
<arg name="confidence_fps" default="30"/>

<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra" default="false"/>
<arg name="enable_infra1" default="false"/>
<arg name="enable_infra2" default="false"/>
<arg name="infra_rgb" default="false"/>

<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="enable_color" default="true"/>

<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="0"/>
<arg name="accel_fps" default="0"/>
<arg name="enable_gyro" default="false"/>
<arg name="enable_accel" default="false"/>
<arg name="enable_pose" default="false"/>

<arg name="enable_pointcloud" default="false"/>
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/> <!-- use RS2_STREAM_ANY to avoid using texture -->
<arg name="pointcloud_texture_index" default="0"/>
<arg name="allow_no_texture_points" default="false"/>
<arg name="ordered_pc" default="false"/>

<arg name="enable_sync" default="false"/>
<arg name="align_depth" default="false"/>

<arg name="base_frame_id" default="$(arg tf_prefix)_link"/>
<arg name="depth_frame_id" default="$(arg tf_prefix)_depth_frame"/>
<arg name="infra1_frame_id" default="$(arg tf_prefix)_infra1_frame"/>
<arg name="infra2_frame_id" default="$(arg tf_prefix)_infra2_frame"/>
<arg name="color_frame_id" default="$(arg tf_prefix)_color_frame"/>
<arg name="fisheye_frame_id" default="$(arg tf_prefix)_fisheye_frame"/>
<arg name="fisheye1_frame_id" default="$(arg tf_prefix)_fisheye1_frame"/>
<arg name="fisheye2_frame_id" default="$(arg tf_prefix)_fisheye2_frame"/>
<arg name="accel_frame_id" default="$(arg tf_prefix)_accel_frame"/>
<arg name="gyro_frame_id" default="$(arg tf_prefix)_gyro_frame"/>
<arg name="pose_frame_id" default="$(arg tf_prefix)_pose_frame"/>

<arg name="depth_optical_frame_id" default="$(arg tf_prefix)_depth_optical_frame"/>
<arg name="infra1_optical_frame_id" default="$(arg tf_prefix)_infra1_optical_frame"/>
<arg name="infra2_optical_frame_id" default="$(arg tf_prefix)_infra2_optical_frame"/>
<arg name="color_optical_frame_id" default="$(arg tf_prefix)_color_optical_frame"/>
<arg name="fisheye_optical_frame_id" default="$(arg tf_prefix)_fisheye_optical_frame"/>
<arg name="fisheye1_optical_frame_id" default="$(arg tf_prefix)_fisheye1_optical_frame"/>
<arg name="fisheye2_optical_frame_id" default="$(arg tf_prefix)_fisheye2_optical_frame"/>
<arg name="accel_optical_frame_id" default="$(arg tf_prefix)_accel_optical_frame"/>
<arg name="gyro_optical_frame_id" default="$(arg tf_prefix)_gyro_optical_frame"/>
<arg name="imu_optical_frame_id" default="$(arg tf_prefix)_imu_optical_frame"/>
<arg name="pose_optical_frame_id" default="$(arg tf_prefix)_pose_optical_frame"/>

<arg name="aligned_depth_to_color_frame_id" default="$(arg tf_prefix)_aligned_depth_to_color_frame"/>
<arg name="aligned_depth_to_infra1_frame_id" default="$(arg tf_prefix)_aligned_depth_to_infra1_frame"/>
<arg name="aligned_depth_to_infra2_frame_id" default="$(arg tf_prefix)_aligned_depth_to_infra2_frame"/>
<arg name="aligned_depth_to_fisheye_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye_frame"/>
<arg name="aligned_depth_to_fisheye1_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye1_frame"/>
<arg name="aligned_depth_to_fisheye2_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye2_frame"/>

<arg name="publish_tf" default="true"/>
<arg name="tf_publish_rate" default="0"/> <!-- 0 - static transform -->

<arg name="odom_frame_id" default="$(arg tf_prefix)_odom_frame"/>
<arg name="topic_odom_in" default="$(arg tf_prefix)/odom_in"/>
<arg name="calib_odom_file" default=""/>
<arg name="publish_odom_tf" default="true"/>
<arg name="filters" default=""/>
<arg name="clip_distance" default="-1"/>
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="unite_imu_method" default="none"/> <!-- Options are: [none, copy, linear_interpolation] -->

<node
pkg="nodelet"
type="nodelet"
name="$(arg tf_prefix)_realsense2_camera"
args="standalone realsense2_camera/RealSenseNodeFactory"
respawn="$(arg respawn)">
<param name="serial_no" type="str" value="$(arg serial_no)"/>
<param name="usb_port_id" type="str" value="$(arg usb_port_id)"/>
<param name="device_type" type="str" value="$(arg device_type)"/>
<param name="json_file_path" type="str" value="$(arg json_file_path)"/>
<param name="rosbag_filename" type="str" value="$(arg rosbag_filename)"/>

<param name="enable_pointcloud" type="bool" value="$(arg enable_pointcloud)"/>
<param name="pointcloud_texture_stream" type="str" value="$(arg pointcloud_texture_stream)"/>
<param name="pointcloud_texture_index" type="int" value="$(arg pointcloud_texture_index)"/>
<param name="allow_no_texture_points" type="bool" value="$(arg allow_no_texture_points)"/>
<param name="ordered_pc" type="bool" value="$(arg ordered_pc)"/>

<param name="enable_sync" type="bool" value="$(arg enable_sync)"/>
<param name="align_depth" type="bool" value="$(arg align_depth)"/>

<param name="fisheye_width" type="int" value="$(arg fisheye_width)"/>
<param name="fisheye_height" type="int" value="$(arg fisheye_height)"/>
<param name="enable_fisheye" type="bool" value="$(arg enable_fisheye)"/>
<param name="enable_fisheye1" type="bool" value="$(arg enable_fisheye1)"/>
<param name="enable_fisheye2" type="bool" value="$(arg enable_fisheye2)"/>

<param name="depth_width" type="int" value="$(arg depth_width)"/>
<param name="depth_height" type="int" value="$(arg depth_height)"/>
<param name="enable_depth" type="bool" value="$(arg enable_depth)"/>

<param name="confidence_width" type="int" value="$(arg confidence_width)"/>
<param name="confidence_height" type="int" value="$(arg confidence_height)"/>
<param name="enable_confidence" type="bool" value="$(arg enable_confidence)"/>
<param name="confidence_fps" type="int" value="$(arg confidence_fps)"/>

<param name="color_width" type="int" value="$(arg color_width)"/>
<param name="color_height" type="int" value="$(arg color_height)"/>
<param name="enable_color" type="bool" value="$(arg enable_color)"/>

<param name="infra_width" type="int" value="$(arg infra_width)"/>
<param name="infra_height" type="int" value="$(arg infra_height)"/>
<param name="enable_infra" type="bool" value="$(arg enable_infra)"/>
<param name="enable_infra1" type="bool" value="$(arg enable_infra1)"/>
<param name="enable_infra2" type="bool" value="$(arg enable_infra2)"/>
<param name="infra_rgb" type="bool" value="$(arg infra_rgb)"/>

<param name="fisheye_fps" type="int" value="$(arg fisheye_fps)"/>
<param name="depth_fps" type="int" value="$(arg depth_fps)"/>
<param name="infra_fps" type="int" value="$(arg infra_fps)"/>
<param name="color_fps" type="int" value="$(arg color_fps)"/>
<param name="gyro_fps" type="int" value="$(arg gyro_fps)"/>
<param name="accel_fps" type="int" value="$(arg accel_fps)"/>
<param name="enable_gyro" type="bool" value="$(arg enable_gyro)"/>
<param name="enable_accel" type="bool" value="$(arg enable_accel)"/>
<param name="enable_pose" type="bool" value="$(arg enable_pose)"/>

<param name="base_frame_id" type="str" value="$(arg base_frame_id)"/>
<param name="depth_frame_id" type="str" value="$(arg depth_frame_id)"/>
<param name="infra1_frame_id" type="str" value="$(arg infra1_frame_id)"/>
<param name="infra2_frame_id" type="str" value="$(arg infra2_frame_id)"/>
<param name="color_frame_id" type="str" value="$(arg color_frame_id)"/>
<param name="fisheye_frame_id" type="str" value="$(arg fisheye_frame_id)"/>
<param name="fisheye1_frame_id" type="str" value="$(arg fisheye1_frame_id)"/>
<param name="fisheye2_frame_id" type="str" value="$(arg fisheye2_frame_id)"/>
<param name="accel_frame_id" type="str" value="$(arg accel_frame_id)"/>
<param name="gyro_frame_id" type="str" value="$(arg gyro_frame_id)"/>
<param name="pose_frame_id" type="str" value="$(arg pose_frame_id)"/>

<param name="depth_optical_frame_id" type="str" value="$(arg depth_optical_frame_id)"/>
<param name="infra1_optical_frame_id" type="str" value="$(arg infra1_optical_frame_id)"/>
<param name="infra2_optical_frame_id" type="str" value="$(arg infra2_optical_frame_id)"/>
<param name="color_optical_frame_id" type="str" value="$(arg color_optical_frame_id)"/>
<param name="fisheye_optical_frame_id" type="str" value="$(arg fisheye_optical_frame_id)"/>
<param name="fisheye1_optical_frame_id" type="str" value="$(arg fisheye1_optical_frame_id)"/>
<param name="fisheye2_optical_frame_id" type="str" value="$(arg fisheye2_optical_frame_id)"/>
<param name="accel_optical_frame_id" type="str" value="$(arg accel_optical_frame_id)"/>
<param name="gyro_optical_frame_id" type="str" value="$(arg gyro_optical_frame_id)"/>
<param name="imu_optical_frame_id" type="str" value="$(arg imu_optical_frame_id)"/>
<param name="pose_optical_frame_id" type="str" value="$(arg pose_optical_frame_id)"/>

<param name="aligned_depth_to_color_frame_id" type="str" value="$(arg aligned_depth_to_color_frame_id)"/>
<param name="aligned_depth_to_infra1_frame_id" type="str" value="$(arg aligned_depth_to_infra1_frame_id)"/>
<param name="aligned_depth_to_infra2_frame_id" type="str" value="$(arg aligned_depth_to_infra2_frame_id)"/>
<param name="aligned_depth_to_fisheye_frame_id" type="str" value="$(arg aligned_depth_to_fisheye_frame_id)"/>
<param name="aligned_depth_to_fisheye1_frame_id" type="str" value="$(arg aligned_depth_to_fisheye1_frame_id)"/>
<param name="aligned_depth_to_fisheye2_frame_id" type="str" value="$(arg aligned_depth_to_fisheye2_frame_id)"/>

<param name="publish_tf" type="bool" value="$(arg publish_tf)"/>
<param name="tf_publish_rate" type="double" value="$(arg tf_publish_rate)"/>

<param name="odom_frame_id" type="str" value="$(arg odom_frame_id)"/>
<param name="topic_odom_in" type="str" value="$(arg topic_odom_in)"/>
<param name="calib_odom_file" type="str" value="$(arg calib_odom_file)"/>
<param name="publish_odom_tf" type="bool" value="$(arg publish_odom_tf)"/>
<param name="filters" type="str" value="$(arg filters)"/>
<param name="clip_distance" type="double" value="$(arg clip_distance)"/>
<param name="linear_accel_cov" type="double" value="$(arg linear_accel_cov)"/>
<param name="initial_reset" type="bool" value="$(arg initial_reset)"/>
<param name="unite_imu_method" type="str" value="$(arg unite_imu_method)"/>

</node>
</launch>
2 changes: 1 addition & 1 deletion jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@

<!-- Launch realsense cameras -->
<include file="$(find jsk_fetch_startup)/launch/fetch_realsense_bringup.launch" if="$(arg use_realsense)">
<arg name="required" value="false" />
<arg name="respawn" value="true" />
</include>

<!-- Odometry -->
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<launch>

<arg name="required" default="false" />
<arg name="respawn" default="true" />

<node
pkg="jsk_fetch_startup"
Expand All @@ -17,8 +17,7 @@
/tracking_module/enable_dynamic_calibration: true
/tracking_module/enable_map_preservation: false
</rosparam>
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="external_manager" value="false"/>
<include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
<arg name="tf_prefix" value="t265"/>
<arg name="serial_no" value=""/>
<arg name="usb_port_id" value=""/>
Expand Down Expand Up @@ -46,21 +45,30 @@
<arg name="publish_odom_tf" value="false"/>
<arg name="publish_tf" value="false"/>

<arg name="required" value="$(arg required)"/>
<arg name="respawn" value="$(arg respawn)"/>
</include>
</group>

<!-- d435 -->
<node
pkg="tf"
type="static_transform_publisher"
name="base_link_to_d435_front_virtual_joint"
args="0.285 0 0.3752 0 0.5236 0 base_link d435_front_link 100"
output="screen"
/>
<group ns="d435_front">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="external_manager" value="false"/>
<group ns="d435_front_right">
<include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
<arg name="device_type" value="d435"/>
<arg name="serial_no" value=""/>
<arg name="tf_prefix" value="d435_front_right"/>
<arg name="initial_reset" value="true"/>
<arg name="align_depth" value="true"/>
<arg name="filters" value="pointcloud"/>
<arg name="color_width" value="640"/>
<arg name="color_height" value="480"/>
<arg name="depth_width" value="640"/>
<arg name="depth_height" value="480"/>
<arg name="clip_distance" value="-2"/>
<arg name="enable_pointcloud" value="true"/>
<arg name="respawn" value="$(arg respawn)"/>
</include>
</group>
<group ns="d435_front_left">
<include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
<arg name="device_type" value="d435"/>
<arg name="serial_no" value=""/>
<arg name="tf_prefix" value="d435_front"/>
Expand All @@ -73,7 +81,7 @@
<arg name="depth_height" value="480"/>
<arg name="clip_distance" value="-2"/>
<arg name="enable_pointcloud" value="true"/>
<arg name="required" value="$(arg required)"/>
<arg name="respawn" value="$(arg respawn)"/>
</include>
</group>

Expand Down

0 comments on commit a3b4082

Please sign in to comment.