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

Updated CoreWrapper to directly call rtabmap methods (don't start rtabmap thread)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@735 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-01-15 16:44:59 +00:00
parent eec6f5241e
commit bbb92046aa
4 changed files with 16 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/CoreNode.cpp
-1
View File
@@ -31,7 +31,6 @@ int main(int argc, char** argv)
}
CoreWrapper rtabmap(deleteDbOnStart);
rtabmap.start();
ROS_INFO("RTAB-Map started...");
ros::spin();
rtabmap/src/CoreWrapper.cpp
+14 -19
View File
@@ -35,7 +35,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
rtabmap_ = new Rtabmap();
UEventsManager::addHandler(rtabmap_);
loadNodeParameters(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini");
rtabmap_->init(UDirectory::homeDir()+"/.rtabmap/rtabmap.ini", deleteDbOnStart);
@@ -60,11 +60,6 @@ CoreWrapper::~CoreWrapper()
delete rtabmap_;
}
void CoreWrapper::start()
{
rtabmap_->start();
}
void CoreWrapper::loadNodeParameters(const std::string & configFile)
{
ROS_INFO("Loading parameters from %s", configFile.c_str());
@@ -86,11 +81,11 @@ void CoreWrapper::loadNodeParameters(const std::string & configFile)
void CoreWrapper::saveNodeParameters(const std::string & configFile)
{
ROS_INFO("Saving parameters to %s", configFile.c_str());
printf("Saving parameters to %s\n", configFile.c_str());
if(!UFile::exists(configFile.c_str()))
{
ROS_WARN("Config file doesn't exist, a new one will be created.");
printf("Config file doesn't exist, a new one will be created.\n");
}
ParametersMap parameters = Parameters::getDefaultParameters();
@@ -106,41 +101,41 @@ void CoreWrapper::saveNodeParameters(const std::string & configFile)
Rtabmap::writeParameters(configFile.c_str(), parameters);
std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"/LTM.db";
ROS_INFO("Database/long-term memory (%lu MB) is located at %s/LTM.db", UFile::length(databasePath)/1000000, databasePath.c_str());
std::string databasePath = parameters.at(Parameters::kRtabmapWorkingDirectory())+"LTM.db";
printf("Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
}
void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
{
if(msg->data.size())
{
ROS_INFO("Received image.");
//ROS_INFO("Received image.");
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
UEventsManager::post(new CameraEvent(ptr->image.clone()));
rtabmap_->process(ptr->image.clone());
}
}
bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdResetMemory));
rtabmap_->resetMemory();
return true;
}
bool CoreWrapper::dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));
rtabmap_->dumpData();
return true;
}
bool CoreWrapper::deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDeleteMemory));
rtabmap_->resetMemory(true);
return true;
}
bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
rtabmap_->dumpPrediction();
return true;
}
@@ -157,7 +152,7 @@ void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
}
}
ROS_INFO("Updating parameters");
UEventsManager::post(new ParamEvent(parameters));
rtabmap_->updateParameters(parameters);
}
void CoreWrapper::handleEvent(UEvent * anEvent)
@@ -171,7 +166,7 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
if(infoPub_.getNumSubscribers())
{
ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
//ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
rtabmap::InfoPtr msg(new rtabmap::Info);
msg->refId = stat.refImageId();
msg->loopClosureId = stat.loopClosureId();
@@ -180,7 +175,7 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
if(infoPubEx_.getNumSubscribers())
{
ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
//ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
msg->refId = stat.refImageId();
msg->loopClosureId = stat.loopClosureId();
rtabmap/src/CoreWrapper.h
-2
View File
@@ -30,8 +30,6 @@ public:
CoreWrapper(bool deleteDbOnStart = false);
virtual ~CoreWrapper();
void start();
private:
void imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg);
void twistCallback(const geometry_msgs::TwistConstPtr & msg);
rtabmap/src/GuiWrapper.cpp
+2 -2
View File
@@ -60,7 +60,7 @@ int GuiWrapper::exec()
void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
{
ROS_INFO("RTAB-Map info ex received!");
//ROS_INFO("RTAB-Map info ex received!");
// Map from ROS struct to rtabmap struct
rtabmap::Statistics * stat = new rtabmap::Statistics();
@@ -131,7 +131,7 @@ void GuiWrapper::infoExReceivedCallback(const rtabmap::InfoExConstPtr & msg)
stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
}
ROS_INFO("Publishing statistics...");
//ROS_INFO("Publishing statistics...");
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
}