i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated rtabmap_examples launch files with new structure

Browse Source
This commit is contained in:
matlabbe
2023-03-04 15:40:17 -08:00
parent a7675b7c09
commit f5f6097a20
41 changed files with 245 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_examples/launch/tests/bumblebee.launch → rtabmap_examples/launch/bumblebee.launch
+2 -2
View File
@@ -19,7 +19,7 @@
<!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_legacy/stereo_throttle">
<remap from="left/image" to="left/image_raw"/>
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
@@ -36,6 +36,6 @@
<remap from="right/camera_info" to="right/camera_info_throttle"/>
</node>
<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_util/disparity_to_depth"/>
</group>
</launch>
rtabmap_examples/launch/config/distortion_model_PS1080.bin
BIN
View File
Binary file not shown.
rtabmap_examples/launch/config/distortion_model_PS1080.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 551 KiB

rtabmap_examples/launch/config/euroc_left.yaml
+30
View File
@@ -0,0 +1,30 @@
#%YAML:1.0
---
camera_name: MH_01_easy_left
image_width: 752
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 4.5865400000000000e+02, 0., 3.6721499999999997e+02, 0.,
4.5729599999999999e+02, 2.4837500000000000e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 4
data: [ -2.8340810999999999e-01, 7.3959070000000002e-02,
1.9358999999999999e-04, 1.7618711400000001e-05 ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 9.9996634750298619e-01, -1.4227432298321767e-03,
8.0795831104762770e-03, 1.3657459036274155e-03,
9.9997417608074302e-01, 7.0556296505566432e-03,
-8.0894128132919258e-03, -7.0443575534647465e-03,
9.9994246755850658e-01 ]
projection_matrix:
rows: 3
cols: 4
data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02, 0., 0.,
4.3520469597145990e+02, 2.5220085144042969e+02, 0., 0., 0., 1.,
0. ]
rtabmap_examples/launch/config/euroc_right.yaml
+30
View File
@@ -0,0 +1,30 @@
#%YAML:1.0
---
camera_name: MH_01_easy_right
image_width: 752
image_height: 480
camera_matrix:
rows: 3
cols: 3
data: [ 4.5758699999999999e+02, 0., 3.7999900000000002e+02, 0.,
4.5613400000000001e+02, 2.5523800000000000e+02, 0., 0., 1. ]
distortion_coefficients:
rows: 1
cols: 4
data: [ -2.8368365000000001e-01, 7.4512839999999997e-02,
-1.0473000000000000e-04, -3.5559070000000001e-05 ]
distortion_model: plumb_bob
rectification_matrix:
rows: 3
cols: 3
data: [ 9.9996335257946345e-01, -3.6258159819210472e-03,
7.7554468926514068e-03, 3.6804026836554193e-03,
9.9996847525904631e-01, -7.0358456623254633e-03,
-7.7296917225483383e-03, 7.0641309842873097e-03,
9.9994517345668066e-01 ]
projection_matrix:
rows: 3
cols: 4
data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02,
-4.7906395664848930e+01, 0., 4.3520469597145990e+02,
2.5220085144042969e+02, 0., 0., 0., 1., 0. ]
rtabmap_examples/launch/config/rgbdslam_datasets.rviz
+2 -2
View File
@@ -143,7 +143,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap_ros/MapCloud
Class: rtabmap_rviz_plugins/MapCloud
Cloud decimation: 4
Cloud max depth (m): 4
Cloud voxel size (m): 0.01
@@ -169,7 +169,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rtabmap_ros/Info
- Class: rtabmap_rviz_plugins/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
rtabmap_examples/launch/tests/tag_bundle1.pdf → rtabmap_examples/launch/config/tag_bundle1.pdf
View File
rtabmap_examples/launch/tests/tag_bundle1.png → rtabmap_examples/launch/config/tag_bundle1.png
View File
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 323 B

After

Width:  |  Height:  |  Size: 323 B

