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
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;
}
}