41 #include <sensor_msgs/PointCloud2.h> 48 #include <pcl/io/io.h> 49 #include <pcl/io/pcd_io.h> 50 #include <pcl/point_types.h> 54 #include <Eigen/Geometry> 86 cloud_cb (
const pcl::PCLPointCloud2::ConstPtr& cloud)
88 if ((cloud->width * cloud->height) == 0)
91 ROS_INFO (
"Received %d data points in frame %s with the following fields: %s",
92 (
int)cloud->width * cloud->height,
93 cloud->header.frame_id.c_str (),
96 Eigen::Vector4f v = Eigen::Vector4f::Zero ();
97 Eigen::Quaternionf q = Eigen::Quaternionf::Identity ();
98 if (!fixed_frame_.empty ()) {
100 ROS_WARN(
"Could not get transform!");
104 Eigen::Affine3d transform;
106 v = Eigen::Vector4f::Zero ();
107 v.head<3> () = transform.translation ().cast<
float> ();
108 q = transform.rotation ().cast<
float> ();
111 std::stringstream ss;
112 ss << prefix_ << cloud->header.stamp <<
".pcd";
113 ROS_INFO (
"Data saved to %s", ss.str ().c_str ());
120 writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
124 writer.writeBinary (ss.str (), *cloud, v, q);
129 writer.writeASCII (ss.str (), *cloud, v, q, 8);
139 if (priv_nh.
getParam (
"prefix", prefix_))
143 else if (nh_.
getParam (
"prefix", prefix_))
149 priv_nh.
getParam (
"fixed_frame", fixed_frame_);
150 priv_nh.
getParam (
"binary", binary_);
151 priv_nh.
getParam (
"compressed", compressed_);
168 cloud_topic_ =
"input";
171 ROS_INFO (
"Listening for incoming data on topic %s",
void cloud_cb(const pcl::PCLPointCloud2::ConstPtr &cloud)
pcl_ros::PCDWriter PCDWriter
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
tf2_ros::Buffer tf_buffer_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string resolveName(const std::string &name, bool remap=true) const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
int main(int argc, char **argv)
#define ROS_WARN_STREAM(args)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
tf2_ros::TransformListener tf_listener_
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)