i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-14 15:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros-pkg: added exactTime policy to disparity_odom_sync nodelet

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1918 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-23 18:26:22 +00:00
parent 0c003c6cc2
commit 6b23e4ebc3
2 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_mapping_robot_stereo_nav.launch
+1 -1
View File
@@ -94,7 +94,7 @@
</node> </node>
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet"> <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
<remap from="disparity/image" to="disparity_sync"/> <remap from="disparity/image" to="disparity_sync"/>
<remap from="disparity/camera_info" to="camera_info_sync"/> <remap from="disparity/camera_info" to="right/camera_info_throttle_sync"/>
<remap from="cloud" to="/planner_cloud"/> <remap from="cloud" to="/planner_cloud"/>
<param name="voxel_size" type="double" value="0.05"/> <param name="voxel_size" type="double" value="0.05"/>
src/nodelets/disparity_odom_sync.cpp
+27 -9
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h> #include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <stereo_msgs/DisparityImage.h> #include <stereo_msgs/DisparityImage.h>
@@ -46,13 +47,17 @@ class DisparityOdomSyncNodelet : public nodelet::Nodelet
public: public:
//Constructor //Constructor
DisparityOdomSyncNodelet(): DisparityOdomSyncNodelet():
sync_(0) approxSync_(0),
exactSync_(0)
{ {
} }
virtual ~DisparityOdomSyncNodelet() virtual ~DisparityOdomSyncNodelet()
{ {
delete sync_; if(approxSync_)
delete approxSync_;
if(exactSync_)
delete exactSync_;
} }
private: private:
@@ -62,18 +67,29 @@ private:
ros::NodeHandle& private_nh = getPrivateNodeHandle(); ros::NodeHandle& private_nh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 10;
bool approxSync = false;
private_nh.param("queue_size", queueSize, queueSize); private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_); if(approxSync)
sync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3)); {
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_);
approxSync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_);
exactSync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
}
disparitySub_.subscribe(nh, "disparity", 1); disparitySub_.subscribe(nh, "disparity", 1);
infoSub_.subscribe(nh, "camera_info", 1); infoSub_.subscribe(nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1); odomSub_.subscribe(nh, "odom", 1);
disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 10); disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 1);
infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 10); infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 1);
odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 10); odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 1);
}; };
void callback(const stereo_msgs::DisparityImageConstPtr& image, void callback(const stereo_msgs::DisparityImageConstPtr& image,
@@ -101,8 +117,10 @@ private:
message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_; message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_; message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy; typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MyApproxSyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
}; };