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

rtabmap parameters are now private. rtabmapviz has now a parameter "rtabmap" (rtabmap node name) to get/set parameters. Added clear_params option (default true) to rtabmap.launch. This fixes http://official-rtab-map-forum.206.s1.nabble.com/RTAB-Map-rosmon-is-not-running-twice-td8373.html

Browse Source
This commit is contained in:
matlabbe
2021-09-08 15:41:32 -04:00
parent e0fa1c264a
commit 533669dca7
8 changed files with 48 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/GuiWrapper.h
+1
View File
@@ -117,6 +117,7 @@ private:
rtabmap::MainWindow * mainWindow_; rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_; std::string cameraNodeName_;
double lastOdomInfoUpdateTime_; double lastOdomInfoUpdateTime_;
std::string rtabmapNodeName_;
// odometry subscription stuffs // odometry subscription stuffs
std::string frameId_; std::string frameId_;
include/rtabmap_ros/PreferencesDialogROS.h
+2 -1
View File
@@ -36,7 +36,7 @@ using namespace rtabmap;
class PreferencesDialogROS : public PreferencesDialog class PreferencesDialogROS : public PreferencesDialog
{ {
public: public:
PreferencesDialogROS(const QString & configFile); PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName);
virtual ~PreferencesDialogROS(); virtual ~PreferencesDialogROS();
virtual QString getIniFilePath() const; virtual QString getIniFilePath() const;
@@ -52,6 +52,7 @@ protected:
private: private:
QString configFile_; QString configFile_;
std::string rtabmapNodeName_;
}; };
#endif /* PREFERENCESDIALOGROS_H_ */ #endif /* PREFERENCESDIALOGROS_H_ */
launch/rtabmap.launch
+11 -10
View File
@@ -51,6 +51,7 @@
<arg name="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) --> <arg name="gdb" default="false"/> <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
<arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/> <arg if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
<arg unless="$(arg gdb)" name="launch_prefix" default=""/> <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
<arg name="clear_params" default="true"/>
<arg name="output" default="screen"/> <!-- Control node output (screen or log) --> <arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<arg name="publish_tf_map" default="true"/> <arg name="publish_tf_map" default="true"/>
@@ -162,7 +163,7 @@
<group if="$(arg rgbd_sync)"> <group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" /> <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" /> <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)"> <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -182,7 +183,7 @@
<group if="$(arg rgbd_sync)"> <group if="$(arg rgbd_sync)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" /> <node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" /> <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)"> <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -196,7 +197,7 @@
<group unless="$(arg rgbd_sync)"> <group unless="$(arg rgbd_sync)">
<group if="$(arg subscribe_rgbd)"> <group if="$(arg subscribe_rgbd)">
<node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros"> <node name="republish_rgbd_image" type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
<remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/> <remap if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
<remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/> <remap if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
<remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/> <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
@@ -210,7 +211,7 @@
<group if="$(arg visual_odometry)"> <group if="$(arg visual_odometry)">
<!-- RGB-D Odometry --> <!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/> <remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -238,7 +239,7 @@
</node> </node>
<!-- Stereo Odometry --> <!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/> <remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/> <remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -269,7 +270,7 @@
</group> </group>
<!-- ICP Odometry --> <!-- ICP Odometry -->
<node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)"> <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="scan" to="$(arg scan_topic)"/> <remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/> <remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/> <remap from="odom" to="$(arg odom_topic)"/>
@@ -292,7 +293,7 @@
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/> <param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
</node> </node>
<node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="$(arg output)"> <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/> <remap if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
<remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/> <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
@@ -309,7 +310,7 @@
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/> <param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/> <param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/> <param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
@@ -377,7 +378,7 @@
</node> </node>
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)"> <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/> <param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/> <param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="$(arg depth)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/> <param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
@@ -418,7 +419,7 @@
<!-- Visualization RVIZ --> <!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)"> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
<remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/> <remap if="$(arg stereo)" from="left/image" to="$(arg left_image_topic_relay)"/>
<remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/> <remap if="$(arg stereo)" from="right/image" to="$(arg right_image_topic_relay)"/>
<remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/> <remap if="$(arg stereo)" from="left/camera_info" to="$(arg left_camera_info_topic)"/>
src/CommonDataSubscriber.cpp
-18
View File
@@ -988,24 +988,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
delete rgbdSubs_[i]; delete rgbdSubs_[i];
} }
rgbdSubs_.clear(); rgbdSubs_.clear();
//clear params
ros::NodeHandle pnh("~");
pnh.deleteParam("subscribe_depth");
pnh.deleteParam("subscribe_laserScan");
pnh.deleteParam("subscribe_scan");
pnh.deleteParam("subscribe_scan_cloud");
pnh.deleteParam("subscribe_stereo");
pnh.deleteParam("subscribe_rgb");
pnh.deleteParam("subscribe_rgbd");
pnh.deleteParam("subscribe_odom_info");
pnh.deleteParam("subscribe_user_data");
pnh.deleteParam("odom_frame_id");
pnh.deleteParam("rgbd_cameras");
pnh.deleteParam("depth_cameras");
pnh.deleteParam("queue_size");
pnh.deleteParam("approx_sync");
pnh.deleteParam("stereo_approx_sync");
} }
void CommonDataSubscriber::warningLoop() void CommonDataSubscriber::warningLoop()
src/CoreWrapper.cpp
+12 -19
View File
@@ -610,6 +610,7 @@ void CoreWrapper::onInit()
Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_); Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
} }
pnh.param("is_rtabmap_paused", paused_);
if(paused_) if(paused_)
{ {
NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap."); NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -801,11 +802,10 @@ void CoreWrapper::onInit()
} }
} }
// set public parameters // set private parameters
nh.setParam("is_rtabmap_paused", paused_);
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{ {
nh.setParam(iter->first, iter->second); pnh.setParam(iter->first, iter->second);
} }
userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this); userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
@@ -828,13 +828,6 @@ CoreWrapper::~CoreWrapper()
this->saveParameters(configPath_); this->saveParameters(configPath_);
ros::NodeHandle nh;
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
nh.deleteParam(iter->first);
}
nh.deleteParam("is_rtabmap_paused");
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
if(rtabmap_.getMemory()) if(rtabmap_.getMemory())
{ {
@@ -2719,29 +2712,29 @@ void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{ {
ros::NodeHandle nh; ros::NodeHandle pnh("~");
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{ {
std::string vStr; std::string vStr;
bool vBool; bool vBool;
int vInt; int vInt;
double vDouble; double vDouble;
if(nh.getParam(iter->first, vStr)) if(pnh.getParam(iter->first, vStr))
{ {
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr; iter->second = vStr;
} }
else if(nh.getParam(iter->first, vBool)) else if(pnh.getParam(iter->first, vBool))
{ {
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool); iter->second = uBool2Str(vBool);
} }
else if(nh.getParam(iter->first, vInt)) else if(pnh.getParam(iter->first, vInt))
{ {
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt).c_str(); iter->second = uNumber2Str(vInt).c_str();
} }
else if(nh.getParam(iter->first, vDouble)) else if(pnh.getParam(iter->first, vDouble))
{ {
NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble).c_str(); iter->second = uNumber2Str(vDouble).c_str();
@@ -2822,8 +2815,8 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{ {
paused_ = true; paused_ = true;
NODELET_INFO("rtabmap: paused!"); NODELET_INFO("rtabmap: paused!");
ros::NodeHandle nh; ros::NodeHandle pnh("~");
nh.setParam("is_rtabmap_paused", true); pnh.setParam("is_rtabmap_paused", true);
} }
return true; return true;
} }
@@ -2838,8 +2831,8 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
{ {
paused_ = false; paused_ = false;
NODELET_INFO("rtabmap: resumed!"); NODELET_INFO("rtabmap: resumed!");
ros::NodeHandle nh; ros::NodeHandle pnh("~");
nh.setParam("is_rtabmap_paused", false); pnh.setParam("is_rtabmap_paused", false);
} }
return true; return true;
} }
src/GuiWrapper.cpp
+10 -5
View File
@@ -72,7 +72,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
odomSensorSync_(false), odomSensorSync_(false),
maxOdomUpdateRate_(10), maxOdomUpdateRate_(10),
cameraNodeName_(""), cameraNodeName_(""),
lastOdomInfoUpdateTime_(0) lastOdomInfoUpdateTime_(0),
rtabmapNodeName_("rtabmap")
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
@@ -93,14 +94,18 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
configFile.replace('~', QDir::homePath()); configFile.replace('~', QDir::homePath());
pnh.param("rtabmap", rtabmapNodeName_, rtabmapNodeName_);
ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str()); ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
uSleep(500); uSleep(500);
prefDialog_ = new PreferencesDialogROS(configFile); prefDialog_ = new PreferencesDialogROS(configFile, rtabmapNodeName_);
mainWindow_ = new MainWindow(prefDialog_); mainWindow_ = new MainWindow(prefDialog_);
mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]"); mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
mainWindow_->show(); mainWindow_->show();
bool paused = false; bool paused = false;
nh.param("is_rtabmap_paused", paused, paused); ros::NodeHandle rnh(rtabmapNodeName_);
rnh.param("is_rtabmap_paused", paused, paused);
mainWindow_->setMonitoringState(paused); mainWindow_->setMonitoringState(paused);
// To receive odometry events // To receive odometry events
@@ -249,13 +254,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters(); const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters(); rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
bool modified = false; bool modified = false;
ros::NodeHandle nh; ros::NodeHandle rnh(rtabmapNodeName_);
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{ {
//save only parameters with valid names //save only parameters with valid names
if(defaultParameters.find((*i).first) != defaultParameters.end()) if(defaultParameters.find((*i).first) != defaultParameters.end())
{ {
nh.setParam((*i).first, (*i).second); rnh.setParam((*i).first, (*i).second);
modified = true; modified = true;
} }
else if((*i).first.find('/') != (*i).first.npos) else if((*i).first.find('/') != (*i).first.npos)
src/OdometryROS.cpp
+6 -8
View File
@@ -96,14 +96,6 @@ OdometryROS::~OdometryROS()
warningThread_->join(); warningThread_->join();
delete warningThread_; delete warningThread_;
} }
ros::NodeHandle & pnh = getPrivateNodeHandle();
if(pnh.ok())
{
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
}
delete odometry_; delete odometry_;
} }
@@ -332,6 +324,12 @@ void OdometryROS::onInit()
} }
} }
// set private parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.setParam(iter->first, iter->second);
}
Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_); Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
src/PreferencesDialogROS.cpp
+6 -5
View File
@@ -41,8 +41,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap; using namespace rtabmap;
PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) : PreferencesDialogROS::PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName) :
configFile_(configFile) configFile_(configFile),
rtabmapNodeName_(rtabmapNodeName)
{ {
} }
@@ -84,7 +85,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
path = filePath; path = filePath;
} }
ros::NodeHandle nh; ros::NodeHandle rnh(rtabmapNodeName_);
ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str()); ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str());
bool validParameters = true; bool validParameters = true;
int readCount = 0; int readCount = 0;
@@ -108,7 +109,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
double stamp = UTimer::now(); double stamp = UTimer::now();
std::string tmp; std::string tmp;
bool warned = false; bool warned = false;
while(!nh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0) while(!rnh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
{ {
if(!warned) if(!warned)
{ {
@@ -146,7 +147,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
else else
{ {
std::string value; std::string value;
if(nh.getParam(i->first,value)) if(rnh.getParam(i->first,value))
{ {
//backward compatibility //backward compatibility
if(i->first.compare(Parameters::kIcpStrategy()) == 0) if(i->first.compare(Parameters::kIcpStrategy()) == 0)