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

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
src/CommonDataSubscriber.cpp
-18
View File
@@ -988,24 +988,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
delete rgbdSubs_[i];
}
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()
src/CoreWrapper.cpp
+12 -19
View File
@@ -610,6 +610,7 @@ void CoreWrapper::onInit()
Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
}
pnh.param("is_rtabmap_paused", paused_);
if(paused_)
{
NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -801,11 +802,10 @@ void CoreWrapper::onInit()
}
}
// set public parameters
nh.setParam("is_rtabmap_paused", paused_);
// set private parameters
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);
@@ -828,13 +828,6 @@ CoreWrapper::~CoreWrapper()
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());
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&)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
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());
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());
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());
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());
iter->second = uNumber2Str(vDouble).c_str();
@@ -2822,8 +2815,8 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{
paused_ = true;
NODELET_INFO("rtabmap: paused!");
ros::NodeHandle nh;
nh.setParam("is_rtabmap_paused", true);
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", true);
}
return true;
}
@@ -2838,8 +2831,8 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
{
paused_ = false;
NODELET_INFO("rtabmap: resumed!");
ros::NodeHandle nh;
nh.setParam("is_rtabmap_paused", false);
ros::NodeHandle pnh("~");
pnh.setParam("is_rtabmap_paused", false);
}
return true;
}
src/GuiWrapper.cpp
+10 -5
View File
@@ -72,7 +72,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
odomSensorSync_(false),
maxOdomUpdateRate_(10),
cameraNodeName_(""),
lastOdomInfoUpdateTime_(0)
lastOdomInfoUpdateTime_(0),
rtabmapNodeName_("rtabmap")
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
@@ -93,14 +94,18 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
configFile.replace('~', QDir::homePath());
pnh.param("rtabmap", rtabmapNodeName_, rtabmapNodeName_);
ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
uSleep(500);
prefDialog_ = new PreferencesDialogROS(configFile);
prefDialog_ = new PreferencesDialogROS(configFile, rtabmapNodeName_);
mainWindow_ = new MainWindow(prefDialog_);
mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
mainWindow_->show();
bool paused = false;
nh.param("is_rtabmap_paused", paused, paused);
ros::NodeHandle rnh(rtabmapNodeName_);
rnh.param("is_rtabmap_paused", paused, paused);
mainWindow_->setMonitoringState(paused);
// To receive odometry events
@@ -249,13 +254,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
bool modified = false;
ros::NodeHandle nh;
ros::NodeHandle rnh(rtabmapNodeName_);
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
//save only parameters with valid names
if(defaultParameters.find((*i).first) != defaultParameters.end())
{
nh.setParam((*i).first, (*i).second);
rnh.setParam((*i).first, (*i).second);
modified = true;
}
else if((*i).first.find('/') != (*i).first.npos)
src/OdometryROS.cpp
+6 -8
View File
@@ -96,14 +96,6 @@ OdometryROS::~OdometryROS()
warningThread_->join();
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_;
}
@@ -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_.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;
PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
configFile_(configFile)
PreferencesDialogROS::PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName) :
configFile_(configFile),
rtabmapNodeName_(rtabmapNodeName)
{
}
@@ -84,7 +85,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
path = filePath;
}
ros::NodeHandle nh;
ros::NodeHandle rnh(rtabmapNodeName_);
ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str());
bool validParameters = true;
int readCount = 0;
@@ -108,7 +109,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
double stamp = UTimer::now();
std::string tmp;
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)
{
@@ -146,7 +147,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
else
{
std::string value;
if(nh.getParam(i->first,value))
if(rnh.getParam(i->first,value))
{
//backward compatibility
if(i->first.compare(Parameters::kIcpStrategy()) == 0)