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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-01-20 20:26:58 -05:00
parent b59c46724f 0ebaaa7439
commit 0d7529332b
9 changed files with 127 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+35 -5
View File
@@ -530,8 +530,10 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom + info messages");
interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
interOdomSync_->registerCallback(std::bind(&CoreWrapper::interOdomInfoCallback, this, std::placeholders::_1, std::placeholders::_2));
interOdomSyncSub_.subscribe(this, "inter_odom");
interOdomInfoSyncSub_.subscribe(this, "inter_odom_info");
rmw_qos_profile_t qos = rmw_qos_profile_default;
qos.depth = 100;
interOdomSyncSub_.subscribe(this, "inter_odom", qos);
interOdomInfoSyncSub_.subscribe(this, "inter_odom_info", qos);
}
else
{
@@ -648,6 +650,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>("cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLabelSrv_ = this->create_service<rtabmap_ros::srv::SetLabel>("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
listLabelsSrv_ = this->create_service<rtabmap_ros::srv::ListLabels>("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
removeLabelSrv_ = this->create_service<rtabmap_ros::srv::RemoveLabel>("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
addLinkSrv_ = this->create_service<rtabmap_ros::srv::AddLink>("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getNodesInRadiusSrv_ = this->create_service<rtabmap_ros::srv::GetNodesInRadius>("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
@@ -2322,7 +2325,7 @@ std::map<int, Transform> CoreWrapper::filterNodesToAssemble(
std::map<int, Transform> output;
if(mappingMaxNodes_ > 0)
{
std::map<int, float> nodesDist = graph::findNearestNodes(nodes, currentPose, mappingMaxNodes_);
std::map<int, float> nodesDist = graph::findNearestNodes(currentPose, nodes, 0, 0, mappingMaxNodes_);
for(std::map<int, float>::iterator iter=nodesDist.begin(); iter!=nodesDist.end(); ++iter)
{
if(mappingAltitudeDelta_<=0.0 ||
@@ -4006,6 +4009,29 @@ void CoreWrapper::listLabelsCallback(
}
}
void CoreWrapper::removeLabelCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Request> req,
std::shared_ptr<rtabmap_ros::srv::RemoveLabel::Response>)
{
if(rtabmap_.getMemory())
{
int id = rtabmap_.getMemory()->getSignatureIdByLabel(req->label, true);
if(id == 0)
{
RCLCPP_WARN(this->get_logger(), "Label \"%s\" not found in the map, cannot remove it!", req->label.c_str());
}
else if(!rtabmap_.labelLocation(id, ""))
{
RCLCPP_ERROR(this->get_logger(), "Failed removing label \"%s\".", req->label.c_str());
}
else
{
RCLCPP_INFO(this->get_logger(), "Removed label \"%s\".", req->label.c_str());
}
}
}
void CoreWrapper::addLinkCallback(const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<rtabmap_ros::srv::AddLink::Request> req,
std::shared_ptr<rtabmap_ros::srv::AddLink::Response>)
@@ -4024,18 +4050,20 @@ void CoreWrapper::getNodesInRadiusCallback(
{
RCLCPP_INFO(get_logger(), "Get nodes in radius (%f): node_id=%d pose=(%f,%f,%f)", req->radius, req->node_id, req->x, req->y, req->z);
std::map<int, Transform> poses;
std::map<int, float> dists;
if(req->node_id != 0 || (req->x == 0.0f && req->y == 0.0f && req->z == 0.0f))
{
poses = rtabmap_.getNodesInRadius(req->node_id, req->radius);
poses = rtabmap_.getNodesInRadius(req->node_id, req->radius, req->k, &dists);
}
else
{
poses = rtabmap_.getNodesInRadius(Transform(req->x, req->y, req->z, 0,0,0), req->radius);
poses = rtabmap_.getNodesInRadius(Transform(req->x, req->y, req->z, 0,0,0), req->radius, req->k, &dists);
}
//Optimized graph
res->ids.resize(poses.size());
res->poses.resize(poses.size());
res->dists_sqr.resize(poses.size());
int index = 0;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
iter != poses.end();
@@ -4043,6 +4071,8 @@ void CoreWrapper::getNodesInRadiusCallback(
{
res->ids[index] = iter->first;
transformToPoseMsg(iter->second, res->poses[index]);
UASSERT(dists.find(iter->first) != dists.end());
res->dists_sqr[index] = dists.at(iter->first);
++index;
}
}
src/GuiWrapper.cpp
+17
View File
@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/srv/set_goal.hpp"
#include "rtabmap_ros/srv/set_label.hpp"
#include "rtabmap_ros/srv/remove_label.hpp"
#include "rtabmap_ros/PreferencesDialogROS.h"
float max3( const float& a, const float& b, const float& c)
@@ -442,6 +443,22 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available.");
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
{
UASSERT(cmdEvent->value1().isStr());
auto client = this->create_client<rtabmap_ros::srv::RemoveLabel>("remove_label");
if(client->wait_for_service(std::chrono::seconds(1)))
{
auto request = std::make_shared<rtabmap_ros::srv::RemoveLabel::Request>();
request->label = cmdEvent->value1().toStr();
auto result_future = client->async_send_request(request);
result_future.wait();
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available.");
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Not handled command (%d)...", cmd);
src/nodelets/icp_odometry.cpp
+50 -15
View File
@@ -159,8 +159,8 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
iter = parameters.find(Parameters::kIcpRangeMin());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!this->has_parameter("scan_range_min"))
{
@@ -177,8 +177,8 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
iter = parameters.find(Parameters::kIcpRangeMax());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!this->has_parameter("scan_range_max"))
{
@@ -210,6 +210,11 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
}
}
}
else if(this->has_parameter("scan_voxel_size"))
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
}
iter = parameters.find(Parameters::kIcpPointToPlaneK());
if(iter != parameters.end())
{
@@ -221,8 +226,18 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalK_ = value;
}
else
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(this->has_parameter("scan_normal_k"))
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
if(iter != parameters.end())
{
@@ -234,21 +249,41 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalRadius_ = value;
}
}
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
else
{
if(!this->has_parameter("scan_normal_ground_up"))
{
RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalGroundUp_ = value;
}
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(this->has_parameter("scan_normal_radius"))
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
}
iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
if(iter != parameters.end())
{
float value = uStr2Float(iter->second);
if(value != 0.0f)
{
if(!this->has_parameter("scan_normal_ground_up"))
{
RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
scanNormalGroundUp_ = value;
}
else
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
iter->second = uNumber2Str(scanNormalK_);
}
}
}
else if(this->has_parameter("scan_normal_ground_up"))
{
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
}
}
void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)