migrating nodelets and tools from fuerte release to pcl_ros
This commit is contained in:
Julius Kammerl
committed by
Paul Bovbel
Paul Bovbel
parent
d5d9e3816a
commit
a3701bb3df
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
\author Radu Bogdan Rusu
|
||||
|
||||
@b bag_to_pcd is a simple node that reads in a BAG file and saves all the PointCloud messages to disk in PCD (Point
|
||||
Cloud Data) format.
|
||||
|
||||
**/
|
||||
|
||||
#include <sstream>
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <rosbag/bag.h>
|
||||
#include <rosbag/view.h>
|
||||
#include "pcl/io/io.h"
|
||||
#include "pcl/io/pcd_io.h"
|
||||
#include "pcl_ros/transforms.h"
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
|
||||
typedef sensor_msgs::PointCloud2 PointCloud;
|
||||
typedef PointCloud::Ptr PointCloudPtr;
|
||||
typedef PointCloud::ConstPtr PointCloudConstPtr;
|
||||
|
||||
/* ---[ */
|
||||
int
|
||||
main (int argc, char** argv)
|
||||
{
|
||||
ros::init (argc, argv, "bag_to_pcd");
|
||||
if (argc < 4)
|
||||
{
|
||||
std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory>" << std::endl;
|
||||
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds" << std::endl;
|
||||
return (-1);
|
||||
}
|
||||
|
||||
// TF
|
||||
tf::TransformListener tf_listener;
|
||||
tf::TransformBroadcaster tf_broadcaster;
|
||||
|
||||
rosbag::Bag bag;
|
||||
rosbag::View view;
|
||||
rosbag::View::iterator view_it;
|
||||
|
||||
try
|
||||
{
|
||||
bag.open (argv[1], rosbag::bagmode::Read);
|
||||
}
|
||||
catch (rosbag::BagException)
|
||||
{
|
||||
std::cerr << "Error opening file " << argv[1] << std::endl;
|
||||
return (-1);
|
||||
}
|
||||
|
||||
view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2"));
|
||||
view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
|
||||
view_it = view.begin ();
|
||||
|
||||
std::string output_dir = std::string (argv[3]);
|
||||
boost::filesystem::path outpath (output_dir);
|
||||
if (!boost::filesystem::exists (outpath))
|
||||
{
|
||||
if (!boost::filesystem::create_directories (outpath))
|
||||
{
|
||||
std::cerr << "Error creating directory " << output_dir << std::endl;
|
||||
return (-1);
|
||||
}
|
||||
std::cerr << "Creating directory " << output_dir << std::endl;
|
||||
}
|
||||
|
||||
// Add the PointCloud2 handler
|
||||
std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
|
||||
|
||||
PointCloud cloud_t;
|
||||
ros::Duration r (0.001);
|
||||
// Loop over the whole bag file
|
||||
while (view_it != view.end ())
|
||||
{
|
||||
// Handle TF messages first
|
||||
tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
|
||||
if (tf != NULL)
|
||||
{
|
||||
tf_broadcaster.sendTransform (tf->transforms);
|
||||
ros::spinOnce ();
|
||||
r.sleep ();
|
||||
}
|
||||
else
|
||||
{
|
||||
// Get the PointCloud2 message
|
||||
PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
|
||||
if (cloud == NULL)
|
||||
{
|
||||
++view_it;
|
||||
continue;
|
||||
}
|
||||
// Transform it
|
||||
pcl_ros::transformPointCloud ("/base_link", *cloud, cloud_t, tf_listener);
|
||||
|
||||
std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
|
||||
std::cerr << "Data saved to " << ss.str () << std::endl;
|
||||
pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
|
||||
Eigen::Quaternionf::Identity (), true);
|
||||
}
|
||||
// Increment the iterator
|
||||
++view_it;
|
||||
}
|
||||
|
||||
return (0);
|
||||
}
|
||||
/* ]--- */
|
||||
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
\author Ethan Rublee
|
||||
|
||||
@b convert a pcd to an image file
|
||||
run with:
|
||||
rosrun pcl convert_pcd_image cloud_00042.pcd
|
||||
It will publish a ros image message on /pcd/image
|
||||
View the image with:
|
||||
rosrun image_view image_view image:=/pcd/image
|
||||
**/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <pcl/io/io.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
/* ---[ */
|
||||
int
|
||||
main (int argc, char **argv)
|
||||
{
|
||||
ros::init (argc, argv, "image_publisher");
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher image_pub = nh.advertise <sensor_msgs::Image> ("output", 1);
|
||||
|
||||
if (argc != 2)
|
||||
{
|
||||
std::cout << "usage:\n" << argv[0] << " cloud.pcd" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
sensor_msgs::Image image;
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
pcl::io::loadPCDFile (std::string (argv[1]), cloud);
|
||||
|
||||
try
|
||||
{
|
||||
pcl::toROSMsg (cloud, image); //convert the cloud
|
||||
}
|
||||
catch (std::runtime_error e)
|
||||
{
|
||||
ROS_ERROR_STREAM("Error in converting cloud to image message: "
|
||||
<< e.what());
|
||||
return 1; //fail!
|
||||
}
|
||||
ros::Rate loop_rate (5);
|
||||
while (nh.ok ())
|
||||
{
|
||||
image_pub.publish (image);
|
||||
ros::spinOnce ();
|
||||
loop_rate.sleep ();
|
||||
}
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
/* ]--- */
|
||||
@@ -0,0 +1,97 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2010, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: surface_convex_hull.cpp 34612 2010-12-08 01:06:27Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
\author Ethan Rublee
|
||||
**/
|
||||
// ROS core
|
||||
#include <ros/ros.h>
|
||||
//Image message
|
||||
#include <sensor_msgs/Image.h>
|
||||
//pcl::toROSMsg
|
||||
#include <pcl/io/pcd_io.h>
|
||||
//stl stuff
|
||||
#include <string>
|
||||
|
||||
class PointCloudToImage
|
||||
{
|
||||
public:
|
||||
void
|
||||
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
|
||||
{
|
||||
if ((cloud->width * cloud->height) == 0)
|
||||
return; //return if the cloud is not dense!
|
||||
try
|
||||
{
|
||||
pcl::toROSMsg (*cloud, image_); //convert the cloud
|
||||
}
|
||||
catch (std::runtime_error e)
|
||||
{
|
||||
ROS_ERROR_STREAM("Error in converting cloud to image message: "
|
||||
<< e.what());
|
||||
}
|
||||
image_pub_.publish (image_); //publish our cloud image
|
||||
}
|
||||
PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
|
||||
{
|
||||
sub_ = nh_.subscribe (cloud_topic_, 30,
|
||||
&PointCloudToImage::cloud_cb, this);
|
||||
image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30);
|
||||
|
||||
//print some info about the node
|
||||
std::string r_ct = nh_.resolveName (cloud_topic_);
|
||||
std::string r_it = nh_.resolveName (image_topic_);
|
||||
ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct );
|
||||
ROS_INFO_STREAM("Publishing image on topic " << r_it );
|
||||
}
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
sensor_msgs::Image image_; //cache the image message
|
||||
std::string cloud_topic_; //default input
|
||||
std::string image_topic_; //default output
|
||||
ros::Subscriber sub_; //cloud subscriber
|
||||
ros::Publisher image_pub_; //image message publisher
|
||||
};
|
||||
|
||||
int
|
||||
main (int argc, char **argv)
|
||||
{
|
||||
ros::init (argc, argv, "convert_pointcloud_to_image");
|
||||
PointCloudToImage pci; //this loads up the node
|
||||
ros::spin (); //where she stops nobody knows
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
\author Radu Bogdan Rusu
|
||||
|
||||
@b pcd_to_pointcloud is a simple node that loads PCD (Point Cloud Data) files from disk and publishes them as ROS messages on the network.
|
||||
|
||||
**/
|
||||
|
||||
// ROS core
|
||||
#include <ros/ros.h>
|
||||
#include <pcl/io/io.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
#include "pcl_ros/publisher.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
class PCDGenerator
|
||||
{
|
||||
protected:
|
||||
string tf_frame_;
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle private_nh_;
|
||||
public:
|
||||
|
||||
// ROS messages
|
||||
sensor_msgs::PointCloud2 cloud_;
|
||||
|
||||
string file_name_, cloud_topic_;
|
||||
double rate_;
|
||||
|
||||
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
|
||||
{
|
||||
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
|
||||
|
||||
cloud_topic_ = "cloud_pcd";
|
||||
pub_.advertise (nh_, cloud_topic_.c_str (), 1);
|
||||
private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
|
||||
ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Start
|
||||
int
|
||||
start ()
|
||||
{
|
||||
if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
|
||||
return (-1);
|
||||
cloud_.header.frame_id = tf_frame_;
|
||||
return (0);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Spin (!)
|
||||
bool spin ()
|
||||
{
|
||||
int nr_points = cloud_.width * cloud_.height;
|
||||
string fields_list = pcl::getFieldsList (cloud_);
|
||||
double interval = rate_ * 1e+6;
|
||||
while (nh_.ok ())
|
||||
{
|
||||
ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ());
|
||||
cloud_.header.stamp = ros::Time::now ();
|
||||
|
||||
if (pub_.getNumSubscribers () > 0)
|
||||
{
|
||||
ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
|
||||
pub_.publish (cloud_);
|
||||
}
|
||||
else
|
||||
{
|
||||
ros::Duration (0.001).sleep ();
|
||||
continue;
|
||||
}
|
||||
|
||||
usleep (interval);
|
||||
|
||||
if (interval == 0) // We only publish once if a 0 seconds interval is given
|
||||
break;
|
||||
}
|
||||
|
||||
ros::Duration (3.0).sleep ();
|
||||
return (true);
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
/* ---[ */
|
||||
int
|
||||
main (int argc, char** argv)
|
||||
{
|
||||
if (argc < 3)
|
||||
{
|
||||
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
|
||||
return (-1);
|
||||
}
|
||||
|
||||
ros::init (argc, argv, "pcd_to_pointcloud");
|
||||
|
||||
PCDGenerator c;
|
||||
c.file_name_ = string (argv[1]);
|
||||
c.rate_ = atof (argv[2]);
|
||||
|
||||
if (c.start () == -1)
|
||||
{
|
||||
ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
|
||||
return (-1);
|
||||
}
|
||||
ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ());
|
||||
c.spin ();
|
||||
|
||||
return (0);
|
||||
}
|
||||
/* ]--- */
|
||||
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2009, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
* $Id: pointcloud_to_pcd.cpp 33238 2010-03-11 00:46:58Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
// ROS core
|
||||
#include <ros/ros.h>
|
||||
// PCL includes
|
||||
#include <pcl/io/io.h>
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
/**
|
||||
\author Radu Bogdan Rusu
|
||||
|
||||
@b pointcloud_to_pcd is a simple node that retrieves a ROS point cloud message and saves it to disk into a PCD (Point
|
||||
Cloud Data) file format.
|
||||
|
||||
**/
|
||||
class PointCloudToPCD
|
||||
{
|
||||
protected:
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
private:
|
||||
std::string prefix_;
|
||||
|
||||
public:
|
||||
string cloud_topic_;
|
||||
|
||||
ros::Subscriber sub_;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Callback
|
||||
void
|
||||
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
|
||||
{
|
||||
if ((cloud->width * cloud->height) == 0)
|
||||
return;
|
||||
|
||||
ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
|
||||
(int)cloud->width * cloud->height,
|
||||
cloud->header.frame_id.c_str (),
|
||||
pcl::getFieldsList (*cloud).c_str ());
|
||||
|
||||
std::stringstream ss;
|
||||
ss << prefix_ << cloud->header.stamp << ".pcd";
|
||||
ROS_INFO ("Data saved to %s", ss.str ().c_str ());
|
||||
|
||||
pcl::io::savePCDFile (ss.str (), *cloud, Eigen::Vector4f::Zero (),
|
||||
Eigen::Quaternionf::Identity (), false);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
PointCloudToPCD ()
|
||||
{
|
||||
// Check if a prefix parameter is defined for output file names.
|
||||
ros::NodeHandle priv_nh("~");
|
||||
if (priv_nh.getParam ("prefix", prefix_))
|
||||
{
|
||||
ROS_INFO_STREAM ("PCD file prefix is: " << prefix_);
|
||||
}
|
||||
else if (nh_.getParam ("prefix", prefix_))
|
||||
{
|
||||
ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
|
||||
<< prefix_);
|
||||
}
|
||||
|
||||
cloud_topic_ = "input";
|
||||
|
||||
sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
|
||||
ROS_INFO ("Listening for incoming data on topic %s",
|
||||
nh_.resolveName (cloud_topic_).c_str ());
|
||||
}
|
||||
};
|
||||
|
||||
/* ---[ */
|
||||
int
|
||||
main (int argc, char** argv)
|
||||
{
|
||||
ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
|
||||
|
||||
PointCloudToPCD b;
|
||||
ros::spin ();
|
||||
|
||||
return (0);
|
||||
}
|
||||
/* ]--- */
|
||||
Reference in New Issue
Block a user