Skip to content

Commit

Permalink
Merge pull request #1371 from sktometometo/PR/fetch/enable-t265-odometry
Browse files Browse the repository at this point in the history 
[merge after #1383] add realsense support and enable T265 odometry
  • Loading branch information
@k-okada
k-okada authored Sep 22, 2021
2 parents 952a205 + d3b9390 commit d8d655c
Show file tree
Hide file tree
Showing 10 changed files with 571 additions and 130 deletions.
9 changes: 9 additions & 0 deletions jsk_fetch_robot/jsk_fetch_startup/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,15 @@

## SetUp (Running following commands in the first time)

### Install a udev rule for realsense

udev rule have to be manually installed according to [this issue](https://github.com/IntelRealSense/realsense-ros/issues/1426) when using realsense-ros from ROS repository.

```bash
wget https://github.com/IntelRealSense/librealsense/raw/master/config/99-realsense-libusb.rules
sudo mv 99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && sudo udevadm trigger
```

### upstart
```
Expand Down
206 changes: 206 additions & 0 deletions jsk_fetch_robot/jsk_fetch_startup/launch/devices/realsense_nodelet.launch.xml
View file
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>
60 changes: 52 additions & 8 deletions jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<arg name="launch_teleop" default="true" />
<arg name="base_camera_mount" default="true" />
<arg name="head_box" default="true" />
<arg name="use_visual_odom" default="false" />
<arg name="use_realsense" default="true" />
<arg name="use_fetch_description" default="false" />

<!-- GDB Debug Option -->
Expand All @@ -21,15 +21,59 @@
<arg name="rgb_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>
<arg name="depth_camera_info_url" default="file:///etc/ros/$(env ROS_DISTRO)/depth_latest.yaml"/>


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

<!-- Odometry -->
<param name="base_controller/publish_tf" value="false"/>
<!-- stop using graft: s-kitagawa (2019/10/17) -->
<!-- <include file="$(find fetch_bringup)/launch/include/graft.launch.xml"/> -->

<!-- Odometry without visual odom -->
<!-- use robot localization ukf -->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true" unless="$(arg use_visual_odom)">
<remap from="odometry/filtered" to="/odom_combined" />
<!-- odometry topic mux -->
<node
pkg="topic_tools"
type="mux"
name="odom_topic_mux"
args="odom_combined odom_robot_localization odom_visual"
>
<remap from="/mux" to="/odom_topic_mux" />
<param name="~initial_topic" value="odom_visual" />
</node>
<!-- odometry tf mux -->
<node
pkg="topic_tools"
type="mux"
name="odom_tf_mux"
args="tf tf_odom_robot_localization tf_odom_visual"
>
<remap from="/mux" to="/odom_tf_mux" />
<param name="~initial_topic" value="tf_odom_visual" />
</node>
<!-- odometry mux selector -->
<node
pkg="jsk_fetch_startup"
type="odometry_mux_selector.py"
name="odometry_mux_selector"
>
<param name="~topic_odom_primary" value="odom_visual" />
<param name="~topic_odom_secondary" value="odom_robot_localization" />
<param name="~topic_tf_primary" value="tf_odom_visual" />
<param name="~topic_tf_secondary" value="tf_odom_robot_localization" />
<param name="~duration_timeout_topic" value="10.0" />

<remap from="~select_service_topic" to="/odom_topic_mux/select" />
<remap from="~select_service_tf" to="/odom_tf_mux/select" />

<remap from="~sound_play" to="sound_play" />
</node>

<!-- Odometry with robot localization ukf using wheel and imu -->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_se" clear_params="true">
<remap from="odometry/filtered" to="/odom_robot_localization" />
<remap from="/tf" to="/tf_odom_robot_localization"/>
<rosparam>
frequency: 50
sensor_timeout: 1.0
Expand All @@ -50,11 +94,11 @@
</rosparam>
</node>

<!-- Use Visual Odom -->
<include file="$(find jsk_fetch_startup)/launch/fetch_t265.launch" if="$(arg use_visual_odom)">
<!-- Visual Odom -->
<include file="$(find jsk_fetch_startup)/launch/fetch_visual_odom.launch">
<arg name="topic_odom_out" value="/odom_visual" />
<arg name="odom_frame_id" value="odom" />
<arg name="base_link_frame_id" value="base_link" />
<arg name="topic_tf" value="tf_odom_visual" />
</include>

<!-- /imu has no frame_id information and there is no bug fix release in indigo. -->
Expand Down
Loading

0 comments on commit d8d655c

Please sign in to comment.