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

demo_hector_mapping.launch: when "hector" argument is false, icp_odometry is used

Browse Source
This commit is contained in:
matlabbe
2017-09-09 22:46:37 -04:00
parent bf17e7368f
commit db7118554d
7 changed files with 121 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_hector_mapping.launch
+30 -4
View File
@@ -11,14 +11,17 @@
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<!-- Choose hector_slam or icp_odometry for odometry -->
<arg name="hector" default="true" />
<param name="use_sim_time" type="bool" value="True"/>
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
<node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />
<!-- Odometry from laser scans -->
<!-- We use Hector mapping to generate odometry for us -->
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
<node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="hector_map" />
@@ -42,6 +45,28 @@
<param name="scan_topic" value="/jn0/base_scan"/>
</node>
<!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
<node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
<remap from="scan" to="/jn0/base_scan"/>
<remap from="odom" to="/scanmatch_odom"/>
<remap from="odom_info" to="/rtabmap/odom_info"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0.05"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="5"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0.3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
<param name="Icp/PM" type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
<param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="0"/>
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
</node>
<group ns="rtabmap">
<!-- SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
@@ -65,8 +90,8 @@
<param name="Reg/Strategy" type="string" value="1"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Vis/MaxDepth" type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="false"/>
</node>
<!-- Visualisation RTAB-Map -->
@@ -74,6 +99,7 @@
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
<remap from="rgb/image" to="/data_throttled_image"/>
<remap from="depth/image" to="/data_throttled_image_depth"/>
launch/rtabmap.launch
+1 -1
View File
@@ -172,7 +172,7 @@
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_cloud_normal_k" type="double" value="$(arg scan_normal_k)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
launch/tests/test_icp_odometry.launch
+48 -3
View File
@@ -4,6 +4,8 @@
<!-- We test here ICP odometry using a guess from visual odometry -->
<arg name="rgbd" default="false"/>
<arg name="pm" default="false"/>
<arg name="nodelet" default="false"/>
<include file="$(find freenect_launch)/launch/freenect.launch" >
<arg name="depth_registration" value="true"/>
@@ -22,6 +24,7 @@
<param name="Odom/AlignWithGround" type="string" value="true"/>
</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">
<remap from="scan_cloud" to="/voxel_cloud"/>
<remap from="depth/image" to="depth_registered/image_raw"/>
@@ -29,25 +32,67 @@
<remap from="rgb/image" to="rgb/image_rect_mono"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_cloud_normal_k" type="int" value="10"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<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">
<node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager" output="screen">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_cloud_normal_k" type="int" value="10"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="1"/>
</node>
</group>
<group unless="$(arg nodelet)">
<node if="$(arg rgbd)" pkg="rtabmap_ros" 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"/>
<remap from="rgb/image" to="rgb/image_rect_mono"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<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">
<remap from="scan_cloud" to="/voxel_cloud"/>
<param name="frame_id" type="string" value="camera_link"/>
<param name="scan_normal_k" type="int" value="10"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PM" type="string" value="$(arg pm)"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="1"/>
</node>
</group>
</group>
<!-- We just use odometry without rtabmap node, so set a static /map->/odom
transform so that rviz config below works out-of-the-box -->
<node pkg="tf" type="static_transform_publisher" name="map_odom"
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"/>
</launch>
src/CoreWrapper.cpp
+7 -1
View File
@@ -162,8 +162,14 @@ void CoreWrapper::onInit()
pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_cloud_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
}
pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
if(pnh.hasParam("flip_scan"))
src/OdometryROS.cpp
+1 -1
View File
@@ -533,7 +533,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
{
sensor_msgs::PointCloud2 cloudMsg;
if(info.localScanMap.channels() == 6)
if(info.localScanMap.channels() >= 5)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
pcl::toROSMsg(*cloud, cloudMsg);
src/nodelets/icp_odometry.cpp
+7 -1
View File
@@ -74,8 +74,14 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle();
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_cloud_normal_k", scanCloudNormalRadius_, scanCloudNormalRadius_);
}
pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
src/nodelets/rgbdicp_odometry.cpp
+7 -1
View File
@@ -111,8 +111,14 @@ private:
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_cloud_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
}
pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");