mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
Simplified topics between ros nodes
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@461 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
@@ -1,8 +1,10 @@
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
########################################
|
||||
# If a loop is found with the current image ("refId"),
|
||||
# "loopClosureId" is not null. Field "actuators" contains
|
||||
# (when actions are used) actions executed after the
|
||||
# "loopClosureId".
|
||||
########################################
|
||||
|
||||
Header header
|
||||
|
||||
@@ -10,4 +12,6 @@ int32 refId
|
||||
int32 loopClosureId
|
||||
|
||||
uint32 actuatorStep
|
||||
float32[] actuators
|
||||
float32[] actuators
|
||||
|
||||
rtabmap/RtabmapInfoEx infoEx
|
||||
@@ -1,13 +1,12 @@
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
#
|
||||
|
||||
########################################
|
||||
# Statistics stuff:
|
||||
# These fields are empty if RTAb-Map's
|
||||
# parameter publishStats=false
|
||||
########################################
|
||||
|
||||
Header header
|
||||
|
||||
rtabmap/RtabmapInfo info
|
||||
|
||||
sensor_msgs/CompressedImage refImage
|
||||
int32 refChild
|
||||
|
||||
@@ -40,4 +39,5 @@ rtabmap/KeyPoint[] loopWordsValues
|
||||
|
||||
##SM masks##
|
||||
uint8[] refMotionMask
|
||||
uint8[] loopMotionMask
|
||||
uint8[] loopMotionMask
|
||||
|
||||
|
||||
@@ -76,9 +76,7 @@ bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Re
|
||||
{
|
||||
ros::NodeHandle nh("~");
|
||||
nh.setParam("image_hz", request.imgRate);
|
||||
nh.setParam("auto_restart", request.autoRestart);
|
||||
camera_->setImageRate(request.imgRate);
|
||||
camera_->setAutoRestart(request.autoRestart);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
+59
-79
@@ -26,7 +26,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
{
|
||||
ros::NodeHandle nh("~");
|
||||
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
|
||||
infoExPub_ = nh.advertise<rtabmap::RtabmapInfoEx>("info_x", 1);
|
||||
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
|
||||
|
||||
rtabmap_ = new Rtabmap();
|
||||
@@ -46,9 +45,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
|
||||
nh = ros::NodeHandle();
|
||||
smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this);
|
||||
imageTopic_ = nh.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
|
||||
parametersUpdatedTopic_ = nh.subscribe("rtabmap_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
|
||||
|
||||
image_transport::ImageTransport it(nh);
|
||||
imageTopic_ = it.subscribe("image", 1, &CoreWrapper::imageReceivedCallback, this);
|
||||
|
||||
UEventsManager::addHandler(this);
|
||||
}
|
||||
|
||||
@@ -200,8 +201,8 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
||||
|
||||
if(stat.extended())
|
||||
{
|
||||
rtabmap::RtabmapInfoExPtr msg(new rtabmap::RtabmapInfoEx);
|
||||
msg->info.refId = stat.refImageId();
|
||||
rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
|
||||
msg->refId = stat.refImageId();
|
||||
if(stat.refImage())
|
||||
{
|
||||
int params[3] = {0};
|
||||
@@ -229,9 +230,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
||||
memcpy(&compressed.data[0], buf->data.ptr, buf->width);
|
||||
cvReleaseMat(&buf);
|
||||
|
||||
msg->refImage = compressed;
|
||||
msg->infoEx.refImage = compressed;
|
||||
}
|
||||
msg->info.loopClosureId = stat.loopClosureId();
|
||||
msg->loopClosureId = stat.loopClosureId();
|
||||
if(stat.loopClosureImage())
|
||||
{
|
||||
int params[3] = {0};
|
||||
@@ -259,81 +260,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
||||
memcpy(&compressed.data[0], buf->data.ptr, buf->width);
|
||||
cvReleaseMat(&buf);
|
||||
|
||||
msg->loopClosureImage = compressed;
|
||||
msg->infoEx.loopClosureImage = compressed;
|
||||
}
|
||||
|
||||
const std::list<std::vector<float> > & actuators = stat.getActions();
|
||||
if(actuators.size())
|
||||
{
|
||||
msg->info.actuatorStep = actuators.front().size();
|
||||
}
|
||||
for(std::list<std::vector<float> >::const_iterator iter=actuators.begin();iter!=actuators.end();++iter)
|
||||
{
|
||||
if((iter->size() == 0 && msg->info.actuatorStep > 0) || msg->info.actuatorStep % iter->size() != 0)
|
||||
{
|
||||
ROS_ERROR("Actuators must have all the same length.");
|
||||
}
|
||||
msg->info.actuators.insert(msg->info.actuators.end(), iter->begin(), iter->end());
|
||||
}
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
msg->posteriorKeys = uKeys(stat.posterior());
|
||||
msg->posteriorValues = uValues(stat.posterior());
|
||||
msg->likelihoodKeys = uKeys(stat.likelihood());
|
||||
msg->likelihoodValues = uValues(stat.likelihood());
|
||||
msg->weightsKeys = uKeys(stat.weights());
|
||||
msg->weightsValues = uValues(stat.weights());
|
||||
|
||||
//SURF stuff...
|
||||
msg->refWordsKeys = uListToVector(uKeys(stat.refWords()));
|
||||
msg->refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
||||
int index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
|
||||
i!=stat.refWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->refWordsValues.at(index).angle = i->second.angle;
|
||||
msg->refWordsValues.at(index).response = i->second.response;
|
||||
msg->refWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->refWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->refWordsValues.at(index).size = i->second.size;
|
||||
msg->refWordsValues.at(index).octave = i->second.octave;
|
||||
msg->refWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
msg->loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
|
||||
msg->loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
||||
index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
|
||||
i!=stat.loopWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->loopWordsValues.at(index).angle = i->second.angle;
|
||||
msg->loopWordsValues.at(index).response = i->second.response;
|
||||
msg->loopWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->loopWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->loopWordsValues.at(index).size = i->second.size;
|
||||
msg->loopWordsValues.at(index).octave = i->second.octave;
|
||||
msg->loopWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
|
||||
// SM masks
|
||||
msg->refMotionMask = stat.refMotionMask();
|
||||
msg->loopMotionMask = stat.loopMotionMask();
|
||||
|
||||
// Statistics data
|
||||
msg->statsKeys = uKeys(stat.data());
|
||||
msg->statsValues = uValues(stat.data());
|
||||
|
||||
infoExPub_.publish(msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
|
||||
ROS_INFO("Loop closure detected! newId=%d with oldId=%d", stat.refImageId(), stat.loopClosureId());
|
||||
msg->refId = stat.refImageId();
|
||||
msg->loopClosureId = stat.loopClosureId();
|
||||
const std::list<std::vector<float> > & actuators = stat.getActions();
|
||||
if(actuators.size())
|
||||
{
|
||||
@@ -347,6 +276,57 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
|
||||
}
|
||||
msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end());
|
||||
}
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
msg->infoEx.posteriorKeys = uKeys(stat.posterior());
|
||||
msg->infoEx.posteriorValues = uValues(stat.posterior());
|
||||
msg->infoEx.likelihoodKeys = uKeys(stat.likelihood());
|
||||
msg->infoEx.likelihoodValues = uValues(stat.likelihood());
|
||||
msg->infoEx.weightsKeys = uKeys(stat.weights());
|
||||
msg->infoEx.weightsValues = uValues(stat.weights());
|
||||
|
||||
//SURF stuff...
|
||||
msg->infoEx.refWordsKeys = uListToVector(uKeys(stat.refWords()));
|
||||
msg->infoEx.refWordsValues = std::vector<rtabmap::KeyPoint>(stat.refWords().size());
|
||||
int index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.refWords().begin();
|
||||
i!=stat.refWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->infoEx.refWordsValues.at(index).angle = i->second.angle;
|
||||
msg->infoEx.refWordsValues.at(index).response = i->second.response;
|
||||
msg->infoEx.refWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->infoEx.refWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->infoEx.refWordsValues.at(index).size = i->second.size;
|
||||
msg->infoEx.refWordsValues.at(index).octave = i->second.octave;
|
||||
msg->infoEx.refWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
msg->infoEx.loopWordsKeys = uListToVector(uKeys(stat.loopWords()));
|
||||
msg->infoEx.loopWordsValues = std::vector<rtabmap::KeyPoint>(stat.loopWords().size());
|
||||
index = 0;
|
||||
for(std::multimap<int, cv::KeyPoint>::const_iterator i=stat.loopWords().begin();
|
||||
i!=stat.loopWords().end();
|
||||
++i)
|
||||
{
|
||||
msg->infoEx.loopWordsValues.at(index).angle = i->second.angle;
|
||||
msg->infoEx.loopWordsValues.at(index).response = i->second.response;
|
||||
msg->infoEx.loopWordsValues.at(index).ptx = i->second.pt.x;
|
||||
msg->infoEx.loopWordsValues.at(index).pty = i->second.pt.y;
|
||||
msg->infoEx.loopWordsValues.at(index).size = i->second.size;
|
||||
msg->infoEx.loopWordsValues.at(index).octave = i->second.octave;
|
||||
msg->infoEx.loopWordsValues.at(index).class_id = i->second.class_id;
|
||||
++index;
|
||||
}
|
||||
|
||||
// SM masks
|
||||
msg->infoEx.refMotionMask = stat.refMotionMask();
|
||||
msg->infoEx.loopMotionMask = stat.loopMotionMask();
|
||||
|
||||
// Statistics data
|
||||
msg->infoEx.statsKeys = uKeys(stat.data());
|
||||
msg->infoEx.statsValues = uValues(stat.data());
|
||||
|
||||
infoPub_.publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -17,6 +17,7 @@
|
||||
#include <rtabmap/core/RtabmapEvent.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include "rtabmap/SensoryMotorState.h"
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
@@ -49,10 +50,9 @@ private:
|
||||
private:
|
||||
rtabmap::Rtabmap * rtabmap_;
|
||||
ros::Subscriber smStateTopic_;
|
||||
ros::Subscriber imageTopic_;
|
||||
image_transport::Subscriber imageTopic_;
|
||||
ros::Subscriber parametersUpdatedTopic_;
|
||||
ros::Publisher infoPub_;
|
||||
ros::Publisher infoExPub_;
|
||||
ros::Publisher parametersLoadedPub_;
|
||||
std::string configFile_;
|
||||
|
||||
|
||||
+76
-97
@@ -27,7 +27,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this);
|
||||
infoExTopic_ = nh.subscribe("rtabmap/info_x", 1, &GuiWrapper::infoExReceivedCallback, this);
|
||||
velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
|
||||
app_ = new QApplication(argc, argv);
|
||||
mainWindow_ = new MainWindow(new PreferencesDialogROS());
|
||||
@@ -63,10 +62,83 @@ int GuiWrapper::exec()
|
||||
|
||||
void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||
{
|
||||
ROS_INFO("Loop closure detected! newId=%d with oldId=%d", msg->refId, msg->loopClosureId);
|
||||
ROS_INFO("RTAB-Map info received!");
|
||||
|
||||
// Map from ROS struct to rtabmap struct
|
||||
rtabmap::Statistics * stat = new rtabmap::Statistics();
|
||||
|
||||
stat->setExtended(true); // Extended
|
||||
|
||||
stat->setRefImageId(msg->refId);
|
||||
if(msg->infoEx.refImage.data.size() > 0)
|
||||
{
|
||||
// Decompress
|
||||
const CvMat compressed = cvMat(1, msg->infoEx.refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.refImage.data[0]));
|
||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
||||
stat->setRefImage(&decompressed);
|
||||
}
|
||||
stat->setLoopClosureId(msg->loopClosureId);
|
||||
if(msg->infoEx.loopClosureImage.data.size() > 0)
|
||||
{
|
||||
// Decompress
|
||||
const CvMat compressed = cvMat(1, msg->infoEx.loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->infoEx.loopClosureImage.data[0]));
|
||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
||||
stat->setLoopClosureImage(&decompressed);
|
||||
}
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
std::map<int, float> mapIntFloat;
|
||||
for(unsigned int i=0; i<msg->infoEx.posteriorKeys.size() && i<msg->infoEx.posteriorValues.size(); ++i)
|
||||
{
|
||||
mapIntFloat.insert(std::pair<int, float>(msg->infoEx.posteriorKeys.at(i), msg->infoEx.posteriorValues.at(i)));
|
||||
}
|
||||
stat->setPosterior(mapIntFloat);
|
||||
mapIntFloat.clear();
|
||||
for(unsigned int i=0; i<msg->infoEx.likelihoodKeys.size() && i<msg->infoEx.likelihoodValues.size(); ++i)
|
||||
{
|
||||
mapIntFloat.insert(std::pair<int, float>(msg->infoEx.likelihoodKeys.at(i), msg->infoEx.likelihoodValues.at(i)));
|
||||
}
|
||||
stat->setLikelihood(mapIntFloat);
|
||||
std::map<int, int> mapIntInt;
|
||||
for(unsigned int i=0; i<msg->infoEx.weightsKeys.size() && i<msg->infoEx.weightsValues.size(); ++i)
|
||||
{
|
||||
mapIntInt.insert(std::pair<int, int>(msg->infoEx.weightsKeys.at(i), msg->infoEx.weightsValues.at(i)));
|
||||
}
|
||||
stat->setWeights(mapIntInt);
|
||||
|
||||
//SURF stuff...
|
||||
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
|
||||
for(unsigned int i=0; i<msg->infoEx.refWordsKeys.size() && i<msg->infoEx.refWordsValues.size(); i++)
|
||||
{
|
||||
cv::KeyPoint pt;
|
||||
pt.angle = msg->infoEx.refWordsValues.at(i).angle;
|
||||
pt.response = msg->infoEx.refWordsValues.at(i).response;
|
||||
//pt.laplacian = msg->refWordsValues.at(i).laplacian;
|
||||
pt.pt.x = msg->infoEx.refWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->infoEx.refWordsValues.at(i).pty;
|
||||
pt.size = msg->infoEx.refWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.refWordsKeys.at(i), pt));
|
||||
}
|
||||
stat->setRefWords(mapIntKeypoint);
|
||||
mapIntKeypoint.clear();
|
||||
for(unsigned int i=0; i<msg->infoEx.loopWordsKeys.size() && i<msg->infoEx.loopWordsValues.size(); i++)
|
||||
{
|
||||
cv::KeyPoint pt;
|
||||
pt.angle = msg->infoEx.loopWordsValues.at(i).angle;
|
||||
pt.response = msg->infoEx.loopWordsValues.at(i).response;
|
||||
//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
|
||||
pt.pt.x = msg->infoEx.loopWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->infoEx.loopWordsValues.at(i).pty;
|
||||
pt.size = msg->infoEx.loopWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->infoEx.loopWordsKeys.at(i), pt));
|
||||
}
|
||||
stat->setLoopWords(mapIntKeypoint);
|
||||
|
||||
//SM stuff
|
||||
stat->setRefMotionMask(msg->infoEx.refMotionMask);
|
||||
stat->setLoopMotionMask(msg->infoEx.loopMotionMask);
|
||||
|
||||
//Actions
|
||||
std::list<std::vector<float> > actions;
|
||||
for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep)
|
||||
{
|
||||
@@ -78,104 +150,11 @@ void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||
actions.push_back(a);
|
||||
}
|
||||
stat->setActions(actions);
|
||||
UEventsManager::post(new rtabmap::RtabmapEvent(&stat));
|
||||
}
|
||||
|
||||
void GuiWrapper::infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
|
||||
{
|
||||
ROS_INFO("Statistics received!");
|
||||
|
||||
// Map from ROS struct to rtabmap struct
|
||||
rtabmap::Statistics * stat = new rtabmap::Statistics();
|
||||
|
||||
stat->setExtended(true); // Extended
|
||||
|
||||
stat->setRefImageId(msg->info.refId);
|
||||
if(msg->refImage.data.size() > 0)
|
||||
{
|
||||
// Decompress
|
||||
const CvMat compressed = cvMat(1, msg->refImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->refImage.data[0]));
|
||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
||||
stat->setRefImage(&decompressed);
|
||||
}
|
||||
stat->setLoopClosureId(msg->info.loopClosureId);
|
||||
if(msg->loopClosureImage.data.size() > 0)
|
||||
{
|
||||
// Decompress
|
||||
const CvMat compressed = cvMat(1, msg->loopClosureImage.data.size(), CV_8UC1, const_cast<unsigned char*>(&msg->loopClosureImage.data[0]));
|
||||
IplImage * decompressed = cvDecodeImage(&compressed, CV_LOAD_IMAGE_ANYCOLOR);
|
||||
stat->setLoopClosureImage(&decompressed);
|
||||
}
|
||||
|
||||
//Posterior, likelihood, childCount
|
||||
std::map<int, float> mapIntFloat;
|
||||
for(unsigned int i=0; i<msg->posteriorKeys.size() && i<msg->posteriorValues.size(); ++i)
|
||||
{
|
||||
mapIntFloat.insert(std::pair<int, float>(msg->posteriorKeys.at(i), msg->posteriorValues.at(i)));
|
||||
}
|
||||
stat->setPosterior(mapIntFloat);
|
||||
mapIntFloat.clear();
|
||||
for(unsigned int i=0; i<msg->likelihoodKeys.size() && i<msg->likelihoodValues.size(); ++i)
|
||||
{
|
||||
mapIntFloat.insert(std::pair<int, float>(msg->likelihoodKeys.at(i), msg->likelihoodValues.at(i)));
|
||||
}
|
||||
stat->setLikelihood(mapIntFloat);
|
||||
std::map<int, int> mapIntInt;
|
||||
for(unsigned int i=0; i<msg->weightsKeys.size() && i<msg->weightsValues.size(); ++i)
|
||||
{
|
||||
mapIntInt.insert(std::pair<int, int>(msg->weightsKeys.at(i), msg->weightsValues.at(i)));
|
||||
}
|
||||
stat->setWeights(mapIntInt);
|
||||
|
||||
//SURF stuff...
|
||||
std::multimap<int, cv::KeyPoint> mapIntKeypoint;
|
||||
for(unsigned int i=0; i<msg->refWordsKeys.size() && i<msg->refWordsValues.size(); i++)
|
||||
{
|
||||
cv::KeyPoint pt;
|
||||
pt.angle = msg->refWordsValues.at(i).angle;
|
||||
pt.response = msg->refWordsValues.at(i).response;
|
||||
//pt.laplacian = msg->refWordsValues.at(i).laplacian;
|
||||
pt.pt.x = msg->refWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->refWordsValues.at(i).pty;
|
||||
pt.size = msg->refWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->refWordsKeys.at(i), pt));
|
||||
}
|
||||
stat->setRefWords(mapIntKeypoint);
|
||||
mapIntKeypoint.clear();
|
||||
for(unsigned int i=0; i<msg->loopWordsKeys.size() && i<msg->loopWordsValues.size(); i++)
|
||||
{
|
||||
cv::KeyPoint pt;
|
||||
pt.angle = msg->loopWordsValues.at(i).angle;
|
||||
pt.response = msg->loopWordsValues.at(i).response;
|
||||
//pt.laplacian = msg->loopWordsValues.at(i).laplacian;
|
||||
pt.pt.x = msg->loopWordsValues.at(i).ptx;
|
||||
pt.pt.y = msg->loopWordsValues.at(i).pty;
|
||||
pt.size = msg->loopWordsValues.at(i).size;
|
||||
mapIntKeypoint.insert(std::pair<int, cv::KeyPoint>(msg->loopWordsKeys.at(i), pt));
|
||||
}
|
||||
stat->setLoopWords(mapIntKeypoint);
|
||||
|
||||
//SM stuff
|
||||
stat->setRefMotionMask(msg->refMotionMask);
|
||||
stat->setLoopMotionMask(msg->loopMotionMask);
|
||||
|
||||
//Actions
|
||||
std::list<std::vector<float> > actions;
|
||||
for(unsigned int i=0; i<msg->info.actuators.size(); i+=msg->info.actuatorStep)
|
||||
{
|
||||
std::vector<float> a(msg->info.actuatorStep);
|
||||
for(unsigned int j=0; j<a.size(); ++j)
|
||||
{
|
||||
a[j] = msg->info.actuators[i+j];
|
||||
}
|
||||
actions.push_back(a);
|
||||
}
|
||||
stat->setActions(actions);
|
||||
|
||||
// Statistics data
|
||||
for(unsigned int i=0; i<msg->statsKeys.size() && i<msg->statsValues.size(); i++)
|
||||
for(unsigned int i=0; i<msg->infoEx.statsKeys.size() && i<msg->infoEx.statsValues.size(); i++)
|
||||
{
|
||||
stat->addStatistic(msg->statsKeys.at(i), msg->statsValues.at(i));
|
||||
stat->addStatistic(msg->infoEx.statsKeys.at(i), msg->infoEx.statsValues.at(i));
|
||||
}
|
||||
|
||||
ROS_INFO("Publishing statistics...");
|
||||
|
||||
@@ -34,12 +34,10 @@ protected:
|
||||
|
||||
private:
|
||||
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
|
||||
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
|
||||
void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
|
||||
|
||||
private:
|
||||
ros::Subscriber infoTopic_;
|
||||
ros::Subscriber infoExTopic_;
|
||||
ros::Subscriber velocity_sub_;
|
||||
QApplication * app_;
|
||||
rtabmap::MainWindow * mainWindow_;
|
||||
|
||||
@@ -42,11 +42,6 @@ void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
|
||||
publishCommands(msg->actuators, msg->actuatorStep);
|
||||
}
|
||||
|
||||
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
|
||||
{
|
||||
publishCommands(msg->info.actuators, msg->info.actuatorStep);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "rtabmap_out");
|
||||
@@ -70,7 +65,6 @@ int main(int argc, char** argv)
|
||||
ros::Subscriber infoTopic;
|
||||
ros::Subscriber infoExTopic;
|
||||
infoTopic = nh.subscribe("rtabmap/info", 1, infoReceivedCallback);
|
||||
infoExTopic = nh.subscribe("rtabmap/info_x", 1, infoExReceivedCallback);
|
||||
rosPublisher = nh.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
|
||||
|
||||
ros::spin();
|
||||
|
||||
@@ -1,3 +1,2 @@
|
||||
float32 imgRate
|
||||
bool autoRestart
|
||||
---
|
||||
Reference in New Issue
Block a user