mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-13 04:59:50 +08:00
icp_odometry: publishing odom_filtered_input_scan debugging topic. Added test_velodyne.launch
This commit is contained in:
matlabbe
@@ -0,0 +1,122 @@
|
|||||||
|
|
||||||
|
<launch>
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
|
||||||
|
Prerequisities: rtabmap should be built with libpointmatcher
|
||||||
|
Example:
|
||||||
|
$ roslaunch velodyne_pointcloud VLP16_points.launch rpm:=600
|
||||||
|
$ roslaunch rtabmap_ros test_velodyne.launch
|
||||||
|
$ rosrun rviz rviz -f map
|
||||||
|
$ Show TF and /rtabmap/cloud_map topics
|
||||||
|
-->
|
||||||
|
|
||||||
|
<arg name="rtabmapviz" default="true"/>
|
||||||
|
<arg name="use_imu" default="false"/> <!-- Assuming IMU fixed to lidar and the top of the lidar always points to the ceiling -->
|
||||||
|
<arg name="scan_20_hz" default="false"/> <!-- If you launch the velodyne with "rpm:=1200" argument -->
|
||||||
|
<arg name="use_sim_time" default="false"/>
|
||||||
|
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
|
||||||
|
|
||||||
|
<arg name="frame_id" default="velodyne"/>
|
||||||
|
|
||||||
|
<node if="$(arg use_imu)" pkg="hector_imu_attitude_to_tf" type="imu_attitude_to_tf_node" name="imu_attitude_to_tf_node" output="screen">
|
||||||
|
<remap from="imu_topic" to="/imu/data" />
|
||||||
|
<param name="base_stabilized_frame" type="string" value="base_stabilized" />
|
||||||
|
<param name="base_frame" type="string" value="velodyne" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<group ns="rtabmap">
|
||||||
|
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
|
||||||
|
<remap from="scan_cloud" to="/velodyne_points"/>
|
||||||
|
<param name="frame_id" type="string" value="$(arg frame_id)"/>
|
||||||
|
<param name="odom_frame_id" type="string" value="odom"/>
|
||||||
|
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
|
||||||
|
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
|
||||||
|
|
||||||
|
<remap if="$(arg use_imu)" from="imu" to="/imu/data"/>
|
||||||
|
<param if="$(arg use_imu)" name="guess_frame_id" type="string" value="base_stabilized"/>
|
||||||
|
<param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
|
||||||
|
|
||||||
|
<!-- ICP parameters -->
|
||||||
|
<param name="Icp/PointToPlane" type="string" value="true"/>
|
||||||
|
<param name="Icp/Iterations" type="string" value="10"/>
|
||||||
|
<param name="Icp/VoxelSize" type="string" value="0.3"/>
|
||||||
|
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
|
||||||
|
<param name="Icp/Epsilon" type="string" value="0.001"/>
|
||||||
|
<param name="Icp/PointToPlaneK" type="string" value="20"/>
|
||||||
|
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
|
||||||
|
<param name="Icp/MaxTranslation" type="string" value="2"/>
|
||||||
|
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
|
||||||
|
<param name="Icp/PM" type="string" value="true"/>
|
||||||
|
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
|
||||||
|
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
|
||||||
|
|
||||||
|
<!-- Odom parameters -->
|
||||||
|
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
|
||||||
|
<param name="Odom/Strategy" type="string" value="0"/>
|
||||||
|
<param name="OdomF2M/ScanSubtractRadius" type="string" value="0.3"/>
|
||||||
|
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node pkg="rtabmap_ros" 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"/>
|
||||||
|
<param name="subscribe_scan_cloud" type="bool" value="true"/>
|
||||||
|
<param name="approx_sync" type="bool" value="false"/>
|
||||||
|
|
||||||
|
<remap from="scan_cloud" to="assembled_cloud"/>
|
||||||
|
|
||||||
|
<!-- RTAB-Map's parameters -->
|
||||||
|
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
|
||||||
|
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
|
||||||
|
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
|
||||||
|
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
|
||||||
|
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
|
||||||
|
<param name="RGBD/AngularUpdate" type="string" value="0.05"/>
|
||||||
|
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
|
||||||
|
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
|
||||||
|
<param name="Mem/STMSize" type="string" value="30"/>
|
||||||
|
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
|
||||||
|
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
|
||||||
|
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
|
||||||
|
|
||||||
|
<param name="Reg/Strategy" type="string" value="1"/>
|
||||||
|
<param name="Grid/CellSize" type="string" value="0.1"/>
|
||||||
|
<param name="Grid/RangeMax" type="string" value="20"/>
|
||||||
|
<param name="Grid/ClusterRadius" type="string" value="1"/>
|
||||||
|
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
|
||||||
|
|
||||||
|
<!-- ICP parameters -->
|
||||||
|
<param name="Icp/VoxelSize" type="string" value="0.3"/>
|
||||||
|
<param name="Icp/PointToPlaneK" type="string" value="20"/>
|
||||||
|
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
|
||||||
|
<param name="Icp/PointToPlane" type="string" value="true"/>
|
||||||
|
<param name="Icp/Iterations" type="string" value="10"/>
|
||||||
|
<param name="Icp/Epsilon" type="string" value="0.001"/>
|
||||||
|
<param name="Icp/MaxTranslation" type="string" value="3"/>
|
||||||
|
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
|
||||||
|
<param name="Icp/PM" type="string" value="true"/>
|
||||||
|
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
|
||||||
|
<param name="Icp/CorrespondenceRatio" type="string" value="0.4"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" 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"/>
|
||||||
|
<param name="subscribe_scan_cloud" type="bool" value="true"/>
|
||||||
|
<param name="approx_sync" type="bool" value="false"/>
|
||||||
|
<remap from="scan_cloud" to="/velodyne_points"/>
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
|
||||||
|
<remap from="cloud" to="/velodyne_points"/>
|
||||||
|
<remap from="odom" to="odom"/>
|
||||||
|
<param if="$(arg scan_20_hz)" name="max_clouds" type="int" value="20" />
|
||||||
|
<param unless="$(arg scan_20_hz)" name="max_clouds" type="int" value="10" />
|
||||||
|
<param name="fixed_frame_id" type="string" value="" />
|
||||||
|
</node>
|
||||||
|
</group>
|
||||||
|
|
||||||
|
</launch>
|
||||||
@@ -96,6 +96,8 @@ private:
|
|||||||
|
|
||||||
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
|
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
|
||||||
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
|
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
|
||||||
|
|
||||||
|
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void updateParameters(ParametersMap & parameters)
|
virtual void updateParameters(ParametersMap & parameters)
|
||||||
@@ -229,10 +231,26 @@ private:
|
|||||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
|
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
|
||||||
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
||||||
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
|
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
|
||||||
|
|
||||||
|
if(filtered_scan_pub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
pcl::toROSMsg(*pclScanNormal, msg);
|
||||||
|
msg.header = scanMsg->header;
|
||||||
|
filtered_scan_pub_.publish(msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
scan = util3d::laserScan2dFromPointCloud(*pclScan);
|
scan = util3d::laserScan2dFromPointCloud(*pclScan);
|
||||||
|
|
||||||
|
if(filtered_scan_pub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
pcl::toROSMsg(*pclScan, msg);
|
||||||
|
msg.header = scanMsg->header;
|
||||||
|
filtered_scan_pub_.publish(msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -285,6 +303,13 @@ private:
|
|||||||
maxLaserScans /= scanDownsamplingStep_;
|
maxLaserScans /= scanDownsamplingStep_;
|
||||||
}
|
}
|
||||||
scan = util3d::laserScanFromPointCloud(*pclScan);
|
scan = util3d::laserScanFromPointCloud(*pclScan);
|
||||||
|
if(filtered_scan_pub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
pcl::toROSMsg(*pclScan, msg);
|
||||||
|
msg.header = cloudMsg->header;
|
||||||
|
filtered_scan_pub_.publish(msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -316,10 +341,26 @@ private:
|
|||||||
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
|
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
|
||||||
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
|
||||||
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
|
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
|
||||||
|
|
||||||
|
if(filtered_scan_pub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
pcl::toROSMsg(*pclScanNormal, msg);
|
||||||
|
msg.header = cloudMsg->header;
|
||||||
|
filtered_scan_pub_.publish(msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
scan = util3d::laserScanFromPointCloud(*pclScan);
|
scan = util3d::laserScanFromPointCloud(*pclScan);
|
||||||
|
|
||||||
|
if(filtered_scan_pub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
pcl::toROSMsg(*pclScan, msg);
|
||||||
|
msg.header = cloudMsg->header;
|
||||||
|
filtered_scan_pub_.publish(msg);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -344,6 +385,7 @@ protected:
|
|||||||
private:
|
private:
|
||||||
ros::Subscriber scan_sub_;
|
ros::Subscriber scan_sub_;
|
||||||
ros::Subscriber cloud_sub_;
|
ros::Subscriber cloud_sub_;
|
||||||
|
ros::Publisher filtered_scan_pub_;
|
||||||
int scanCloudMaxPoints_;
|
int scanCloudMaxPoints_;
|
||||||
int scanDownsamplingStep_;
|
int scanDownsamplingStep_;
|
||||||
double scanVoxelSize_;
|
double scanVoxelSize_;
|
||||||
|
|||||||
Reference in New Issue
Block a user