mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Added demo_catvehicle_mapping.launch (#592). MapCloud display: fixed clouds requested when changing sensor modality.
This commit is contained in:
matlabbe
@@ -0,0 +1,110 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren, Rahul Bhadani
|
||||
Hannah Grace Mason, Joe Macinnes, Landon Chase Bentley
|
||||
|
||||
Copyright (c) 2015-2018 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This file includes gazebo reference for front stereo camera simulation mounted
|
||||
on the top of the car. For more information and for the tutorials used to create
|
||||
this file, see
|
||||
http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
|
||||
|
||||
|
||||
|
||||
-->
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:property name="M_PI" value="3.1415926535897931" />
|
||||
|
||||
<!--triclops camera-->
|
||||
<gazebo reference="triclops_link">
|
||||
<mu1>0.6</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
</gazebo>
|
||||
|
||||
<joint name="triclops_optical" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="-${M_PI/2} 0 -${M_PI/2}"/>
|
||||
<parent link="triclops_link"/>
|
||||
<child link="triclops_left_optical_link"/>
|
||||
</joint>
|
||||
<link name="triclops_left_optical_link"/>
|
||||
|
||||
<gazebo reference="triclops_link">
|
||||
<sensor type="multicamera" name="triclops">
|
||||
<update_rate>16.0</update_rate>
|
||||
<camera name="left">
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1280</width>
|
||||
<height>960</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<camera name="right">
|
||||
<pose>0 -0.07 0 0 0 0</pose>
|
||||
<horizontal_fov>1.3962634</horizontal_fov>
|
||||
<image>
|
||||
<width>1280</width>
|
||||
<height>960</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.02</near>
|
||||
<far>300</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<plugin name="camera_triclops_controller" filename="libgazebo_ros_multicamera.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<robotNamespace>/$(arg roboname)</robotNamespace>
|
||||
<cameraName>triclops</cameraName>
|
||||
<imageTopicName>image_rect_color</imageTopicName>
|
||||
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
|
||||
<frameName>triclops_left_optical_link</frameName>
|
||||
<hackBaseline>0.07</hackBaseline>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This file includes the control interfaces for ROS-based control
|
||||
through Gazebo. For more information and for the tutorials used to create
|
||||
this file, see
|
||||
http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
|
||||
|
||||
Sensors are included separately, based on the arguments passed to the xacro include
|
||||
|
||||
-->
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
|
||||
<xacro:VLP-16 parent="velodyne_link" name="velodyne" topic="/$(arg roboname)/velodyne_points" organize_cloud="false" hz="10" samples="440" gpu="true">
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0" />
|
||||
</xacro:VLP-16>
|
||||
|
||||
</robot>
|
||||
|
||||
@@ -0,0 +1,85 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
|
||||
|
||||
2) Install https://github.com/jmscslgroup/catvehicle
|
||||
We need a stereo camera and velodyne point cloud looking like the real
|
||||
sensor (ring-like pattern), so we have to modifiy the default catvehicle
|
||||
urdf. First we have to install velodyne_simulator package to have the
|
||||
right gazebo plugin and copy this modified velodyne config:
|
||||
* sudo apt install ros-$ROS_DISTRO-velodyne-simulator
|
||||
* cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_velodyne_points.gazebo ~/catkin_ws/src/catvehicle/urdf/.
|
||||
Secondly, we have to modify the triclops config to make it stereo:
|
||||
* cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_triclops.gazebo ~/catkin_ws/src/catvehicle/urdf/.
|
||||
Set world->map frame instead of world->odom frame here:
|
||||
* https://github.com/jmscslgroup/catvehicle/blob/f58cc64103538bc93cd42dd30f59c4b937151f88/launch/catvehicle.launch#L104
|
||||
|
||||
3) roslaunch catvehicle catvehicle_city.launch gui:=true
|
||||
4) roslaunch catvehicle catvehicle_spawn.launch velodyne_points:=true triclops:=true Z:=5
|
||||
5) roslaunch catvehicle joystick.launch
|
||||
6) SLAM, 3 choices:
|
||||
A) LiDAR-SLAM + Visual loop closure detection:
|
||||
roslaunch rtabmap_ros demo_catvehicle_mapping.launch
|
||||
B) LiDAR-SLAM without camera:
|
||||
roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
|
||||
C) Visual-SLAM without lidar:
|
||||
roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
|
||||
|
||||
Note: Gazebo real-time factor should be equal or below 1. If it is over 1,
|
||||
in gazebo client, select Physics, then change real time update rate to 500.
|
||||
-->
|
||||
<arg name="camera" default="true" />
|
||||
<arg name="lidar" default="true" />
|
||||
<arg name="cell_size" default="0.2" />
|
||||
<arg name="rtabmapviz" default="true" />
|
||||
<arg name="rviz" default="true" />
|
||||
<arg name="light" default="false" /> <!-- Don't record all scans if false -->
|
||||
<arg name="altitude" default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) -->
|
||||
|
||||
<!-- RTAB-Map -->
|
||||
<remap from="/catvehicle/grid_map" to="/catvehicle/map"/>
|
||||
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
|
||||
<arg name="namespace" value="catvehicle"/>
|
||||
|
||||
<!-- LiDAR SLAM parameters -->
|
||||
<arg if="$(arg lidar)" name="args" value="-d --Rtabmap/DetectionRate 2 --Reg/Strategy 1 --OdomF2M/ScanMaxSize 30000 --Odom/ScanKeyFrameThr 0.6 --Icp/VoxelSize $(arg cell_size) --Icp/Iterations 10 --Icp/MaxTranslation 1 --Icp/RangeMin 1 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RangeMin 1 --Grid/RayTracing true --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK 10 --Grid/PreVoxelFiltering false --Grid/3D false --Kp/RoiRatios '0 0 0 0.3' --Vis/RoiRatios '0 0 0 0.3' --RGBD/LinearUpdate 0.2 --Grid/MaxObstacleHeight 4 --GridGlobal/AltitudeDelta $(arg altitude)"/>
|
||||
|
||||
<!-- Visual SLAM parameters -->
|
||||
<arg unless="$(arg lidar)" name="args" value="-d --Rtabmap/DetectionRate 2 --Reg/Strategy 0 --Icp/VoxelSize $(arg cell_size) --Grid/ClusterRadius 1 --Grid/RangeMax 20 --Grid/RangeMin 2 --Grid/RayTracing true --Grid/CellSize $(arg cell_size) --Grid/3D false --Kp/RoiRatios '0 0 0 0.3' --Vis/RoiRatios '0 0 0 0.3' --Grid/DepthRoiRatios '0 0 0 0.3' --Grid/NormalsSegmentation false --Grid/MaxObstacleHeight 4 --Grid/MaxGroundHeight 0.7 --Grid/NoiseFilteringRadius 0.5 --GridGlobal/AltitudeDelta $(arg altitude)"/>
|
||||
|
||||
<arg name="subscribe_scan_cloud" value="$(arg lidar)"/>
|
||||
<arg name="stereo" value="$(arg camera)"/>
|
||||
<arg unless="$(arg camera)" name="depth" value="false"/>
|
||||
<arg unless="$(arg camera)" name="subscribe_rgb" value="false"/>
|
||||
<arg if="$(arg lidar)" name="icp_odometry" value="true"/>
|
||||
<arg unless="$(arg lidar)" name="visual_odometry" value="true"/>
|
||||
|
||||
<arg if="$(arg lidar)" name="odom_topic" value="lidar_odom"/>
|
||||
<arg unless="$(arg lidar)" name="odom_topic" value="visual_odom"/>
|
||||
|
||||
<arg name="odom_guess_frame_id" value="catvehicle/odom"/>
|
||||
<arg name="frame_id" value="catvehicle/base_link"/>
|
||||
<arg name="map_frame_id" value="catvehicle/map"/>
|
||||
<arg if="$(arg lidar)" name="vo_frame_id" value="catvehicle/lidar_odom"/>
|
||||
<arg unless="$(arg lidar)" name="vo_frame_id" value="catvehicle/visual_odom"/>
|
||||
|
||||
<arg name="stereo_namespace" value="/catvehicle/triclops"/>
|
||||
<arg name="right_image_topic" value="/catvehicle/triclops/right/image_rect_color"/>
|
||||
<arg name="scan_cloud_topic" value="/catvehicle/velodyne_points"/>
|
||||
<arg name="rgbd_sync" value="$(arg camera)"/>
|
||||
<arg name="approx_rgbd_sync" value="false"/>
|
||||
<arg name="approx_sync" value="$(eval camera and lidar)"/>
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
<arg name="wait_for_transform" value="0.3"/>
|
||||
|
||||
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
|
||||
|
||||
<arg name="scan_cloud_assembling" value="$(eval lidar and not light)"/>
|
||||
<arg name="scan_cloud_assembling_fixed_frame" value="catvehicle/lidar_odom"/>
|
||||
<arg name="scan_cloud_assembling_range_max" value="60"/>
|
||||
<arg name="scan_cloud_assembling_voxel_size" value="$(arg cell_size)"/>
|
||||
<arg name="scan_cloud_assembling_range_min" value="1"/>
|
||||
</include>
|
||||
|
||||
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find catvehicle)/config/catvehicle.rviz"/>
|
||||
</launch>
|
||||
@@ -26,6 +26,8 @@
|
||||
<arg name="lidar3d_ray_tracing" default="true" />
|
||||
<arg name="lidar3d_grid3d" default="true" />
|
||||
|
||||
<arg name="camera" default="true" />
|
||||
|
||||
<!-- Load Robot Description -->
|
||||
<arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
|
||||
<param name="robot_description" textfile="$(arg model)" />
|
||||
@@ -33,17 +35,19 @@
|
||||
<!-- Lidar 3D based on demo_husky.launch -->
|
||||
<arg if="$(arg lidar3d)" name="cell_size" default="0.2" />
|
||||
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05" />
|
||||
<arg if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK --Grid/3D $(arg lidar3d_grid3d)" />
|
||||
<arg if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK 10 --Grid/3D $(arg lidar3d_grid3d)" />
|
||||
<arg unless="$(arg lidar3d)" name="args3d" value="--Grid/RangeMax 10" /> <!-- 2d scan, use default params -->
|
||||
|
||||
<!-- RTAB-Map -->
|
||||
<remap from="/rtabmap/grid_map" to="/map"/>
|
||||
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
|
||||
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
|
||||
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
|
||||
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d) --Kp/MaxFeatures -1 --RGBD/ProximityBySpace false --Rtabmap/DetectionRate 3"/>
|
||||
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)"/>
|
||||
<arg name="localization" value="$(arg localization)"/>
|
||||
<arg if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
|
||||
<arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
|
||||
<arg name="subscribe_depth" value="$(arg camera)"/>
|
||||
<arg name="subscribe_rgb" value="$(arg camera)"/>
|
||||
<arg name="visual_odometry" value="false"/>
|
||||
<arg name="odom_topic" value="/odom"/>
|
||||
<arg name="frame_id" value="base_link"/>
|
||||
@@ -55,6 +59,9 @@
|
||||
<arg name="rgbd_sync" value="true"/>
|
||||
<arg name="approx_rgbd_sync" value="false"/>
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
<arg name="tag_linear_variance" value="0.01"/>
|
||||
<arg name="tag_angular_variance" value="1"/>
|
||||
|
||||
|
||||
<arg name="scan_cloud_assembling" value="$(arg lidar3d)"/>
|
||||
<arg name="scan_cloud_assembling_fixed_frame" value="odom"/>
|
||||
|
||||
@@ -321,6 +321,7 @@
|
||||
<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) -->
|
||||
|
||||
Reference in New Issue
Block a user