mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-16 08:10:19 +08:00
rtabmap.launch.py: added options to connect to nav2 (through goal_pose topic or action server). RVIZ/MapCloud: fixed downloading option. rtabmap node: all services are now in node namespace. Added dev containers for Humble and Jazzy.
This commit is contained in:
matlabbe
@@ -0,0 +1,11 @@
|
||||
{
|
||||
"image": "introlab3it/rtabmap_ros:jazzy-latest",
|
||||
"customizations": {
|
||||
"vscode": {
|
||||
"extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
|
||||
}
|
||||
},
|
||||
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
|
||||
"workspaceFolder": "/ros2_ws",
|
||||
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
|
||||
}
|
||||
@@ -274,6 +274,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
|
||||
"publish_tf": LaunchConfiguration('publish_tf_map'),
|
||||
"initial_pose": LaunchConfiguration('initial_pose'),
|
||||
"use_action_for_goal": LaunchConfiguration('use_action_for_goal'),
|
||||
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
|
||||
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
||||
"odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
|
||||
@@ -316,7 +317,8 @@ def launch_setup(context, *args, **kwargs):
|
||||
("tag_detections", LaunchConfiguration('tag_topic')),
|
||||
("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
|
||||
("odom", LaunchConfiguration('odom_topic')),
|
||||
("imu", LaunchConfiguration('imu_topic'))],
|
||||
("imu", LaunchConfiguration('imu_topic')),
|
||||
("goal_out", LaunchConfiguration('output_goal_topic'))],
|
||||
arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]],
|
||||
prefix=LaunchConfiguration('launch_prefix'),
|
||||
namespace=LaunchConfiguration('namespace')),
|
||||
@@ -425,6 +427,9 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('output', default_value='screen', description='Control node output (screen or log).'),
|
||||
DeclareLaunchArgument('initial_pose', default_value='', description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),
|
||||
|
||||
DeclareLaunchArgument('output_goal_topic', default_value='/goal_pose', description='Output goal topic (can be connected to nav2).'),
|
||||
DeclareLaunchArgument('use_action_for_goal', default_value='false', description='Connect to nav2\'s navigate_to_pose action server instead of publishing the output goal topic.'),
|
||||
|
||||
DeclareLaunchArgument('ground_truth_frame_id', default_value='', description='e.g., "world"'),
|
||||
DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'),
|
||||
|
||||
|
||||
@@ -175,6 +175,7 @@ private:
|
||||
void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask );
|
||||
|
||||
private:
|
||||
std::shared_ptr<rclcpp::Node> clientNode_;
|
||||
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataPub_;
|
||||
|
||||
std::map<int, CloudInfoPtr> cloud_infos_;
|
||||
|
||||
@@ -57,7 +57,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
#include <rtabmap_msgs/srv/get_map.hpp>
|
||||
|
||||
|
||||
namespace rtabmap_rviz_plugins
|
||||
{
|
||||
|
||||
@@ -97,6 +96,10 @@ MapCloudDisplay::MapCloudDisplay()
|
||||
//QIcon icon;
|
||||
//this->setIcon(icon);
|
||||
|
||||
auto options = rclcpp::NodeOptions().arguments(
|
||||
{"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"});
|
||||
clientNode_ = std::make_shared<rclcpp::Node>("_", options);
|
||||
|
||||
style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares",
|
||||
"Rendering mode to use, in order of computational complexity.",
|
||||
this, SLOT( updateStyle() ), this );
|
||||
@@ -500,64 +503,64 @@ void MapCloudDisplay::updateCloudParameters()
|
||||
fromScan_ = cloud_from_scan_->getBool();
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
|
||||
void MapCloudDisplay::downloadMap(bool graphOnly)
|
||||
{
|
||||
RCLCPP_ERROR(rviz_ros_node_.lock()->get_raw_node()->get_logger(), "MapCloud plugin: DownloadMap still not working on ros2");
|
||||
return;
|
||||
// FIXME: ros2: can connect to client, rtabmap returns data but the callback here is never called?!
|
||||
/*
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
|
||||
request->global_map = false;
|
||||
request->optimized = true;
|
||||
request->graph_only = graphOnly;
|
||||
std::string rtabmapNs = download_namespace->getStdString();
|
||||
std::string srvName = uFormat("%s/get_map_data", rtabmapNs.c_str());
|
||||
// QMessageBox * messageBox = new QMessageBox(
|
||||
// QMessageBox::NoIcon,
|
||||
// tr("Calling \"%1\" service...").arg(srvName.c_str()),
|
||||
// tr("Downloading the map... please wait (rviz could become gray!)"),
|
||||
// QMessageBox::NoButton);
|
||||
// messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
|
||||
// messageBox->show();
|
||||
// QApplication::processEvents();
|
||||
// uSleep(100); // hack make sure the text in the QMessageBox is shown...
|
||||
// QApplication::processEvents();
|
||||
std::string srvName = rtabmapNs+"/get_map_data";
|
||||
QMessageBox * messageBox = new QMessageBox(
|
||||
QMessageBox::NoIcon,
|
||||
tr("Calling \"%1\" service...").arg(srvName.c_str()),
|
||||
tr("Downloading the map... please wait (rviz could become gray!)"),
|
||||
QMessageBox::NoButton,
|
||||
getAssociatedWidget());
|
||||
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
|
||||
messageBox->show();
|
||||
QApplication::processEvents();
|
||||
uSleep(100); // hack make sure the text in the QMessageBox is shown...
|
||||
QApplication::processEvents();
|
||||
|
||||
RVIZ_COMMON_LOG_WARNING(uFormat("Wait for service %s", srvName.c_str()));
|
||||
auto client = rviz_ros_node_.lock()->get_raw_node()->create_client<rtabmap_ros::srv::GetMap>(srvName);
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str()));
|
||||
|
||||
auto client = clientNode_->create_client<rtabmap_msgs::srv::GetMap>(srvName);
|
||||
if(client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
using ServiceResponseFuture = rclcpp::Client<rtabmap_ros::srv::GetMap>::SharedFuture;
|
||||
auto response_received_callback = [this, &graphOnly](ServiceResponseFuture future) {
|
||||
auto result = future.get();
|
||||
RVIZ_COMMON_LOG_WARNING(uFormat("Process data"));
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str()));
|
||||
auto result = client->async_send_request(request);
|
||||
if (rclcpp::spin_until_future_complete(clientNode_, result) ==
|
||||
rclcpp::FutureReturnCode::SUCCESS)
|
||||
{
|
||||
RVIZ_COMMON_LOG_INFO(uFormat("Process data"));
|
||||
auto future = result.get();
|
||||
if(graphOnly)
|
||||
{
|
||||
//messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(result->data.graph.poses.size()));
|
||||
//QApplication::processEvents();
|
||||
processMapData(result->data);
|
||||
//messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(result->data.graph.poses.size()));
|
||||
|
||||
// QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size()));
|
||||
QApplication::processEvents();
|
||||
processMapData(future->data);
|
||||
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size()));
|
||||
QApplication::processEvents();
|
||||
QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
}
|
||||
else
|
||||
{
|
||||
//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
|
||||
// .arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
|
||||
//QApplication::processEvents();
|
||||
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
|
||||
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
|
||||
QApplication::processEvents();
|
||||
this->reset();
|
||||
processMapData(result->data);
|
||||
//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
|
||||
// .arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
|
||||
processMapData(future->data);
|
||||
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
|
||||
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
|
||||
|
||||
// QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
QTimer::singleShot(1000, messageBox, SLOT(close()));
|
||||
}
|
||||
};
|
||||
RVIZ_COMMON_LOG_WARNING(uFormat("Calling service %s", srvName.c_str()));
|
||||
auto result_future = client->async_send_request(request, response_received_callback);
|
||||
RVIZ_COMMON_LOG_WARNING(uFormat("Wait"));
|
||||
result_future.wait();
|
||||
RVIZ_COMMON_LOG_WARNING(uFormat("Wait end"));
|
||||
} else {
|
||||
std::string msg = uFormat("Failed to call service %s", srvName.c_str());
|
||||
RVIZ_COMMON_LOG_ERROR(msg);
|
||||
messageBox->setText(msg.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -567,8 +570,8 @@ void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
|
||||
srvName.c_str(),
|
||||
rtabmapNs.c_str());
|
||||
RVIZ_COMMON_LOG_ERROR(msg);
|
||||
//messageBox->setText(msg.c_str());
|
||||
}*/
|
||||
messageBox->setText(msg.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void MapCloudDisplay::downloadNamespaceChanged()
|
||||
|
||||
@@ -208,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
if(useActionForGoal_)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support.");
|
||||
useActionForGoal_ = false;
|
||||
}
|
||||
#endif
|
||||
useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
|
||||
@@ -647,45 +648,46 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
}
|
||||
|
||||
// setup services
|
||||
updateSrv_ = this->create_service<std_srvs::srv::Empty>("update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resetSrv_ = this->create_service<std_srvs::srv::Empty>("reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
pauseSrv_ = this->create_service<std_srvs::srv::Empty>("pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resumeSrv_ = this->create_service<std_srvs::srv::Empty>("resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>("load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>("trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
backupDatabase_ = this->create_service<std_srvs::srv::Empty>("backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>("detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>("global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>("cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>("get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>("get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>("get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>("publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>("get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>("set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
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_msgs::srv::SetLabel>("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
const std::string servicePrefix = get_name() + std::string("/");
|
||||
updateSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
backupDatabase_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
#ifdef WITH_OCTOMAP_MSGS
|
||||
#ifdef RTABMAP_OCTOMAP
|
||||
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
#endif
|
||||
#endif
|
||||
//private services
|
||||
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>("log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>("log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>("log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>("log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
int optimizeIterations = 0;
|
||||
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
|
||||
@@ -848,7 +850,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
|
||||
fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1));
|
||||
#endif
|
||||
imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
|
||||
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
|
||||
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
|
||||
|
||||
parametersClient_ = std::make_shared<rclcpp::SyncParametersClient>(this);
|
||||
auto on_parameter_event_callback =
|
||||
@@ -4417,14 +4419,12 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
|
||||
RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
|
||||
}
|
||||
}
|
||||
else
|
||||
#endif
|
||||
if(nextMetricGoalPub_->get_subscription_count())
|
||||
{
|
||||
nextMetricGoalPub_->publish(poseMsg);
|
||||
if(!useActionForGoal_)
|
||||
{
|
||||
lastPublishedMetricGoal_ = currentMetricGoal_;
|
||||
}
|
||||
lastPublishedMetricGoal_ = currentMetricGoal_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -142,7 +142,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
|
||||
UEventsManager::addHandler(this);
|
||||
UEventsManager::addHandler(mainWindow_);
|
||||
|
||||
republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("republish_node_data", 1);
|
||||
republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>(rtabmapNodeName_+"/republish_node_data", 1);
|
||||
|
||||
if(subscribeInfoOnly)
|
||||
{
|
||||
@@ -369,7 +369,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
|
||||
if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
|
||||
{
|
||||
if(!callEmptyService("reset"))
|
||||
if(!callEmptyService(rtabmapNodeName_+"/reset"))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service");
|
||||
}
|
||||
@@ -390,7 +390,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
callEmptyService("pause_odom");
|
||||
|
||||
// Pause rtabmap
|
||||
if(!callEmptyService("pause"))
|
||||
if(!callEmptyService(rtabmapNodeName_+"/pause"))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service");
|
||||
}
|
||||
@@ -398,7 +398,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume)
|
||||
{
|
||||
// Resume rtabmap
|
||||
if(!callEmptyService("resume"))
|
||||
if(!callEmptyService(rtabmapNodeName_+"/resume"))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service");
|
||||
}
|
||||
@@ -418,7 +418,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
}
|
||||
else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
|
||||
{
|
||||
if(!callEmptyService("trigger_new_map"))
|
||||
if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map"))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service");
|
||||
}
|
||||
@@ -429,7 +429,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
UASSERT(cmdEvent->value2().isBool());
|
||||
UASSERT(cmdEvent->value3().isBool());
|
||||
|
||||
if(!callMapDataService("get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool()))
|
||||
if(!callMapDataService(rtabmapNodeName_+"/get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool()))
|
||||
{
|
||||
this->post(new RtabmapEvent3DMap(1)); // service error
|
||||
}
|
||||
@@ -438,7 +438,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
{
|
||||
UASSERT(cmdEvent->value1().isStr() || cmdEvent->value1().isInt() || cmdEvent->value1().isUInt());
|
||||
|
||||
auto client = this->create_client<rtabmap_msgs::srv::SetGoal>("set_goal");
|
||||
auto client = this->create_client<rtabmap_msgs::srv::SetGoal>(rtabmapNodeName_+"/set_goal");
|
||||
if(client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::SetGoal::Request>();
|
||||
@@ -468,7 +468,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
}
|
||||
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
|
||||
{
|
||||
if(!callEmptyService("cancel_goal"))
|
||||
if(!callEmptyService(rtabmapNodeName_+"/cancel_goal"))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service");
|
||||
}
|
||||
@@ -478,7 +478,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
UASSERT(cmdEvent->value1().isStr());
|
||||
UASSERT(cmdEvent->value2().isUndef() || cmdEvent->value2().isInt() || cmdEvent->value2().isUInt());
|
||||
|
||||
auto client = this->create_client<rtabmap_msgs::srv::SetLabel>("set_label");
|
||||
auto client = this->create_client<rtabmap_msgs::srv::SetLabel>(rtabmapNodeName_+"/set_label");
|
||||
if(client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
|
||||
@@ -495,7 +495,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
|
||||
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
|
||||
{
|
||||
UASSERT(cmdEvent->value1().isStr());
|
||||
auto client = this->create_client<rtabmap_msgs::srv::RemoveLabel>("remove_label");
|
||||
auto client = this->create_client<rtabmap_msgs::srv::RemoveLabel>(rtabmapNodeName_+"/remove_label");
|
||||
if(client->wait_for_service(std::chrono::seconds(1)))
|
||||
{
|
||||
auto request = std::make_shared<rtabmap_msgs::srv::RemoveLabel::Request>();
|
||||
|
||||
Reference in New Issue
Block a user