mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Added sensor_fusion.launch example (kinect+IMU)
This commit is contained in:
matlabbe
@@ -0,0 +1,143 @@
|
||||
<launch>
|
||||
|
||||
<!-- This launch assumes that you have already
|
||||
started you preferred RGB-D sensor and your IMU.
|
||||
TF between frame_id and the sensors should already be set too. -->
|
||||
|
||||
<arg name="frame_id" default="base_link" />
|
||||
<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="imu_topic" default="/imu/data" />
|
||||
<arg name="imu_ignore_acc" default="true" />
|
||||
<arg name="imu_remove_gravitational_acceleration" default="true" />
|
||||
|
||||
<!-- Localization-only mode -->
|
||||
<arg name="localization" default="false"/>
|
||||
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
|
||||
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
|
||||
|
||||
<group ns="rtabmap">
|
||||
<!-- Visual Odometry -->
|
||||
<node pkg="rtabmap_ros" 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)"/>
|
||||
<remap from="odom" to="/vo"/>
|
||||
|
||||
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||
<param name="publish_tf" type="bool" value="false"/>
|
||||
<param name="publish_null_when_lost" type="bool" value="true"/>
|
||||
|
||||
<param name="Odom/FillInfoData" type="string" value="true"/>
|
||||
<param name="Odom/ResetCountdown" type="string" value="1"/>
|
||||
<param name="Vis/FeatureType" type="string" value="6"/>
|
||||
</node>
|
||||
|
||||
<!-- SLAM -->
|
||||
<node name="rtabmap" pkg="rtabmap_ros" 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)"/>
|
||||
<remap from="depth/image" to="$(arg depth_topic)"/>
|
||||
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
|
||||
<remap from="odom" to="/odometry/filtered"/>
|
||||
|
||||
<param name="Kp/DetectorStrategy" type="string" value="6"/> <!-- use same features as odom -->
|
||||
|
||||
<!-- localization mode -->
|
||||
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
|
||||
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
|
||||
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
|
||||
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
|
||||
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
|
||||
|
||||
<param name="frequency" value="10"/>
|
||||
<param name="sensor_timeout" value="0.5"/>
|
||||
<param name="two_d_mode" value="false"/>
|
||||
|
||||
<param name="odom_frame" value="odom"/>
|
||||
<param name="base_link_frame" value="$(arg frame_id)"/>
|
||||
<param name="world_frame" value="odom"/>
|
||||
|
||||
<param name="transform_time_offset" value="0.0"/>
|
||||
|
||||
<param name="odom0" value="/vo"/>
|
||||
<param name="imu0" value="$(arg imu_topic)"/>
|
||||
|
||||
<!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
|
||||
<rosparam param="odom0_config">[true, true, true,
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
false, false, false,
|
||||
false, false, false]</rosparam>
|
||||
|
||||
<rosparam if="$(arg imu_ignore_acc)" param="imu0_config">[
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
false, false, false] </rosparam>
|
||||
<rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
false, false, false,
|
||||
true, true, true,
|
||||
true, true, true] </rosparam>
|
||||
|
||||
<param name="odom0_differential" value="false"/>
|
||||
<param name="imu0_differential" value="false"/>
|
||||
|
||||
<param name="odom0_relative" value="true"/>
|
||||
<param name="imu0_relative" value="true"/>
|
||||
|
||||
<param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>
|
||||
|
||||
<param name="print_diagnostics" value="true"/>
|
||||
|
||||
<!-- ======== ADVANCED PARAMETERS ======== -->
|
||||
<param name="odom0_queue_size" value="5"/>
|
||||
<param name="imu0_queue_size" value="5"/>
|
||||
|
||||
<!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
|
||||
vroll, vpitch, vyaw, ax, ay, az. -->
|
||||
<rosparam param="process_noise_covariance">[0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0015]</rosparam>
|
||||
|
||||
<!-- The values are ordered as x, y,
|
||||
z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
|
||||
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
|
||||
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,37 @@
|
||||
<launch>
|
||||
|
||||
<arg name="localization" default="false"/>
|
||||
|
||||
<!-- Kinect: -->
|
||||
<include file="$(find freenect_launch)/launch/freenect.launch">
|
||||
<arg name="depth_registration" value="true" />
|
||||
<arg name="publish_tf" value="false" />
|
||||
</include>
|
||||
|
||||
<!-- IMU Sensor: -->
|
||||
<node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
|
||||
<param name="frame_id" value="imu_link"/>
|
||||
<param name="period_ms" value="10"/>
|
||||
<param name="uid" type="string" value="6xDEo7"/>
|
||||
<param name="cov_orientation" type="double" value="0.0005"/>
|
||||
<param name="cov_velocity" type="double" value="0.00025"/>
|
||||
<param name="cov_acceleration" type="double" value="0.1"/>
|
||||
<param name="remove_gravitational_acceleration" type="bool" value="true"/>
|
||||
</node>
|
||||
|
||||
<!-- IMU frame: just over the RGB camera -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
|
||||
args="-0.032 0.0 0.032 0.0 0.0 0.0 /camera_rgb_frame /imu_link 100" />
|
||||
|
||||
<arg name="pi/2" value="1.5707963267948966" />
|
||||
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
|
||||
<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">
|
||||
<arg name="frame_id" value="camera_rgb_frame"/>
|
||||
<arg name="localization" value="$(arg localization)"/>
|
||||
<arg name="imu_remove_gravitational_acceleration" value="false"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
Reference in New Issue
Block a user