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

Simplified topics between ros nodes

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@461 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-03-03 04:44:40 +00:00
parent 3b8445a865
commit 04962c1a35
9 changed files with 155 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap/msg/RtabmapInfo.msg
+10 -6
View File
@@ -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 Header header
@@ -10,4 +12,6 @@ int32 refId
int32 loopClosureId int32 loopClosureId
uint32 actuatorStep uint32 actuatorStep
float32[] actuators float32[] actuators
rtabmap/RtabmapInfoEx infoEx
rtabmap/msg/RtabmapInfoEx.msg
+8 -8
View File
@@ -1,13 +1,12 @@
#
# ########################################
# # Statistics stuff:
# # These fields are empty if RTAb-Map's
# # parameter publishStats=false
########################################
Header header Header header
rtabmap/RtabmapInfo info
sensor_msgs/CompressedImage refImage sensor_msgs/CompressedImage refImage
int32 refChild int32 refChild
@@ -40,4 +39,5 @@ rtabmap/KeyPoint[] loopWordsValues
##SM masks## ##SM masks##
uint8[] refMotionMask uint8[] refMotionMask
uint8[] loopMotionMask uint8[] loopMotionMask
rtabmap/src/CameraWrapper.cpp
-2
View File
@@ -76,9 +76,7 @@ bool CameraWrapper::changeCameraImgRateCallback(rtabmap::ChangeCameraImgRate::Re
{ {
ros::NodeHandle nh("~"); ros::NodeHandle nh("~");
nh.setParam("image_hz", request.imgRate); nh.setParam("image_hz", request.imgRate);
nh.setParam("auto_restart", request.autoRestart);
camera_->setImageRate(request.imgRate); camera_->setImageRate(request.imgRate);
camera_->setAutoRestart(request.autoRestart);
return true; return true;
} }
rtabmap/src/CoreWrapper.cpp
+59 -79
View File
@@ -26,7 +26,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
{ {
ros::NodeHandle nh("~"); ros::NodeHandle nh("~");
infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1); infoPub_ = nh.advertise<rtabmap::RtabmapInfo>("info", 1);
infoExPub_ = nh.advertise<rtabmap::RtabmapInfoEx>("info_x", 1);
parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1); parametersLoadedPub_ = nh.advertise<std_msgs::Empty>("parameters_loaded", 1);
rtabmap_ = new Rtabmap(); rtabmap_ = new Rtabmap();
@@ -46,9 +45,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
nh = ros::NodeHandle(); nh = ros::NodeHandle();
smStateTopic_ = nh.subscribe("sm_state", 1, &CoreWrapper::smReceivedCallback, this); 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); 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); UEventsManager::addHandler(this);
} }
@@ -200,8 +201,8 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
if(stat.extended()) if(stat.extended())
{ {
rtabmap::RtabmapInfoExPtr msg(new rtabmap::RtabmapInfoEx); rtabmap::RtabmapInfoPtr msg(new rtabmap::RtabmapInfo);
msg->info.refId = stat.refImageId(); msg->refId = stat.refImageId();
if(stat.refImage()) if(stat.refImage())
{ {
int params[3] = {0}; int params[3] = {0};
@@ -229,9 +230,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
memcpy(&compressed.data[0], buf->data.ptr, buf->width); memcpy(&compressed.data[0], buf->data.ptr, buf->width);
cvReleaseMat(&buf); cvReleaseMat(&buf);
msg->refImage = compressed; msg->infoEx.refImage = compressed;
} }
msg->info.loopClosureId = stat.loopClosureId(); msg->loopClosureId = stat.loopClosureId();
if(stat.loopClosureImage()) if(stat.loopClosureImage())
{ {
int params[3] = {0}; int params[3] = {0};
@@ -259,81 +260,9 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
memcpy(&compressed.data[0], buf->data.ptr, buf->width); memcpy(&compressed.data[0], buf->data.ptr, buf->width);
cvReleaseMat(&buf); 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(); const std::list<std::vector<float> > & actuators = stat.getActions();
if(actuators.size()) if(actuators.size())
{ {
@@ -347,6 +276,57 @@ void CoreWrapper::handleEvent(UEvent * anEvent)
} }
msg->actuators.insert(msg->actuators.end(), iter->begin(), iter->end()); 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); infoPub_.publish(msg);
} }
} }
rtabmap/src/CoreWrapper.h
+2 -2
View File
@@ -17,6 +17,7 @@
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include "rtabmap/SensoryMotorState.h" #include "rtabmap/SensoryMotorState.h"
#include <image_transport/image_transport.h>
namespace rtabmap namespace rtabmap
{ {
@@ -49,10 +50,9 @@ private:
private: private:
rtabmap::Rtabmap * rtabmap_; rtabmap::Rtabmap * rtabmap_;
ros::Subscriber smStateTopic_; ros::Subscriber smStateTopic_;
ros::Subscriber imageTopic_; image_transport::Subscriber imageTopic_;
ros::Subscriber parametersUpdatedTopic_; ros::Subscriber parametersUpdatedTopic_;
ros::Publisher infoPub_; ros::Publisher infoPub_;
ros::Publisher infoExPub_;
ros::Publisher parametersLoadedPub_; ros::Publisher parametersLoadedPub_;
std::string configFile_; std::string configFile_;
rtabmap/src/GuiWrapper.cpp
+76 -97
View File
@@ -27,7 +27,6 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
infoTopic_ = nh.subscribe("rtabmap/info", 1, &GuiWrapper::infoReceivedCallback, this); 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); velocity_sub_ = nh.subscribe("cmd_vel", 1, &GuiWrapper::velocityReceivedCallback, this);
app_ = new QApplication(argc, argv); app_ = new QApplication(argc, argv);
mainWindow_ = new MainWindow(new PreferencesDialogROS()); mainWindow_ = new MainWindow(new PreferencesDialogROS());
@@ -63,10 +62,83 @@ int GuiWrapper::exec()
void GuiWrapper::infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg) 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(); rtabmap::Statistics * stat = new rtabmap::Statistics();
stat->setExtended(true); // Extended
stat->setRefImageId(msg->refId); 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); 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; std::list<std::vector<float> > actions;
for(unsigned int i=0; i<msg->actuators.size(); i+=msg->actuatorStep) 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); actions.push_back(a);
} }
stat->setActions(actions); 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 // 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..."); ROS_INFO("Publishing statistics...");
rtabmap/src/GuiWrapper.h
-2
View File
@@ -34,12 +34,10 @@ protected:
private: private:
void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg); void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & infoMsg);
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & infoExMsg);
void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg); void velocityReceivedCallback(const geometry_msgs::TwistConstPtr & msg);
private: private:
ros::Subscriber infoTopic_; ros::Subscriber infoTopic_;
ros::Subscriber infoExTopic_;
ros::Subscriber velocity_sub_; ros::Subscriber velocity_sub_;
QApplication * app_; QApplication * app_;
rtabmap::MainWindow * mainWindow_; rtabmap::MainWindow * mainWindow_;
rtabmap/src/OutputNode.cpp
-6
View File
@@ -42,11 +42,6 @@ void infoReceivedCallback(const rtabmap::RtabmapInfoConstPtr & msg)
publishCommands(msg->actuators, msg->actuatorStep); publishCommands(msg->actuators, msg->actuatorStep);
} }
void infoExReceivedCallback(const rtabmap::RtabmapInfoExConstPtr & msg)
{
publishCommands(msg->info.actuators, msg->info.actuatorStep);
}
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "rtabmap_out"); ros::init(argc, argv, "rtabmap_out");
@@ -70,7 +65,6 @@ int main(int argc, char** argv)
ros::Subscriber infoTopic; ros::Subscriber infoTopic;
ros::Subscriber infoExTopic; ros::Subscriber infoExTopic;
infoTopic = nh.subscribe("rtabmap/info", 1, infoReceivedCallback); 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); rosPublisher = nh.advertise<geometry_msgs::Twist>("rtabmap/cmd_vel", 1);
ros::spin(); ros::spin();
rtabmap/srv/ChangeCameraImgRate.srv
-1
View File
@@ -1,3 +1,2 @@
float32 imgRate float32 imgRate
bool autoRestart
--- ---