i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Making odometry's services private (under odometry node namespace) like rtabmap node instead of being global. For example, "reset_odom" will be under "rgbd_odometry/reset_odom". Added new parameter for rtabmap_viz to call correctly odometry services, with "odometry_node_name" set to "rgbd_odometry" by default.

Browse Source
This commit is contained in:
matlabbe
2025-10-19 17:02:25 -07:00
parent 3b5a4ed675
commit 2477bc21c3
20 changed files with 115 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_demos/launch/husky/husky_slam2d.launch.py
+2 -1
View File
@@ -123,6 +123,7 @@ def generate_launch_description():
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
namespace=robot_ns,
parameters=[rtabmap_parameters, shared_parameters],
parameters=[rtabmap_parameters, shared_parameters,
{"odometry_node_name": "icp_odometry"}],
remappings=remappings),
])
rtabmap_demos/launch/husky/husky_slam3d.launch.py
+2 -1
View File
@@ -141,6 +141,7 @@ def generate_launch_description():
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
namespace=robot_ns,
parameters=[rtabmap_parameters, shared_parameters],
parameters=[rtabmap_parameters, shared_parameters,
{"odometry_node_name": "icp_odometry"}],
remappings=remappings),
])
rtabmap_demos/launch/husky/husky_slam3d_assemble.launch.py
+2 -1
View File
@@ -129,6 +129,7 @@ def generate_launch_description():
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
namespace=robot_ns,
parameters=[rtabmap_parameters, shared_parameters],
parameters=[rtabmap_parameters, shared_parameters,
{"odometry_node_name": "icp_odometry"}],
remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]),
])
rtabmap_demos/launch/isaac/isaac_vslam.launch.py
+2 -1
View File
@@ -103,7 +103,8 @@ def launch_setup(context, *args, **kwargs):
condition=IfCondition(rtabmap_viz),
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
namespace='rtabmap',
parameters=[parameters],
parameters=[parameters,
{"odometry_node_name": vo_node_prefix+'_odometry'}],
remappings=remappings),
]
rtabmap_demos/launch/stereo_outdoor_demo.launch.py
+2 -1
View File
@@ -141,7 +141,8 @@ def generate_launch_description():
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
parameters=[parameters],
parameters=[parameters,
{"odometry_node_name": 'stereo_odometry'}],
remappings=remappings),
Node(
package='rviz2', executable='rviz2', name="rviz2", output='screen',
rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py
+1 -1
View File
@@ -65,7 +65,7 @@ def generate_launch_description():
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[parameters],
remappings=remappings,
arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
arguments=['-d', '--udebug']), # This will delete the previous database (~/.ros/rtabmap.db)
# Localization mode:
Node(
rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py
+2 -1
View File
@@ -76,7 +76,8 @@ def launch_setup(context, *args, **kwargs):
# Visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[parameters],
parameters=[parameters,
{"odometry_node_name": 'icp_odometry'}],
remappings=remappings),
]
rtabmap_demos/launch/turtlebot4/turtlebot4_slam.launch.py
+2 -1
View File
@@ -114,6 +114,7 @@ def generate_launch_description():
Node(
condition=IfCondition(rtabmap_viz),
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[rtabmap_parameters, shared_parameters],
parameters=[rtabmap_parameters, shared_parameters,
{"odometry_node_name": 'icp_odometry'}],
remappings=remappings),
])
rtabmap_examples/launch/euroc_datasets.launch.py
+2 -1
View File
@@ -83,7 +83,8 @@ def generate_launch_description():
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[parameters],
parameters=[parameters,
{'odometry_node_name': "stereo_odometry"}],
remappings=remappings),
# Image rectification and publishing synchronized camera_info
rtabmap_examples/launch/lidar3d.launch.py
+2 -1
View File
@@ -154,7 +154,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[shared_parameters, rtabmap_parameters],
parameters=[shared_parameters, rtabmap_parameters,
{'odometry_node_name': "icp_odometry"}],
remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
]
rtabmap_examples/launch/lidar3d_assemble.launch.py
+2 -1
View File
@@ -173,7 +173,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
# Just for visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[shared_parameters, rtabmap_parameters],
parameters=[shared_parameters, rtabmap_parameters,
{'odometry_node_name': "icp_odometry"}],
remappings=remappings + [('scan_cloud', viz_topic)])
]
rtabmap_examples/launch/lidar3d_assemble_x2.launch.py
+2 -1
View File
@@ -201,7 +201,8 @@ def launch_setup(context: LaunchContext, *args, **kwargs):
# Just for visualization
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[shared_parameters, rtabmap_parameters],
parameters=[shared_parameters, rtabmap_parameters,
{'odometry_node_name': "icp_odometry"}],
remappings=remappings + [('scan_cloud', viz_topic)])
]
rtabmap_examples/launch/realsense_d435i_color.launch.py
+11 -1
View File
@@ -37,6 +37,15 @@ def generate_launch_description():
# Make sure IR emitter is enabled
SetParameter(name='depth_module.emitter_enabled', value=1),
DeclareLaunchArgument(
'args', default_value='',
description='Extra arguments set to rtabmap and odometry nodes.'),
DeclareLaunchArgument(
'odom_args', default_value='',
description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
# Launch camera driver
IncludeLaunchDescription(
@@ -55,13 +64,14 @@ def generate_launch_description():
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
remappings=remappings),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
remappings=remappings,
arguments=['-d']),
arguments=['-d', LaunchConfiguration("args")]),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
rtabmap_examples/launch/realsense_d435i_infra.launch.py
+10 -1
View File
@@ -35,6 +35,14 @@ def generate_launch_description():
DeclareLaunchArgument(
'unite_imu_method', default_value='2',
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
DeclareLaunchArgument(
'args', default_value='',
description='Extra arguments set to rtabmap and odometry nodes.'),
DeclareLaunchArgument(
'odom_args', default_value='',
description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
#Hack to disable IR emitter
SetParameter(name='depth_module.emitter_enabled', value=0),
@@ -56,13 +64,14 @@ def generate_launch_description():
Node(
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
parameters=parameters,
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
remappings=remappings),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
remappings=remappings,
arguments=['-d']),
arguments=['-d', LaunchConfiguration("args")]),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
rtabmap_examples/launch/realsense_d435i_stereo.launch.py
+17 -6
View File
@@ -16,11 +16,11 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
def generate_launch_description():
parameters=[{
parameters={
'frame_id':'camera_link',
'subscribe_stereo':True,
'subscribe_odom_info':True,
'wait_imu_to_init':True}]
'wait_imu_to_init':True}
remappings=[
('imu', '/imu/data'),
@@ -38,6 +38,15 @@ def generate_launch_description():
#Hack to disable IR emitter
SetParameter(name='depth_module.emitter_enabled', value=0),
DeclareLaunchArgument(
'args', default_value='',
description='Extra arguments set to rtabmap and odometry nodes.'),
DeclareLaunchArgument(
'odom_args', default_value='',
description='Extra arguments just for odometry node. If the same argument is already set in \"args\", it will be overwritten by the one in \"odom_args\".'),
# Launch camera driver
IncludeLaunchDescription(
@@ -55,18 +64,20 @@ def generate_launch_description():
Node(
package='rtabmap_odom', executable='stereo_odometry', output='screen',
parameters=parameters,
parameters=[parameters],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
remappings=remappings),
Node(
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=parameters,
parameters=[parameters],
remappings=remappings,
arguments=['-d']),
arguments=['-d', LaunchConfiguration("args")]),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=parameters,
parameters=[parameters,
{'odometry_node_name': "stereo_odometry"}],
remappings=remappings),
# Compute quaternion of the IMU
rtabmap_launch/launch/rtabmap.launch.py
+13 -2
View File
@@ -40,7 +40,17 @@ class ConditionalBool(Substitution):
return self.text_else
def launch_setup(context, *args, **kwargs):
rtabmap_viz_odometry_node_name = "rgbd_odometry"
use_icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
use_icp_odometry = use_icp_odometry == 'true' or use_icp_odometry == 'True'
use_stereo_odometry = LaunchConfiguration('stereo').perform(context)
use_stereo_odometry = use_stereo_odometry == 'true' or use_stereo_odometry == 'True'
if use_icp_odometry:
rtabmap_viz_odometry_node_name = "icp_odometry"
elif use_stereo_odometry:
rtabmap_viz_odometry_node_name = "stereo_odometry"
return [
DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
@@ -359,7 +369,8 @@ def launch_setup(context, *args, **kwargs):
"qos_scan": LaunchConfiguration('qos_scan'),
"qos_odom": LaunchConfiguration('qos_odom'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_user_data": LaunchConfiguration('qos_user_data')
"qos_user_data": LaunchConfiguration('qos_user_data'),
"odometry_node_name": rtabmap_viz_odometry_node_name
}],
remappings=[
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
rtabmap_odom/src/OdometryROS.cpp
+9 -8
View File
@@ -370,15 +370,16 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
odometry_->reset(initialPose_);
}
resetSrv_ = this->create_service<std_srvs::srv::Empty>("reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resetToPoseSrv_ = this->create_service<rtabmap_msgs::srv::ResetPose>("reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
pauseSrv_ = this->create_service<std_srvs::srv::Empty>("pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resumeSrv_ = this->create_service<std_srvs::srv::Empty>("resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
const std::string servicePrefix = get_name() + std::string("/");
resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset_odom", std::bind(&OdometryROS::resetOdom, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resetToPoseSrv_ = this->create_service<rtabmap_msgs::srv::ResetPose>(servicePrefix + "reset_odom_to_pose", std::bind(&OdometryROS::resetToPose, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause_odom", std::bind(&OdometryROS::pause, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume_odom", std::bind(&OdometryROS::resume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>("log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>("log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>("log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>("log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&OdometryROS::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&OdometryROS::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&OdometryROS::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&OdometryROS::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
rtabmap_viz/include/rtabmap_viz/GuiWrapper.h
+1 -1
View File
@@ -116,9 +116,9 @@ private:
private:
rtabmap::PreferencesDialog * prefDialog_;
rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_;
double lastOdomInfoUpdateTime_;
std::string rtabmapNodeName_;
std::string odometryNodeName_;
// odometry subscription stuffs
std::string frameId_;
rtabmap_viz/src/GuiWrapper.cpp
+30 -39
View File
@@ -65,9 +65,9 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
Node("rtabmap_viz", options),
rtabmap_sync::CommonDataSubscriber(*this, true),
mainWindow_(0),
cameraNodeName_(""),
lastOdomInfoUpdateTime_(0),
rtabmapNodeName_("rtabmap"),
odometryNodeName_("rgbd_odometry"),
frameId_("base_link"),
odomFrameId_(""),
waitForTransform_(0.2), // 200 ms
@@ -93,7 +93,10 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
configFile.replace('~', QDir::homePath());
rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
rtabmapNodeName_ = this->declare_parameter("rtabmap_node_name", rtabmapNodeName_);
odometryNodeName_ = this->declare_parameter("odometry_node_name", odometryNodeName_);
RCLCPP_INFO(get_logger(), "%s: rtabmap_node_name = %s", get_name(), rtabmapNodeName_.c_str());
RCLCPP_INFO(get_logger(), "%s: odometry_node_name = %s", get_name(), odometryNodeName_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap_viz: Using configuration from \"%s\"", configFile.toStdString().c_str());
uSleep(500);
@@ -114,9 +117,17 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_);
cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
subscribeInfoOnly = this->declare_parameter("subscribe_info_only", subscribeInfoOnly);
initCachePath = this->declare_parameter("init_cache_path", initCachePath);
RCLCPP_INFO(get_logger(), "%s: frame_id = \"%s\"", get_name(), frameId_.c_str());
RCLCPP_INFO(get_logger(), "%s: odom_frame_id = \"%s\"", get_name(), odomFrameId_.c_str());
RCLCPP_INFO(get_logger(), "%s: wait_for_transform = %f", get_name(), waitForTransform_);
RCLCPP_INFO(get_logger(), "%s: odom_sensor_sync = %s", get_name(), odomSensorSync_?"true":"false");
RCLCPP_INFO(get_logger(), "%s: max_odom_update_rate = %f", get_name(), maxOdomUpdateRate_);
RCLCPP_INFO(get_logger(), "%s: subscribe_info_only = %s", get_name(), subscribeInfoOnly?"true":"false");
RCLCPP_INFO(get_logger(), "%s: init_cache_path = \"%s\"", get_name(), initCachePath.c_str());
if(initCachePath.size())
{
initCachePath = uReplaceChar(initCachePath, '~', UDirectory::homeDir());
@@ -327,7 +338,7 @@ bool GuiWrapper::callMapDataService(const std::string & name, bool global, bool
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available.", name.c_str());
RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", name.c_str());
}
return false;
}
@@ -356,7 +367,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
RCLCPP_INFO(this->get_logger(), "Parameters updated");
auto client = std::make_shared<rclcpp::AsyncParametersClient>(this, rtabmapNodeName_);
if (!client->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(this->get_logger(), "Can't call rtabmap parameters service, is the node running?");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s\" parameters service, is the node running? If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
else
{
@@ -374,28 +385,18 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
{
if(!callEmptyService(rtabmapNodeName_+"/reset"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPause)
{
// Pause the camera if the rtabmap/camera node is used
if(!cameraNodeName_.empty())
{
std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
if(system(str.c_str()) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str());
}
}
// Pause visual_odometry
callEmptyService("pause_odom");
// Pause visual_odometry (can fail silently)
callEmptyService(odometryNodeName_ + "/pause_odom");
// Pause rtabmap
if(!callEmptyService(rtabmapNodeName_+"/pause"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/pause\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume)
@@ -403,27 +404,17 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
// Resume rtabmap
if(!callEmptyService(rtabmapNodeName_+"/resume"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/resume\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
// Pause visual_odometry
callEmptyService("resume_odom");
// Resume the camera if the rtabmap/camera node is used
if(!cameraNodeName_.empty())
{
std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
if(system(str.c_str()) !=0)
{
RCLCPP_ERROR(this->get_logger(), "Command \"%s\" returned non zero value.", str.c_str());
}
}
// Pause visual_odometry (can fail silently)
callEmptyService(odometryNodeName_ + "/resume_odom");
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
{
if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/trigger_new_map\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdPublish3DMap)
@@ -466,14 +457,14 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"set_goal\" not available.");
RCLCPP_WARN(this->get_logger(), "Service \"%s/set_goal\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
{
if(!callEmptyService(rtabmapNodeName_+"/cancel_goal"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/cancel_goal\" service. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdLabel)
@@ -492,7 +483,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"set_label\" not available.");
RCLCPP_WARN(this->get_logger(), "Service \"%s/set_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
@@ -508,7 +499,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else
{
RCLCPP_WARN(this->get_logger(), "Service \"remove_label\" not available.");
RCLCPP_WARN(this->get_logger(), "Service \"%s/remove_label\" not available. If necessary, you can remap expected rtabmap node with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRepublishData)
@@ -525,9 +516,9 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else if(anEvent->getClassName().compare("OdometryResetEvent") == 0)
{
if(!callEmptyService("reset_odom"))
if(!callEmptyService(odometryNodeName_ + "/reset_odom"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"reset_odom\" service, (will only work with rtabmap/visual_odometry node.)");
RCLCPP_ERROR(this->get_logger(), "Can't call \"%s/reset_odom\" service (will only work with rtabmap's odometry nodes, you can remap the node name with \"odometry_node_name\" parameter)", odometryNodeName_.c_str());
}
}
return false;
rtabmap_viz/src/PreferencesDialogROS.cpp
+1 -1
View File
@@ -149,7 +149,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
if (!client->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_ERROR(node_->get_logger(), "Can't call rtabmap parameters service, is the node running?");
RCLCPP_ERROR(node_->get_logger(), "Can't call \"%s\" parameters service, is the node running? If necessary, you can remap the expected rtabmap node name with \"rtabmap_node_name\" parameter.", rtabmapNodeName_.c_str());
}
int readCount = 0;
if(client->service_is_ready())