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

rtabmap-pkg: Updated rtabmap interface

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@804 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-02-03 22:12:44 +00:00
parent 61ae26327f
commit c362dc628c
2 changed files with 102 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/src/CoreWrapper.cpp
+33 -39
View File
@@ -50,8 +50,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
image_transport::ImageTransport it(nh); image_transport::ImageTransport it(nh);
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this); imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
UEventsManager::addHandler(this);
} }
CoreWrapper::~CoreWrapper() CoreWrapper::~CoreWrapper()
@@ -112,6 +110,8 @@ void CoreWrapper::imageReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
//ROS_INFO("Received image."); //ROS_INFO("Received image.");
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg); cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
rtabmap_->process(ptr->image.clone(), ptr->header.seq); rtabmap_->process(ptr->image.clone(), ptr->header.seq);
const Statistics & stats = rtabmap_->getStatistics();
this->publishStats(stats);
} }
} }
@@ -152,24 +152,19 @@ void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
} }
} }
ROS_INFO("Updating parameters"); ROS_INFO("Updating parameters");
rtabmap_->updateParameters(parameters); rtabmap_->parseParameters(parameters);
} }
void CoreWrapper::handleEvent(UEvent * anEvent) void CoreWrapper::publishStats(const Statistics & stats)
{
if(anEvent->getClassName().compare("RtabmapEvent") == 0)
{ {
if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers()) if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
{ {
RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
const Statistics & stat = rtabmapEvent->getStats();
if(infoPub_.getNumSubscribers()) 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); rtabmap::InfoPtr msg(new rtabmap::Info);
msg->refId = stat.refImageId(); msg->refId = stats.refImageId();
msg->loopClosureId = stat.loopClosureId(); msg->loopClosureId = stats.loopClosureId();
infoPub_.publish(msg); infoPub_.publish(msg);
} }
@@ -177,16 +172,16 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
{ {
//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); rtabmap::InfoExPtr msg(new rtabmap::InfoEx);
msg->refId = stat.refImageId(); msg->refId = stats.refImageId();
msg->loopClosureId = stat.loopClosureId(); msg->loopClosureId = stats.loopClosureId();
// Detailed info // Detailed info
if(stat.extended()) if(stats.extended())
{ {
if(!stat.refImage().empty()) if(!stats.refImage().empty())
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
if(stat.refImage().channels() == 1) if(stats.refImage().channels() == 1)
{ {
img.encoding = sensor_msgs::image_encodings::MONO8; img.encoding = sensor_msgs::image_encodings::MONO8;
} }
@@ -194,16 +189,16 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
{ {
img.encoding = sensor_msgs::image_encodings::BGR8; img.encoding = sensor_msgs::image_encodings::BGR8;
} }
img.image = stat.refImage(); img.image = stats.refImage();
sensor_msgs::ImagePtr rosMsg = img.toImageMsg(); sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header.frame_id = "camera"; rosMsg->header.frame_id = "camera";
rosMsg->header.stamp = ros::Time::now(); rosMsg->header.stamp = ros::Time::now();
msg->refImage = *rosMsg; msg->refImage = *rosMsg;
} }
if(!stat.loopImage().empty()) if(!stats.loopImage().empty())
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
if(stat.loopImage().channels() == 1) if(stats.loopImage().channels() == 1)
{ {
img.encoding = sensor_msgs::image_encodings::MONO8; img.encoding = sensor_msgs::image_encodings::MONO8;
} }
@@ -211,7 +206,7 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
{ {
img.encoding = sensor_msgs::image_encodings::BGR8; img.encoding = sensor_msgs::image_encodings::BGR8;
} }
img.image = stat.loopImage(); img.image = stats.loopImage();
sensor_msgs::ImagePtr rosMsg = img.toImageMsg(); sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header.frame_id = "camera"; rosMsg->header.frame_id = "camera";
rosMsg->header.stamp = ros::Time::now(); rosMsg->header.stamp = ros::Time::now();
@@ -219,21 +214,21 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
} }
//Posterior, likelihood, childCount //Posterior, likelihood, childCount
msg->posteriorKeys = uKeys(stat.posterior()); msg->posteriorKeys = uKeys(stats.posterior());
msg->posteriorValues = uValues(stat.posterior()); msg->posteriorValues = uValues(stats.posterior());
msg->likelihoodKeys = uKeys(stat.likelihood()); msg->likelihoodKeys = uKeys(stats.likelihood());
msg->likelihoodValues = uValues(stat.likelihood()); msg->likelihoodValues = uValues(stats.likelihood());
msg->rawLikelihoodKeys = uKeys(stat.rawLikelihood()); msg->rawLikelihoodKeys = uKeys(stats.rawLikelihood());
msg->rawLikelihoodValues = uValues(stat.rawLikelihood()); msg->rawLikelihoodValues = uValues(stats.rawLikelihood());
msg->weightsKeys = uKeys(stat.weights()); msg->weightsKeys = uKeys(stats.weights());
msg->weightsValues = uValues(stat.weights()); msg->weightsValues = uValues(stats.weights());
//Features stuff... //Features stuff...
msg->refWordsKeys = uKeys(stat.refWords()); msg->refWordsKeys = uKeys(stats.refWords());
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size()); msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stats.refWords().size());
int index = 0; int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin(); for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.refWords().begin();
i!=stat.refWords().end(); i!=stats.refWords().end();
++i) ++i)
{ {
msg->refWordsValues.at(index).angle = i->second.angle; msg->refWordsValues.at(index).angle = i->second.angle;
@@ -246,11 +241,11 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
++index; ++index;
} }
msg->loopWordsKeys = uKeys(stat.loopWords()); msg->loopWordsKeys = uKeys(stats.loopWords());
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size()); msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stats.loopWords().size());
index = 0; index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin(); for(std::multimap<int, cv::KeyPoint>::const_iterator i=stats.loopWords().begin();
i!=stat.loopWords().end(); i!=stats.loopWords().end();
++i) ++i)
{ {
msg->loopWordsValues.at(index).angle = i->second.angle; msg->loopWordsValues.at(index).angle = i->second.angle;
@@ -264,11 +259,10 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
} }
// Statistics data // Statistics data
msg->statsKeys = uKeys(stat.data()); msg->statsKeys = uKeys(stats.data());
msg->statsValues = uValues(stat.data()); msg->statsValues = uValues(stats.data());
} }
infoPubEx_.publish(msg); infoPubEx_.publish(msg);
} }
} }
} }
}
rtabmap/src/CoreWrapper.h
+3 -4
View File
@@ -13,8 +13,7 @@
#include <std_srvs/Empty.h> #include <std_srvs/Empty.h>
#include <std_msgs/Empty.h> #include <std_msgs/Empty.h>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include "utilite/UEventsHandler.h" #include <rtabmap/core/Statistics.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
@@ -24,7 +23,7 @@ namespace rtabmap
class Rtabmap; class Rtabmap;
} }
class CoreWrapper : public UEventsHandler class CoreWrapper
{ {
public: public:
CoreWrapper(bool deleteDbOnStart = false); CoreWrapper(bool deleteDbOnStart = false);
@@ -43,7 +42,7 @@ private:
void loadNodeParameters(const std::string & configFile); void loadNodeParameters(const std::string & configFile);
void saveNodeParameters(const std::string & configFile); void saveNodeParameters(const std::string & configFile);
virtual void handleEvent(UEvent * anEvent); void publishStats(const rtabmap::Statistics & stats);
private: private:
rtabmap::Rtabmap * rtabmap_; rtabmap::Rtabmap * rtabmap_;