i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge master->ros2 (fixed #674)

Browse Source
This commit is contained in:
matlabbe
2021-11-03 12:05:28 -04:00
parent f54404812f 70bc399907
commit ab1288cccc
8 changed files with 67 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -55,7 +55,7 @@ find_package(octomap_msgs)
find_package(move_base_msgs)
## System dependencies are found with CMake's conventions
find_package(RTABMap 0.20.14 REQUIRED)
find_package(RTABMap 0.20.15 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
find_package(PCL 1.7 REQUIRED COMPONENTS kdtree) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908
launch/tests/test_velodyne.launch
+24 -12
View File
@@ -1,4 +1,4 @@
<!-- -->
<launch>
<!--
@@ -18,15 +18,25 @@
<arg name="scan_topic" default="/velodyne_points"/>
<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"/>
<arg name="queue_size" default="10"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="queue_size_odom" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.2"/>
<arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<arg name="frame_id" default="velodyne"/>
<arg name="queue_size" default="1"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
<arg name="loop_ratio" default="0.4"/> <!-- Set to 0.2 for kitti -->
<!-- Grid parameters -->
<arg name="ground_is_obstacle" default="true"/>
<arg name="grid_max_range" default="20"/>
<arg name="resolution" default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
<arg name="iterations" default="10"/>
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (car, kitti) -->
<!-- For F2M Odometry -->
<arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
<arg name="local_map_size" default="15000"/>
<arg name="key_frame_thr" default="0.8"/>
<!-- For FLOAM Odometry -->
<arg name="floam" default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
<arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
@@ -48,7 +58,7 @@
<remap from="scan_cloud" to="$(arg scan_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="queue_size" type="int" value="$(arg queue_size_odom)"/>
<param name="wait_for_transform_duration" type="double" value="0.2"/>
<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"/>
@@ -75,13 +85,15 @@
<param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp" type="string" value="0.8"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.9"/>
<param name="Odom/ScanKeyFrameThr" type="string" value="$(arg key_frame_thr)"/>
<param if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
<param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="$(arg local_map_size)"/>
<param name="OdomLOAM/Sensor" type="string" value="$(arg floam_sensor)"/>
<param name="OdomLOAM/Resolution" type="string" value="$(arg resolution)"/>
<param if="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
<param unless="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
@@ -109,9 +121,9 @@
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg resolution)"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/RangeMax" type="string" value="$(arg grid_max_range)"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Grid/GroundIsObstacle" type="string" value="$(arg ground_is_obstacle)"/>
<param name="Optimizer/GravitySigma" type="string" value="0.3"/>
<!-- ICP parameters -->
package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_ros</name>
<version>0.20.14</version>
<version>0.20.15</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/CoreWrapper.cpp
+18 -13
View File
@@ -377,7 +377,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
if(!paramValue.empty())
{
if(iter->second.first)
if(!iter->second.second.empty() && parameters_.find(iter->second.second)!=parameters_.end())
{
RCLCPP_WARN(this->get_logger(), "Rtabmap: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
iter->first.c_str(), iter->second.second.c_str(), parameters_.find(iter->second.second)->second.c_str(), paramValue.c_str());
}
else if(iter->second.first)
{
// can be migrated
uInsert(parameters_, ParametersPair(iter->second.second, paramValue));
@@ -403,26 +408,26 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
// Backward compatibility (MapsManager)
mapsManager_.backwardCompatibilityParameters(*this, parameters_);
bool gridFromDepth = Parameters::defaultGridFromDepth();
if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
int gridSensor = Parameters::defaultGridSensor();
if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridSensor()) == parameters_.end())
{
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is "
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (default 1) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is "
"true. The occupancy grid map will be constructed from "
"laser scans. To get occupancy grid map from cloud projection, set \"%s\" "
"to true. To suppress this warning, "
"add <param name=\"%s\" type=\"string\" value=\"false\"/>",
Parameters::kGridFromDepth().c_str(),
Parameters::kGridFromDepth().c_str(),
Parameters::kGridFromDepth().c_str());
parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
"add <param name=\"%s\" type=\"string\" value=\"0\"/>",
Parameters::kGridSensor().c_str(),
Parameters::kGridSensor().c_str(),
Parameters::kGridSensor().c_str());
parameters_.insert(ParametersPair(Parameters::kGridSensor(), "0"));
}
Parameters::parse(parameters_, Parameters::kGridFromDepth(), gridFromDepth);
if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && !gridFromDepth)
Parameters::parse(parameters_, Parameters::kGridSensor(), gridSensor);
if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && gridSensor==0)
{
RCLCPP_INFO(this->get_logger(), "Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is true.",
RCLCPP_INFO(this->get_logger(), "Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" or \"gen_scan\" is true and %s is 0.",
Parameters::kGridRangeMax().c_str(),
Parameters::defaultGridRangeMax(),
Parameters::kGridFromDepth().c_str());
Parameters::kGridSensor().c_str());
parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
}
if(this->isSubscribedToScan3d() && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())
src/MapsManager.cpp
+2 -2
View File
@@ -1209,10 +1209,10 @@ void MapsManager::publishMaps(
else if(poses.size())
{
UWARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). "
"Make sure you activated \"%s\" and \"%s\" to true. "
"Make sure you enabled \"%s\" and set \"%s\"=1. "
"See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.",
(int)poses.size(), (int)octomap_->octree()->size(),
Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str());
Parameters::kGrid3D().c_str(), Parameters::kGridSensor().c_str());
}
}
}
src/MsgConversion.cpp
+1 -1
View File
@@ -1074,7 +1074,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
{
UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_pts.size());
}
if(wordsDescriptors.rows != (int)msg.word_ids.size())
if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_ids.size())
{
UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_ids.size(), wordsDescriptors.rows);
wordsDescriptors = cv::Mat();
src/OdometryROS.cpp
+6 -1
View File
@@ -291,7 +291,12 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
if(get_parameter(iter->first, parameter))
{
std::string vStr = parameter.as_string();
if(iter->second.first && parameters_.find(iter->second.second) != parameters_.end())
if(!iter->second.second.empty() && parameters_.find(iter->second.second)!=parameters_.end())
{
RCLCPP_WARN(this->get_logger(), "Odometry: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
iter->first.c_str(), iter->second.second.c_str(), parameters_.find(iter->second.second)->second.c_str(), vStr.c_str());
}
else if(iter->second.first && parameters_.find(iter->second.second) != parameters_.end())
{
// can be migrated
parameters_.at(iter->second.second)= vStr;
src/nodelets/stereo_odometry.cpp
+14 -1
View File
@@ -180,7 +180,20 @@ void StereoOdometry::callback(
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
return;
}
else if(stereoTransform.isIdentity())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
"Identity transform returned between left and right cameras. Verify that if TF between "
"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
return;
}
}