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

Flush odometry's synchronizer queues on reset

Browse Source
This commit is contained in:
matlabbe
2016-06-10 14:17:51 -04:00
parent cc0591c60a
commit 99eb696e36
4 changed files with 63 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+2
View File
@@ -543,6 +543,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
odometry_->reset();
this->flushCallbacks();
return true;
}
@@ -551,6 +552,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
this->flushCallbacks();
return true;
}
src/OdometryROS.h
+4 -1
View File
@@ -53,7 +53,7 @@ public:
public:
OdometryROS(int argc, char * argv[], bool stereo = false);
~OdometryROS();
virtual ~OdometryROS();
void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -73,6 +73,9 @@ public:
bool isOdometryF2M() const;
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
protected:
virtual void flushCallbacks() = 0;
private:
rtabmap::Odometry * odometry_;
src/RGBDOdometryNode.cpp
+32 -6
View File
@@ -54,14 +54,14 @@ public:
RGBDOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv),
sync_(0),
sync2_(0)
sync2_(0),
queueSize_(5)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
int queueSize = 5;
int depthCameras = 1;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
{
@@ -110,7 +110,7 @@ public:
info2_sub_.getTopic().c_str());
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize),
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
@@ -140,12 +140,12 @@ public:
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
}
~RGBDOdometry()
virtual ~RGBDOdometry()
{
if(sync_)
{
@@ -328,6 +328,31 @@ public:
}
}
protected:
virtual void flushCallbacks()
{
// flush callbacks
if(sync_)
{
delete sync_;
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(sync2_)
{
delete sync2_;
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
}
private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
@@ -339,6 +364,7 @@ private:
message_filters::Synchronizer<MySyncPolicy> * sync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_;
int queueSize_;
};
int main(int argc, char *argv[])
src/StereoOdometryNode.cpp
+25 -6
View File
@@ -54,16 +54,16 @@ public:
StereoOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv, true),
approxSync_(0),
exactSync_(0)
exactSync_(0),
queueSize_(5)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
bool approxSync = false;
int queueSize = 5;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("queue_size", queueSize_, queueSize_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
ros::NodeHandle left_nh(nh, "left");
@@ -89,17 +89,17 @@ public:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
~StereoOdometry()
virtual ~StereoOdometry()
{
if(approxSync_)
{
@@ -188,6 +188,24 @@ public:
}
}
protected:
virtual void flushCallbacks()
{
//flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
private:
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
@@ -197,6 +215,7 @@ private:
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int queueSize_;
};
int main(int argc, char *argv[])