transform.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
00003  *  Copyright (C) 2011 Jesse Vera
00004  *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id$
00008  */
00009 
00020 #include "transform.h"
00021 
00022 namespace velodyne_pointcloud
00023 {
00025   Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh):
00026     data_(new velodyne_rawdata::RawData())
00027   {
00028     private_nh.param("frame_id", config_.frame_id, std::string("odom"));
00029     std::string tf_prefix = tf::getPrefixParam(private_nh);
00030     config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
00031     ROS_INFO_STREAM("target frame ID: " << config_.frame_id);
00032 
00033     data_->setup(private_nh);
00034 
00035     // advertise output point cloud (before subscribing to input data)
00036     output_ =
00037       node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
00038 
00039     // subscribe to VelodyneScan packets using transform filter
00040     velodyne_scan_.subscribe(node, "velodyne_packets", 10);
00041     tf_filter_ =
00042       new tf::MessageFilter<velodyne_msgs::VelodyneScan>(velodyne_scan_,
00043                                                          listener_,
00044                                                          config_.frame_id, 10);
00045     tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1));
00046   }
00047 
00053   void
00054     Transform::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg)
00055   {
00056     if (output_.getNumSubscribers() == 0)         // no one listening?
00057       return;                                     // avoid much work
00058 
00059     // allocate an output point cloud with same time as raw data
00060     VPointCloud::Ptr outMsg(new VPointCloud());
00061     outMsg->header.stamp = scanMsg->header.stamp;
00062     outMsg->header.frame_id = config_.frame_id;
00063     outMsg->height = 1;
00064 
00065     // process each packet provided by the driver
00066     for (size_t next = 0; next < scanMsg->packets.size(); ++next)
00067       {
00068         // clear input point cloud to handle this packet
00069         inPc_.points.clear();
00070         inPc_.width = 0;
00071         inPc_.height = 1;
00072         inPc_.header.frame_id = scanMsg->header.frame_id;
00073         inPc_.header.stamp = scanMsg->packets[next].stamp;
00074 
00075         // unpack the raw data
00076         data_->unpack(scanMsg->packets[next], inPc_);
00077 
00078         // clear transform point cloud for this packet
00079         tfPc_.points.clear();           // is this needed?
00080         tfPc_.width = 0;
00081         tfPc_.height = 1;
00082         tfPc_.header.stamp = scanMsg->packets[next].stamp;
00083         tfPc_.header.frame_id = config_.frame_id;
00084 
00085         // transform the packet point cloud into the target frame
00086         try
00087           {
00088             ROS_DEBUG_STREAM("transforming from" << inPc_.header.frame_id
00089                              << " to " << config_.frame_id);
00090             pcl_ros::transformPointCloud(config_.frame_id, inPc_, tfPc_,
00091                                          listener_);
00092 #if 0       // use the latest transform available, should usually work fine
00093             pcl_ros::transformPointCloud(inPc_.header.frame_id,
00094                                          ros::Time(0), inPc_,
00095                                          config_.frame_id,
00096                                          tfPc_, listener_);
00097 #endif
00098           }
00099         catch (tf::TransformException ex)
00100           {
00101             // only log tf error once every 100 times
00102             ROS_WARN_THROTTLE(100, "%s", ex.what());
00103             continue;                   // skip this packet
00104           }
00105 
00106         // append transformed packet data to end of output message
00107         outMsg->points.insert(outMsg->points.end(),
00108                              tfPc_.points.begin(),
00109                              tfPc_.points.end());
00110         outMsg->width += tfPc_.points.size();
00111       }
00112 
00113     // publish the accumulated cloud message
00114     ROS_DEBUG_STREAM("Publishing " << outMsg->height * outMsg->width
00115                      << " Velodyne points, time: " << outMsg->header.stamp);
00116     output_.publish(outMsg);
00117   }
00118 
00119 } // namespace velodyne_pointcloud


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Fri Jan 3 2014 12:11:12