Posted by
heshamhendy on
URL: http://official-rtab-map-forum.206.s1.nabble.com/2D-LiDAR-on-quadrotor-tp9124p9125.html
<!-- Remeber to change sime time!!!!!!!!!!!!!!!!!!!!!!!!!!! -->
<launch>
<!--node pkg="tf" type="static_transform_publisher" name="tf_GX5_imu_baselink" args="0.0 0.0 0.0 0 0.0000000 0.0000000 base_link camera_link 500" /-->
<node pkg="tf" type="static_transform_publisher" name="tf_laser_baselink" args="0.0 -0.05 0.0 0 0.0000000 0.0000000 base_link laser 50" />
<node pkg="tf" type="static_transform_publisher" name="tf_GX5_imu_baselink" args="0.0 0.0 0.0 0 0.0000000 0.0000000 base_link gx5_imu 500" />
<node pkg="tf" type="static_transform_publisher" name="tf_GX5_filteredimu_baselink" args="0.0 0.0 0.0 0 0.0000000 0.0000000 base_link imu 500" />
<arg name="imu_ignore_acc" default="false" />
<arg name="imu_remove_gravitational_acceleration" default="true" />
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz, sensor fusiom nodes at once -->
<!-- arg name="odom_strategy" default="9"/--> <!-- default VINS -->
<arg name="stereo" default="false"/>
<arg if="$(arg stereo)" name="depth" default="false"/>
<arg unless="$(arg stereo)" name="depth" default="true"/>
<arg name="subscribe_rgb" default="$(arg depth)"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="false" />
<arg name="rviz" default="true" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find connect_senses)/rviz/rtabmap_specialized_zed2i.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default="odom"/> <!-- If set, TF is used to get odometry instead of the topic (no need since we have the topic) -->
<arg name="odom_frame_id_init" default=""/> <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/pcdFiles/rtabmap/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/> <!-- to be observed, sometimes complains-->>
<arg name="args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)" /> <!-- deprecated, use "args" argument -->
<?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="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
<arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
<arg unless="$(arg gdb)" name="launch_prefix" default=""/>
<arg name="clear_params" default="true"/>
<arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<arg name="publish_tf_map" default="true"/>
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<?arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="$(arg depth)"/?>
<arg name="approx_sync" default="true"/>
<arg name="approx_sync_max_interval" default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/zed2i/zed_node/rgb/image_rect_color" />
<arg name="depth_topic" default="/zed2i/zed_node/depth/depth_registered" />
<arg name="camera_info_topic" default="/zed2i/zed_node/depth/camera_info" />
<arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="rgbd_sync" default="true"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic, set to true if an odom from VIO will be used. -->
<arg name="approx_rgbd_sync" default="false"/> <!-- false=exact synchronization -->
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="rgbd_topic" default="rgbd_image" />
<arg name="depth_scale" default="1.0" /> <!-- Deprecated, use rgbd_depth_scale instead -->
<arg name="rgbd_depth_scale" default="$(arg depth_scale)" />
<arg name="rgbd_decimation" default="1" />
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="gen_cloud" default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
<arg name="gen_cloud_decimation" default="4"/>
<arg name="gen_cloud_voxel" default="0.05"/>
<arg name="subscribe_scan" default="true"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="$(arg gen_cloud)"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="subscribe_scan_descriptor" default="false"/>
<arg name="scan_descriptor_topic" default="/scan_descriptor"/>
<arg name="scan_cloud_max_points" default="0"/>
<arg name="scan_cloud_filtered" default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
<arg name="gen_scan" default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
<arg name="gen_depth" default="false" /> <!-- Generate depth image from scan_cloud -->
<arg name="gen_depth_decimation" default="1" />
<arg name="gen_depth_fill_holes_size" default="0" />
<arg name="gen_depth_fill_iterations" default="1" />
<arg name="gen_depth_fill_holes_error" default="0.1" />
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="true"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name like /zed2i/zed_node/odom or /odometry/filtered-->
<arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
<arg name="publish_tf_odom" default="true"/> <!-- revise in case of fusion-->>
<arg name="odom_tf_angular_variance" default="0.001"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="0.001"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default=""/> <!-- More arguments for odometry (overwr/zed2i/zed_node/imu/datate same parameters in rtabmap_args) -->
<arg name="odom_sensor_sync" default="true"/>
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="odom_max_rate" default="0"/>
<arg name="odom_expected_rate" default="0"/>
<arg name="imu_topic" default="/gx5/imu/data"/> <!-- only used with VIO approaches: ex: /mavros/imu/data (use!!) or /zed2i/zed_node/imu/data or /gx5/nav/filtered_imu/data--> <!-- there is some thong wrong!!!? -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="use_odom_features" default="false"/>
<arg name="scan_cloud_assembling" default="false"/>
<arg name="scan_cloud_assembling_time" default="1"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_max_clouds" default="0"/> <!-- max_clouds and time should not be set at the same time -->
<arg name="scan_cloud_assembling_fixed_frame" default=""/>
<arg name="scan_cloud_assembling_voxel_size" default="0.05"/>
<arg name="scan_cloud_assembling_range_min" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_range_max" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_radius" default="0.0"/> <!-- 0=disabled -->
<arg name="scan_cloud_assembling_noise_min_neighbors" default="5"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<arg name="gps_topic" default="/gps/fix" /> <!-- gps async subscription -->
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
<arg name="tag_linear_variance" default="0.0001" />
<arg name="tag_angular_variance" default="9999" /> <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
<arg name="fiducial_topic" default="/fiducial_transforms" /> <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<arg if="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)"/>
<arg unless="$(arg rgbd_sync)" name="rgbd_topic_relay" default="$(arg rgbd_topic)_relay"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- relays -->
<group if="$(arg depth)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="depth_scale" type="double" value="$(arg rgbd_depth_scale)"/>
<param name="decimation" type="double" value="$(arg rgbd_decimation)"/>
</node>
</group>
</group>
<group if="$(arg stereo)">
<group unless="$(arg subscribe_rgbd)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
</group>
<group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
</group>
<group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
<param if="$(arg compressed)" name="uncompress" value="true"/>
</node>
</group>
</group>
<node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="$(arg scan_cloud_topic)" />
<param name="decimation" type="double" value="$(arg gen_cloud_decimation)"/>
<param name="voxel_size" type="double" value="$(arg gen_cloud_voxel)"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- Visual odometry -->
<group unless="$(arg icp_odometry)">
<group if="$(arg visual_odometry)">
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
</group>
</group>
<!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_odom)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
<param name="max_clouds" type="int" value="$(arg scan_cloud_assembling_max_clouds)"/>
<param name="fixed_frame_id" type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
<param name="voxel_size" type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
<param name="range_min" type="double" value="$(arg scan_cloud_assembling_range_min)"/>
<param name="range_max" type="double" value="$(arg scan_cloud_assembling_range_max)"/>
<param name="noise_radius" type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
<param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="odom_frame_id_init" type="string" value="$(arg odom_frame_id_init)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/>
<param name="gen_scan" type="bool" value="$(arg gen_scan)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
<param name="gen_depth" type="bool" value="$(arg gen_depth)" />
<param name="gen_depth_decimation" type="int" value="$(arg gen_depth_decimation)" />
<param name="gen_depth_fill_holes_size" type="int" value="$(arg gen_depth_fill_holes_size)" />
<param name="gen_depth_fill_iterations" type="int" value="$(arg gen_depth_fill_iterations)" />
<param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
<remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap from="gps/fix" to="$(arg gps_topic)"/>
<remap from="tag_detections" to="$(arg tag_topic)"/>
<remap from="fiducial_transforms" to="$(arg fiducial_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<param name="Optimizer/Strategy" value="2" />
<param name="Optimizer/Robust" value="true" />
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false" />
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<!-- param name="" value="" /-->
<param name="RGBD/StartAtOrigin" value="true" />
<param name="RGBD/LocalBundleOnLoopClosure" value="true" />
<param name="RGBD/OptimizeMaxError" type="string" value="10.50" />
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="50" />
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_rgbd" type="bool" value="$(eval subscribe_rgbd or use_odom_features)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param unless="$(arg icp_odometry)" name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool" value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
<param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
<remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
<remap if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="scan_descriptor" to="$(arg scan_descriptor_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap unless="$(arg subscribe_rgbd)" from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap unless="$(arg subscribe_rgbd)" from="depth/image" to="$(arg depth_topic_relay)"/>
<remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="4"/>
<param name="voxel_size" type="double" value="0.0"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
</node>
<!-- node pkg="tf" type="static_transform_publisher" name="base_link_connections" args="0 0 0 0 0 0 /map_fused /map 100" /-->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_localization_node" clear_params="true" output="screen">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="$(arg frame_id)"/>
<param name="world_frame" value="odom"/>
<param name="publish_tf" value="false"/>
<param name="transform_time_offset" value="0.0"/>
<!-- /rtabmap/localization_pose -->
<!-- param name="imu0" value="/gx5/nav/filtered_imu/data"/--> <!-- /gx5/nav/filtered_imu/data -->>
<param name="odom0" value="/zed2i/zed_node/odom"/>
<param name="odom1" value="/rtabmap/odom"/> <!-- param name="odom1" value="/rtabmap/odom"/-->
<!-- param name="odom2" value="/gx5/nav/odom"/-->
<!-- param name="pose0" value="/rtabmap/localization_pose"/-->
<!-- 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,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
<!-- rosparam param="odom2_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
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="true"/>
<param name="odom1_differential" value="true"/>
<?param name="odom2_differential" value="true"/>
<param name="imu0_differential" value="false"/?>
<param name="odom0_relative" value="true"/>
<param name="odom1_relative" value="true"/>
<?param name="odom2_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="25"/>
<param name="odom1_queue_size" value="25"/>
<?param name="odom2_queue_size" value="25"/>
<param name="imu0_queue_size" value="50"/?>
<!-- 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>