i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-14 07:10: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 pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
<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"/>
<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/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <stereo_msgs/DisparityImage.h>
@@ -46,13 +47,17 @@ class DisparityOdomSyncNodelet : public nodelet::Nodelet
public:
//Constructor
DisparityOdomSyncNodelet():
sync_(0)
approxSync_(0),
exactSync_(0)
{
}
virtual ~DisparityOdomSyncNodelet()
{
delete sync_;
if(approxSync_)
delete approxSync_;
if(exactSync_)
delete exactSync_;
}
private:
@@ -62,18 +67,29 @@ private:
ros::NodeHandle& private_nh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = false;
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_);
sync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
if(approxSync)
{
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);
infoSub_.subscribe(nh, "camera_info", 1);
odomSub_.subscribe(nh, "odom", 1);
disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 10);
infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 10);
odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 10);
disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 1);
infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 1);
odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 1);
};
void callback(const stereo_msgs::DisparityImageConstPtr& image,
@@ -101,8 +117,10 @@ private:
message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MyApproxSyncPolicy;
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_;
};