apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add fixed_frame to pointcloud_to_pcd

Browse Source
This commit is contained in:
Jochen Sprickerhof 2015-08-18 17:01:24 +02:00 committed by Paul Bovbel
parent 2aa0495516
commit aa370cefc8
1 changed files with 29 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
pcl_ros/tools/pointcloud_to_pcd.cpp
View File

@ -40,6 +40,10 @@
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>
// PCL includes // PCL includes
#include <pcl/io/io.h> #include <pcl/io/io.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
@ -47,6 +51,8 @@
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Geometry>
using namespace std; using namespace std;
/** /**
@ -65,6 +71,9 @@ class PointCloudToPCD
std::string prefix_; std::string prefix_;
bool binary_; bool binary_;
bool compressed_; bool compressed_;
std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
public: public:
string cloud_topic_; string cloud_topic_;
@ -84,6 +93,21 @@ class PointCloudToPCD
cloud->header.frame_id.c_str (), cloud->header.frame_id.c_str (),
pcl::getFieldsList (*cloud).c_str ()); pcl::getFieldsList (*cloud).c_str ());
Eigen::Vector4f v = Eigen::Vector4f::Zero ();
Eigen::Quaternionf q = Eigen::Quaternionf::Identity ();
if (!fixed_frame_.empty ()) {
if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) {
ROS_WARN("Could not get transform!");
return;
}
Eigen::Affine3d transform;
transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp)));
v = Eigen::Vector4f::Zero ();
v.head<3> () = transform.translation ().cast<float> ();
q = transform.rotation ().cast<float> ();
}
std::stringstream ss; std::stringstream ss;
ss << prefix_ << cloud->header.stamp << ".pcd"; ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO ("Data saved to %s", ss.str ().c_str ()); ROS_INFO ("Data saved to %s", ss.str ().c_str ());
@ -93,25 +117,22 @@ class PointCloudToPCD
{ {
if(compressed_) if(compressed_)
{ {
writer.writeBinaryCompressed (ss.str (), *cloud, Eigen::Vector4f::Zero (), writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
Eigen::Quaternionf::Identity ());
} }
else else
{ {
writer.writeBinary (ss.str (), *cloud, Eigen::Vector4f::Zero (), writer.writeBinary (ss.str (), *cloud, v, q);
Eigen::Quaternionf::Identity ());
} }
} }
else else
{ {
writer.writeASCII (ss.str (), *cloud, Eigen::Vector4f::Zero (), writer.writeASCII (ss.str (), *cloud, v, q, 8);
Eigen::Quaternionf::Identity (), 8);
} }
} }
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
PointCloudToPCD () : binary_(false), compressed_(false) PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_)
{ {
// Check if a prefix parameter is defined for output file names. // Check if a prefix parameter is defined for output file names.
ros::NodeHandle priv_nh("~"); ros::NodeHandle priv_nh("~");
@ -125,6 +146,7 @@ class PointCloudToPCD
<< prefix_); << prefix_);
} }
priv_nh.getParam ("fixed_frame", fixed_frame_);
priv_nh.getParam ("binary", binary_); priv_nh.getParam ("binary", binary_);
priv_nh.getParam ("compressed", compressed_); priv_nh.getParam ("compressed", compressed_);
if(binary_) if(binary_)