rtabmap_examples/launch/tests/tag_settings.yaml → rtabmap_examples/launch/config/tag_settings.yaml
View File
rtabmap_examples/launch/tests/tags.yaml → rtabmap_examples/launch/config/tags.yaml
View File
rtabmap_examples/launch/tests/euroc_datasets.launch → rtabmap_examples/launch/euroc_datasets.launch
+16 -16
View File
@@ -4,31 +4,31 @@
<!--
Examples:
F2M (default VO):
$ roslaunch rtabmap_ros euroc_datasets.launch
$ roslaunch rtabmap_examples euroc_datasets.launch
$ rosbag play -.-clock V1_01_easy.bag
For MH sequences, we should set MH_seq to true because the ground truth source is different.
$ roslaunch rtabmap_ros euroc_datasets.launch MH_seq:=true
$ roslaunch rtabmap_examples euroc_datasets.launch MH_seq:=true
$ rosbag play -.-clock MH_01_easy.bag
MSCKF (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8"
$ roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 8"
$ rosbag play -.-clock V1_01_easy.bag
We need to ignore the first 24 seconds for correct VIO initialization (drone should not move).
$ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=true
$ roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=true
$ rosbag play -.-clock -s 24 MH_01_easy.bag
OKVIS (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
$ roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock MH_01_easy.bag
VINS (VIO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock MH_01_easy.bag
VINS (VO):
$ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
$ rosbag play -.-clock MH_01_easy.bag
-->
@@ -45,19 +45,19 @@ Examples:
<arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic -->
<arg name="raw_images_for_odom" default="false"/>
<arg name="record_ground_truth" default="false"/>
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="rviz" default="false"/>
<!-- Image rectification and publishing synchronized camera_info-->
<group ns="stereo_camera">
<node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_left">
<param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_left.yaml"/>
<node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info_left">
<param name="yaml_path" value="$(find rtabmap_examples)/launch/config/euroc_left.yaml"/>
<remap from="image" to="/cam0/image_raw"/>
<remap from="camera_info" to="left/camera_info"/>
</node>
<node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_right">
<param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_right.yaml"/>
<node pkg="rtabmap_util" type="yaml_to_camera_info.py" name="yaml_to_camera_info_right">
<param name="yaml_path" value="$(find rtabmap_examples)/launch/config/euroc_right.yaml"/>
<param name="frame_id" value="cam1"/>
<remap from="image" to="/cam1/image_raw"/>
<remap from="camera_info" to="right/camera_info"/>
@@ -79,12 +79,12 @@ Examples:
<node if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
<node if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
<node if="$(arg MH_seq)" pkg="rtabmap_util" type="point_to_tf.py" name="point_to_tf">
<remap from="point" to="/leica/position"/>
<param name="frame_id" value="leica"/>
<param name="fixed_frame_id" value="world"/>
</node>
<node unless="$(arg MH_seq)" pkg="rtabmap_ros" type="transform_to_tf.py" name="transform_to_tf">
<node unless="$(arg MH_seq)" pkg="rtabmap_util" type="transform_to_tf.py" name="transform_to_tf">
<remap from="transform" to="/vicon/firefly_sbx/firefly_sbx"/>
<param name="frame_id" value="world"/>
<param name="child_frame_id" value="vicon/firefly_sbx/firefly_sbx"/>
@@ -99,7 +99,7 @@ Examples:
</node>
<!-- RTAB-Map -->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args) --Rtabmap/ImagesAlreadyRectified false"/>
<arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args)"/>
<arg if="$(arg raw_images_for_odom)" name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
@@ -112,7 +112,7 @@ Examples:
<arg if="$(arg record_ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
<arg name="cfg" value="$(arg cfg)"/>
<arg name="imu_topic" value="/imu/data"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rtabmap_viz" value="$(arg rtabmap_viz)"/>
<arg name="rviz" value="$(arg rviz)"/>
<arg name="wait_imu_to_init" value="true"/>
</include>
rtabmap_examples/launch/tests/rgbdslam_datasets.launch → rtabmap_examples/launch/rgbdslam_datasets.launch
+7 -7
View File
@@ -8,7 +8,7 @@
$ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/6287ce3def8231945326efead0c8a7730bf6a3d5/tum_rename_world_kinect_frame.py
$ python tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
$ roslaunch rtabmap_ros rgbdslam_datasets.launch
$ roslaunch rtabmap_examples rgbdslam_datasets.launch
$ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household.bag
-->
@@ -16,7 +16,7 @@
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="rtabmap_viz" default="false" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
@@ -26,7 +26,7 @@
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -44,7 +44,7 @@
<!-- Visual SLAM -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
@@ -65,7 +65,7 @@
</node>
<!-- Visualisation -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_examples)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="queue_size" type="int" value="30"/>
@@ -79,8 +79,8 @@
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_examples)/launch/config/rgbdslam_datasets.rviz"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_util/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
rtabmap_examples/launch/tests/sensor_fusion.launch → rtabmap_examples/launch/sensor_fusion.launch
+3 -3
View File
@@ -20,7 +20,7 @@
<group ns="rtabmap">
<!-- Visual Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
<node pkg="rtabmap_odom" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -38,7 +38,7 @@
</node>
<!-- SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
@@ -143,4 +143,4 @@
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
</launch>
</launch>
rtabmap_examples/launch/tests/sensor_fusion_kinect_brick.launch → rtabmap_examples/launch/sensor_fusion_kinect_brick.launch
+2 -2
View File
@@ -30,10 +30,10 @@
<node pkg="tf" type="static_transform_publisher" name="optical_rotation"
args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />
<include file="$(find rtabmap_ros)/launch/tests/sensor_fusion.launch">
<include file="$(find rtabmap_examples)/launch/sensor_fusion.launch">
<arg name="frame_id" value="camera_rgb_frame"/>
<arg name="localization" value="$(arg localization)"/>
<arg name="imu_remove_gravitational_acceleration" value="false"/>
</include>
</launch>
</launch>
rtabmap_examples/launch/tests/test_apriltag_ros.launch → rtabmap_examples/launch/test_apriltag_ros.launch
+4 -4
View File
@@ -6,16 +6,16 @@
<!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and landmark_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
<!-- $ roslaunch realsense2_camera rs_camera.launch align_depth:=true -->
<!-- $ roslaunch rtabmap_ros test_apriltag_ros.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
<!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" landmark_angular_variance:=9999 -->
<!-- $ roslaunch rtabmap_examples test_apriltag_ros.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
<!-- $ roslaunch rtabmap_launch rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmap_viz:=false args:="-d -Optimizer/Strategy 1" landmark_angular_variance:=9999 -->
<arg name="camera_frame_id" default="camera_color_optical_frame"/>
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<!-- Set parameters -->
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltag_ros_continuous_node" />
<rosparam command="load" file="$(find rtabmap_examples)/launch/config/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
<rosparam command="load" file="$(find rtabmap_examples)/launch/config/tags.yaml" ns="apriltag_ros_continuous_node" />
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen">
<remap from="image_rect" to="$(arg rgb_topic)" />
rtabmap_examples/launch/tests/test_d435i_vio.launch → rtabmap_examples/launch/test_d435i_vio.launch
+6 -6
View File
@@ -5,7 +5,7 @@
Make sure to disable the IR emitter or put a tape on the IR emitter to
avoid VINS tracking the fixed IR points (that would cause large drifts) -->
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="rviz" default="false"/>
<arg name="depth_mode" default="true"/>
<arg name="odom_strategy" default="9"/> <!-- default VINS -->
@@ -34,7 +34,7 @@
<!-- RTAB-Map: depth mode -->
<!-- We have to launch stereo_odometry externally from rtabmap.launch so that rtabmap can use RGB-D input -->
<group ns="rtabmap">
<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
<node if="$(arg depth_mode)" pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
<remap from="left/image_rect" to="/camera/infra1/image_rect_raw"/>
<remap from="right/image_rect" to="/camera/infra2/image_rect_raw"/>
<remap from="left/camera_info" to="/camera/infra1/camera_info"/>
@@ -44,7 +44,7 @@
<param name="wait_imu_to_init" value="true"/>
</node>
</group>
<include if="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include if="$(arg depth_mode)" file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3"/>
<arg name="rgb_topic" value="/camera/color/image_raw"/>
<arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/>
@@ -53,12 +53,12 @@
<arg name="approx_sync" value="false"/>
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rtabmap_viz" value="$(arg rtabmap_viz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
<!-- RTAB-Map: Stereo mode -->
<include unless="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include unless="$(arg depth_mode)" file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
<arg name="left_image_topic" value="/camera/infra1/image_rect_raw"/>
<arg name="right_image_topic" value="/camera/infra2/image_rect_raw"/>
@@ -68,7 +68,7 @@
<arg name="frame_id" value="camera_link"/>
<arg name="imu_topic" value="/rtabmap/imu"/>
<arg name="wait_imu_to_init" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rtabmap_viz" value="$(arg rtabmap_viz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
rtabmap_examples/launch/tests/test_icp_odometry.launch → rtabmap_examples/launch/test_icp_odometry.launch
+6 -6
View File
@@ -14,7 +14,7 @@
</include>
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_util/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="depth/camera_info" to="depth_registered/camera_info"/>
<remap from="cloud" to="/voxel_cloud" />
@@ -26,7 +26,7 @@
</node>
<group if="$(arg nodelet)">
<node if="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbdicp_odometry" args="load rtabmap_ros/rgbdicp_odometry camera_nodelet_manager">
<node if="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbdicp_odometry" args="load rtabmap_odom/rgbdicp_odometry camera_nodelet_manager">
<remap from="scan_cloud" to="/voxel_cloud"/>
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="rgb/camera_info"/>
@@ -41,7 +41,7 @@
<param name="Icp/PM" type="string" value="$(arg pm)"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
</node>
<node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager" output="screen">
<node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_odom/icp_odometry camera_nodelet_manager" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
@@ -57,7 +57,7 @@
</group>
<group unless="$(arg nodelet)">
<node if="$(arg rgbd)" pkg="rtabmap_ros" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen">
<node if="$(arg rgbd)" pkg="rtabmap_odom" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="rgb/camera_info"/>
@@ -72,7 +72,7 @@
<param name="Icp/PM" type="string" value="$(arg pm)"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
</node>
<node unless="$(arg rgbd)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node unless="$(arg rgbd)" pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
@@ -95,5 +95,5 @@
args="0 0 0 0 0 0 map odom 100" />
<!-- Visualization RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_launch)/launch/config/rgbd.rviz"/>
</launch>
rtabmap_examples/launch/tests/test_k4a_cloud_only.launch → rtabmap_examples/launch/test_k4a_cloud_only.launch
+5 -5
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<launch>
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
<arg name="point_cloud" value="false"/>
@@ -11,7 +11,7 @@
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz" output="screen">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_util/point_cloud_xyz" output="screen">
<remap from="depth/image" to="/depth_to_rgb/image_raw"/>
<remap from="depth/camera_info" to="/depth_to_rgb/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
@@ -23,7 +23,7 @@
<param name="approx_sync" type="bool" value="false"/>
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="voxel_cloud"/>
<param name="frame_id" type="string" value="camera_base"/>
@@ -37,7 +37,7 @@
<param name="OdomF2M/ScanMaxSize" type="string" value="5000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="camera_base"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
@@ -56,7 +56,7 @@
<param name="Icp/MaxTranslation" type="string" value="0.5"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="camera_base"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
rtabmap_examples/launch/tests/test_laser_assembler.launch → rtabmap_examples/launch/test_laser_assembler.launch
+3 -3
View File
@@ -9,7 +9,7 @@
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="rtabmapviz" default="true" />
<arg name="rtabmap_viz" default="true" />
<arg name="rviz" default="false" />
<!-- Kinect -->
@@ -33,7 +33,7 @@
<node type="periodic_snapshotter" pkg="laser_assembler" name="periodic_snapshotter"/>
<!-- RTAB-Map -->
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="queue_size" value="30" />
@@ -44,7 +44,7 @@
<arg name="subscribe_scan_cloud" value="true"/>
<arg name="scan_cloud_topic" value="/assembled_cloud2"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="rtabmap_viz" value="$(arg rtabmap_viz)"/>
<arg name="rviz" value="$(arg rviz)"/>
</include>
rtabmap_examples/launch/tests/test_map_assembler.launch → rtabmap_examples/launch/test_map_assembler.launch
+2 -2
View File
@@ -7,9 +7,9 @@
-->
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<node pkg="rtabmap_util" type="map_assembler" name="map_assembler">
<remap from="mapData" to="mapData"/>
<param name="regenerate_local_grids" value="true"/>
</node>
</group>
</launch>
</launch>
rtabmap_examples/launch/tests/test_map_optimizer.launch → rtabmap_examples/launch/test_map_optimizer.launch
+6 -6
View File
@@ -1,25 +1,25 @@
<?xml version="1.0"?>
<launch>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="rtabmap_args" value="
--delete_db_on_start
--RGBD/OptimizeMaxError 0
--Optimizer/Iterations 0
--RGBD/ProximityBySpace false"/>
<arg name="rtabmapviz" value="false"/>
<arg name="rtabmap_viz" value="false"/>
</include>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
<node pkg="rtabmap_util" type="map_optimizer" name="map_optimizer"/>
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
<remap from="mapData" to="mapData_optimized"/>
<param name="frame_id" value="camera_link"/>
<param name="subscribe_depth" value="false"/>
</node>
<node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
<node pkg="rtabmap_util" type="map_assembler" name="map_assembler">
<remap from="mapData" to="mapData_optimized"/>
</node>
</group>
</launch>
</launch>
rtabmap_examples/launch/tests/test_obstacles_detection.launch → rtabmap_examples/launch/test_obstacles_detection.launch
+4 -4
View File
@@ -2,11 +2,11 @@
<launch>
<!-- Use stereo_outdoorA.bag for testing -->
<include file="$(find rtabmap_ros)/launch/demo/demo_stereo_outdoor.launch"/>
<include file="$(find rtabmap_demos)/launch/demo_stereo_outdoor.launch"/>
<group ns="/stereo_camera" >
<node pkg="nodelet" type="nodelet" name="obstacles_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz obstacles_manager">
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_util/point_cloud_xyz obstacles_manager">
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
@@ -16,7 +16,7 @@
<param name="max_depth" type="double" value="4"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacles_manager">
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_util/obstacles_detection obstacles_manager">
<remap from="cloud" to="cloudXYZ"/>
<param name="frame_id" type="string" value="base_footprint"/>
@@ -26,4 +26,4 @@
</node>
</group>
</launch>
</launch>
rtabmap_examples/launch/tests/test_ouster.launch → rtabmap_examples/launch/test_ouster.launch
+6 -6
View File
@@ -6,7 +6,7 @@
Hand-held 3D lidar mapping example using only a Ouster OS-1 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
$ roslaunch rtabmap_examples test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
@@ -20,7 +20,7 @@
<arg unless="$(arg use_sim_time)" name="os1_udp_dest"/>
<arg name="frame_id" default="os1_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -42,14 +42,14 @@
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os1_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os1_cloud_node/points"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
@@ -81,7 +81,7 @@
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
@@ -126,7 +126,7 @@
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
rtabmap_examples/launch/tests/test_ouster_gen2.launch → rtabmap_examples/launch/test_ouster_gen2.launch
+9 -9
View File
@@ -8,7 +8,7 @@
Example:
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ roslaunch rtabmap_examples test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
RVIZ: Show TF and /rtabmap/cloud_map topics
@@ -29,7 +29,7 @@
$ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
$ sudo ptp4l -i eth0 -m -f ~/os.conf -S
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
$ roslaunch rtabmap_examples test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-->
@@ -40,7 +40,7 @@
<arg unless="$(arg use_sim_time)" name="udp_dest"/>
<arg name="frame_id" default="os_sensor"/>
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/>
<arg name="scan_20_hz" default="true"/>
@@ -72,14 +72,14 @@
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
@@ -90,7 +90,7 @@
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -122,7 +122,7 @@
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
@@ -171,14 +171,14 @@
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<node if="$(arg assemble)" pkg="rtabmap_util" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="" />
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
rtabmap_examples/launch/tests/test_pointxyzrgb.launch → rtabmap_examples/launch/test_pointxyzrgb.launch
+5 -5
View File
@@ -8,7 +8,7 @@
</include>
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_legacy/data_throttle camera_nodelet_manager">
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
<remap from="depth/image_in" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
@@ -16,7 +16,7 @@
<param name="decimation" type="int" value="2"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_util/point_cloud_xyzrgb camera_nodelet_manager">
<remap from="rgb/image" to="rgb/image_out"/>
<remap from="depth/image" to="depth/image_out"/>
<remap from="rgb/camera_info" to="rgb/camera_info_out"/>
@@ -28,7 +28,7 @@
<param name="normal_k" type="int" value="6"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_util/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="depth/image_out"/>
<remap from="depth/camera_info" to="rgb/camera_info_out"/>
<remap from="cloud" to="/voxel_cloud_xyz" />
@@ -58,7 +58,7 @@
</group>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_legacy/stereo_throttle standalone_nodelet">
<remap from="left/image" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
@@ -68,7 +68,7 @@
<param name="approx_sync" type="bool" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_util/point_cloud_xyzrgb standalone_nodelet">
<remap from="left/image" to="/stereo_camera/left/image_rect_color_throttle"/>
<remap from="right/image" to="/stereo_camera/right/image_rect_throttle"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle_throttle"/>
rtabmap_examples/launch/tests/test_prior.launch → rtabmap_examples/launch/test_prior.launch
+6 -6
View File
@@ -15,7 +15,7 @@
Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
using test_prior_rename_kinect_bag_tf.py in this directory!
$ roslaunch rtabmap_ros test_prior.launch
$ roslaunch rtabmap_examples test_prior.launch
$ chmod +x test_prior_tf_to_pose.py
$ ./test_prior_tf_to_pose.py
$ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
@@ -25,7 +25,7 @@
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<arg name="rtabmap_viz" default="false" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
@@ -34,7 +34,7 @@
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_color"/>
<remap from="depth/image" to="/camera/depth/image"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -45,7 +45,7 @@
</node>
<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="kinect"/>
@@ -63,7 +63,7 @@
</node>
<!-- Visualisation -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_examples)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
@@ -79,6 +79,6 @@
</group>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_examples)/launch/config/rgbdslam_datasets.rviz"/>
</launch>
rtabmap_examples/launch/tests/test_rgbd_image.launch → rtabmap_examples/launch/test_rgbd_image.launch
+4 -4
View File
@@ -11,7 +11,7 @@
<arg name="compressed_rate" default="0"/>
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera_nodelet_manager" output="screen">
<remap from="rgb/image" to="rgb/image_rect_color"/>
<remap from="depth/image" to="depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="rgb/camera_info"/>
@@ -23,7 +23,7 @@
<group ns="rtabmap">
<!-- RGB-D Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="subscribe_rgbd" type="bool" value="true"/>
<remap if="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image/compressed"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image"/>
@@ -32,7 +32,7 @@
<param name="frame_id" type="string" value="camera_link"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="camera_link"/>
@@ -41,7 +41,7 @@
<remap unless="$(arg compressed)" from="rgbd_image" to="/camera/rgbd_image"/>
</node>
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="camera_link"/>
rtabmap_examples/launch/tests/test_rtabmap_nodelets.launch → rtabmap_examples/launch/test_rtabmap_nodelets.launch
+4 -4
View File
@@ -19,21 +19,21 @@
<group ns="camera">
<!-- Use RGBD synchronization -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera_nodelet_manager">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node>
<!-- RGB-D Odometry -->
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
</node>
<!-- RTAB-Map -->
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_ros/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_slam/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
@@ -41,7 +41,7 @@
</node>
<!-- Visualisation -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
rtabmap_examples/launch/tests/test_two_kinects_one_map.launch → rtabmap_examples/launch/test_two_kinects_one_map.launch
+6 -6
View File
@@ -7,7 +7,7 @@
already created with one of the Kinect using mapping tutorial:
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rtabmap.launch args:="-d"
$ roslaunch rtabmap_launch rtabmap.launch args:="-d"
-->
@@ -24,7 +24,7 @@
<arg name="device_id" value="#2" />
</include>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="namespace" value="rtabmap1"/>
<arg name="localization" value="true" />
<arg name="frame_id" value="camera1_link" />
@@ -34,10 +34,10 @@
<arg name="depth_topic" value="/camera1/depth_registered/image_raw" />
<arg name="camera_info_topic" value="/camera1/rgb/camera_info" />
<arg name="rtabmapviz" value="false"/>
<arg name="rtabmap_viz" value="false"/>
</include>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
<arg name="namespace" value="rtabmap2"/>
<arg name="localization" value="true" />
<arg name="frame_id" value="camera2_link" />
@@ -47,10 +47,10 @@
<arg name="depth_topic" value="/camera2/depth_registered/image_raw" />
<arg name="camera_info_topic" value="/camera2/rgb/camera_info" />
<arg name="rtabmapviz" value="false"/>
<arg name="rtabmap_viz" value="false"/>
</include>
<!-- Visualization RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_launch)/launch/config/rgbd.rviz"/>
</launch>
rtabmap_examples/launch/tests/test_undistort_depth.launch → rtabmap_examples/launch/test_undistort_depth.launch
+2 -2
View File
@@ -8,11 +8,11 @@
</include>
<arg name="depth" default="/camera/depth_registered/image_raw" />
<arg name="model" default="$(find rtabmap_ros)/launch/calibration/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
<arg name="model" default="$(find rtabmap_examples)/launch/config/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
<group ns="camera">
<!-- Undistort depth image -->
<node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_ros/undistort_depth camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_legacy/undistort_depth camera_nodelet_manager">
<remap from="depth" to="$(arg depth)"/>
<param name="model" value="$(arg model)"/>
</node>
rtabmap_examples/launch/tests/test_use_odom_features.launch → rtabmap_examples/launch/test_use_odom_features.launch
+4 -4
View File
@@ -19,14 +19,14 @@
<group ns="camera">
<!-- Use RGBD synchronization -->
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_sync/rgbd_sync camera_nodelet_manager">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node>
<!-- RGB-D Odometry -->
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
@@ -34,7 +34,7 @@
</node>
<!-- RTAB-Map -->
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
@@ -42,7 +42,7 @@
</node>
<!-- Visualisation -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
rtabmap_examples/launch/tests/test_velodyne.launch → rtabmap_examples/launch/test_velodyne.launch
+7 -7
View File
@@ -6,12 +6,12 @@
Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_velodyne.launch
$ roslaunch rtabmap_examples test_velodyne.launch
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
-->
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
<arg name="imu_topic" default="/imu/data"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
@@ -59,14 +59,14 @@
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
@@ -111,7 +111,7 @@
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
@@ -155,7 +155,7 @@
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
@@ -165,7 +165,7 @@
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_util/point_cloud_assembler" output="screen">
<remap if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg deskewing)" from="cloud" to="$(arg scan_topic)"/>
<remap from="odom" to="odom"/>
rtabmap_examples/launch/tests/test_velodyne_d435i_deskewing.launch → rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch
+8 -8
View File
@@ -7,12 +7,12 @@
Prerequisities: rtabmap should be built with libpointmatcher
We use D435i imu only for lidar deskewing and icp_odometry guess in this example.
Example:
$ roslaunch rtabmap_ros test_velodyne_d435i_deskewing.launch
$ roslaunch rtabmap_examples test_velodyne_d435i_deskewing.launch
$ rosrun rviz rviz -f map
$ Show TF and /rtabmap/cloud_map topics
-->
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
@@ -71,14 +71,14 @@
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_util/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="$(arg imu_topic)/filtered"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="slerp" value="$(arg slerp)"/>
@@ -89,7 +89,7 @@
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="imu" to="$(arg imu_topic)/filtered"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
@@ -131,7 +131,7 @@
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="true"/>
@@ -177,7 +177,7 @@
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
@@ -187,7 +187,7 @@
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_util/point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
rtabmap_examples/launch/tests/test_velodyne_t265_deskewing.launch → rtabmap_examples/launch/test_velodyne_t265_deskewing.launch
+6 -6
View File
@@ -12,7 +12,7 @@
$ Show TF and /rtabmap/cloud_map topics
-->
<arg name="rtabmapviz" default="true"/>
<arg name="rtabmap_viz" default="true"/>
<arg name="scan_20_hz" default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
<arg name="deskewing" default="true"/>
<arg name="slerp" default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
@@ -68,7 +68,7 @@
</group>
<!-- Lidar Deskewing -->
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
<node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_util/lidar_deskewing" output="screen">
<param name="wait_for_transform" value="0.01"/>
<param name="fixed_frame_id" value="$(arg odom_frame_id)"/>
<param name="slerp" value="$(arg slerp)"/>
@@ -79,7 +79,7 @@
<arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
<group ns="rtabmap">
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<node pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
@@ -119,7 +119,7 @@
<param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" output="screen" args="-d">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgb" type="bool" value="false"/>
@@ -162,7 +162,7 @@
<param name="Icp/CorrespondenceRatio" type="string" value="$(arg loop_ratio)"/>
</node>
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<node if="$(arg rtabmap_viz)" name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
@@ -172,7 +172,7 @@
<remap from="odom_info" to="odom_info"/>
</node>
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_util/point_cloud_assembler" output="screen">
<remap from="cloud" to="$(arg scan_topic_deskewed)"/>
<remap from="odom" to="odom"/>
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
rtabmap_examples/launch/tests/test_prior_rename_kinect_bag_tf.py
-19
View File
@@ -1,19 +0,0 @@
#!/usr/bin/env python
import rosbag
from tf.msg import tfMessage
with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
for topic, msg, t in rosbag.Bag('rgbd_dataset_freiburg3_long_office_household.bag').read_messages():
if topic == "/tf" and msg.transforms:
newList = [];
for m in msg.transforms:
if m.child_frame_id != "/kinect":
newList.append(m)
else:
m.child_frame_id = "/kinect_gt"
newList.append(m)
print 'kinect frame renamed!'
if len(newList)>0:
msg.transforms = newList
outbag.write(topic, msg, t)
else:
outbag.write(topic, msg, t)
rtabmap_examples/launch/tests/test_prior_tf_to_pose.py
-46
View File
@@ -1,46 +0,0 @@
#!/usr/bin/env python
import rospy
import tf
import numpy
import tf2_ros
from geometry_msgs.msg import PoseWithCovarianceStamped
if __name__ == '__main__':
rospy.init_node('tf_to_pose', anonymous=True)
listener = tf.TransformListener()
frame = rospy.get_param('~frame', 'world')
childFrame = rospy.get_param('~child_frame', 'kinect_gt')
outputFrame = rospy.get_param('~output_frame', 'kinect')
cov = rospy.get_param('~cov', 1)
rateParam = rospy.get_param('~rate', 30) # 10hz
pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
print 'start loop!'
rate = rospy.Rate(rateParam)
while not rospy.is_shutdown():
poseOut = PoseWithCovarianceStamped()
try:
now = rospy.get_rostime()
listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
(trans,rot) = listener.lookupTransform(frame, childFrame, now)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
print str(e)
rate.sleep()
continue
poseOut.header.stamp.nsecs = now.nsecs
poseOut.header.stamp.secs = now.secs
poseOut.header.frame_id = outputFrame
poseOut.pose.pose.position.x = trans[0]
poseOut.pose.pose.position.y = trans[1]
poseOut.pose.pose.position.z = trans[2]
poseOut.pose.pose.orientation.x = rot[0]
poseOut.pose.pose.orientation.y = rot[1]
poseOut.pose.pose.orientation.z = rot[2]
poseOut.pose.pose.orientation.w = rot[3]
poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
print str(poseOut)
pub.publish(poseOut)
rate.sleep()