CMake install fixes
This commit is contained in:
Julius Kammerl
Paul Bovbel
parent
baa59c0d00
commit
540fcd0604
@ -67,7 +67,7 @@ target_link_libraries (pcd_to_pointcloud pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARI
|
||||
add_executable (pointcloud_to_pcd tools/pointcloud_to_pcd.cpp)
|
||||
target_link_libraries (pointcloud_to_pcd pcl_io ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
|
||||
add_executable (bag_to_pcd tools/bag_to_pcd.cpp)
|
||||
add_executable ( bag_to_pcd tools/bag_to_pcd.cpp)
|
||||
target_link_libraries (bag_to_pcd pcl_io pcl_ros_tf ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
|
||||
|
||||
add_executable (convert_pcd_to_image tools/convert_pcd_to_image.cpp)
|
||||
@ -81,8 +81,15 @@ install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
|
||||
install(TARGETS pcl_ros_tf
|
||||
install(TARGETS pcl_ros_tf pcl_ros_io pcl_ros_filters
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
)
|
||||
|
||||
install(TARGETS pcd_to_pointcloud pointcloud_to_pcd bag_to_pcd convert_pcd_to_image convert_pointcloud_to_image
|
||||
DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
|
||||
|
||||
install(FILES pcl_nodelets.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
|
||||
|
||||
113
pcl_ros/src/pcl_ros/io/bag_io.cpp
Normal file
113
pcl_ros/src/pcl_ros/io/bag_io.cpp
Normal file
@ -0,0 +1,113 @@
|
||||
/*
|
||||
* 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_io.cpp 34896 2010-12-19 06:21:42Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include "pcl_ros/io/bag_io.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
bool
|
||||
pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
|
||||
{
|
||||
try
|
||||
{
|
||||
bag_.open (file_name, rosbag::bagmode::Read);
|
||||
view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
|
||||
|
||||
if (view_.size () == 0)
|
||||
return (false);
|
||||
|
||||
it_ = view_.begin ();
|
||||
}
|
||||
catch (rosbag::BagException &e)
|
||||
{
|
||||
return (false);
|
||||
}
|
||||
return (true);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::BAGReader::onInit ()
|
||||
{
|
||||
boost::shared_ptr<ros::NodeHandle> pnh_;
|
||||
pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
|
||||
// ---[ Mandatory parameters
|
||||
if (!pnh_->getParam ("file_name", file_name_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
if (!pnh_->getParam ("topic_name", topic_name_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
int max_queue_size = 1;
|
||||
pnh_->getParam ("publish_rate", publish_rate_);
|
||||
pnh_->getParam ("max_queue_size", max_queue_size);
|
||||
|
||||
ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
|
||||
|
||||
NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - file_name : %s\n"
|
||||
" - topic_name : %s",
|
||||
file_name_.c_str (), topic_name_.c_str ());
|
||||
|
||||
if (!open (file_name_, topic_name_))
|
||||
return;
|
||||
PointCloud output;
|
||||
output_ = boost::make_shared<PointCloud> (output);
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
|
||||
// Continous publishing enabled?
|
||||
while (pnh_->ok ())
|
||||
{
|
||||
PointCloudConstPtr cloud = getNextCloud ();
|
||||
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
|
||||
pub_output.publish (output_);
|
||||
|
||||
ros::Duration (publish_rate_).sleep ();
|
||||
ros::spinOnce ();
|
||||
}
|
||||
}
|
||||
|
||||
typedef pcl_ros::BAGReader BAGReader;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, BAGReader, BAGReader, nodelet::Nodelet);
|
||||
|
||||
257
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
Normal file
257
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
Normal file
@ -0,0 +1,257 @@
|
||||
/*
|
||||
* 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: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/transforms.h"
|
||||
#include "pcl_ros/io/concatenate_data.h"
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
|
||||
{
|
||||
private_nh_ = getMTPrivateNodeHandle ();
|
||||
// ---[ Mandatory parameters
|
||||
private_nh_.getParam ("output_frame", output_frame_);
|
||||
private_nh_.getParam ("approximate_sync", approximate_sync_);
|
||||
|
||||
if (output_frame_.empty ())
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
|
||||
// ---[ Optional parameters
|
||||
private_nh_.getParam ("max_queue_size", maximum_queue_size_);
|
||||
|
||||
// Output
|
||||
pub_output_ = private_nh_.advertise<PointCloud2> ("output", maximum_queue_size_);
|
||||
|
||||
XmlRpc::XmlRpcValue input_topics;
|
||||
if (!private_nh_.getParam ("input_topics", input_topics))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
// Check the type
|
||||
switch (input_topics.getType ())
|
||||
{
|
||||
case XmlRpc::XmlRpcValue::TypeArray:
|
||||
{
|
||||
if (input_topics.size () == 1)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (input_topics.size () > 8)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] More than 8 topics passed!");
|
||||
return;
|
||||
}
|
||||
|
||||
ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:");
|
||||
for (int d = 0; d < input_topics.size (); ++d)
|
||||
ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
|
||||
|
||||
// Subscribe to the filters
|
||||
filters_.resize (input_topics.size ());
|
||||
|
||||
// 8 topics
|
||||
if (approximate_sync_)
|
||||
ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
> (maximum_queue_size_));
|
||||
else
|
||||
ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2, PointCloud2,
|
||||
PointCloud2, PointCloud2>
|
||||
> (maximum_queue_size_));
|
||||
|
||||
// First input_topics.size () filters are valid
|
||||
for (int d = 0; d < input_topics.size (); ++d)
|
||||
{
|
||||
filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
|
||||
filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_);
|
||||
}
|
||||
|
||||
// Bogus null filter
|
||||
filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
|
||||
|
||||
switch (input_topics.size ())
|
||||
{
|
||||
case 2:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
case 5:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
|
||||
break;
|
||||
}
|
||||
case 6:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
|
||||
break;
|
||||
}
|
||||
case 7:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
|
||||
break;
|
||||
}
|
||||
case 8:
|
||||
{
|
||||
if (approximate_sync_)
|
||||
ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
|
||||
else
|
||||
ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (approximate_sync_)
|
||||
ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
|
||||
else
|
||||
ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
|
||||
{
|
||||
//ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
|
||||
PointCloud2::Ptr in1_t (new PointCloud2 ());
|
||||
PointCloud2::Ptr in2_t (new PointCloud2 ());
|
||||
|
||||
// Transform the point clouds into the specified output frame
|
||||
if (output_frame_ != in1.header.frame_id)
|
||||
pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
|
||||
else
|
||||
in1_t = boost::make_shared<PointCloud2> (in1);
|
||||
|
||||
if (output_frame_ != in2.header.frame_id)
|
||||
pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
|
||||
else
|
||||
in2_t = boost::make_shared<PointCloud2> (in2);
|
||||
|
||||
// Concatenate the results
|
||||
pcl::concatenatePointCloud (*in1_t, *in2_t, out);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::input (
|
||||
const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
|
||||
const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
|
||||
const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
|
||||
const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
|
||||
{
|
||||
PointCloud2::Ptr out1 (new PointCloud2 ());
|
||||
PointCloud2::Ptr out2 (new PointCloud2 ());
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
|
||||
if (in3)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
|
||||
out1 = out2;
|
||||
if (in4)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
|
||||
if (in5)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
|
||||
out1 = out2;
|
||||
if (in6)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
|
||||
if (in7)
|
||||
{
|
||||
pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
|
||||
out1 = out2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
|
||||
}
|
||||
|
||||
typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateDataSynchronizer, PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);
|
||||
|
||||
152
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
Normal file
152
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
Normal file
@ -0,0 +1,152 @@
|
||||
/*
|
||||
* 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: concatenate_fields.cpp 35052 2011-01-03 21:04:57Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
/** \author Radu Bogdan Rusu */
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl/io/io.h>
|
||||
#include "pcl_ros/io/concatenate_fields.h"
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit ()
|
||||
{
|
||||
private_nh_ = getMTPrivateNodeHandle ();
|
||||
// ---[ Mandatory parameters
|
||||
if (!private_nh_.getParam ("input_messages", input_messages_))
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Need a 'input_messages' parameter to be set before continuing!");
|
||||
return;
|
||||
}
|
||||
if (input_messages_ < 2)
|
||||
{
|
||||
NODELET_ERROR ("[onInit] Invalid 'input_messages' parameter given!");
|
||||
return;
|
||||
}
|
||||
// ---[ Optional parameters
|
||||
private_nh_.getParam ("max_queue_size", maximum_queue_size_);
|
||||
private_nh_.getParam ("maximum_seconds", maximum_seconds_);
|
||||
sub_input_ = private_nh_.subscribe ("input", maximum_queue_size_, &PointCloudConcatenateFieldsSynchronizer::input_callback, this);
|
||||
pub_output_ = private_nh_.advertise<sensor_msgs::PointCloud2> ("output", maximum_queue_size_);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback (const PointCloudConstPtr &cloud)
|
||||
{
|
||||
NODELET_DEBUG ("[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), private_nh_.resolveName ("input").c_str ());
|
||||
|
||||
// Erase old data in the queue
|
||||
if (maximum_seconds_ > 0 && queue_.size () > 0)
|
||||
{
|
||||
while (fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ) > maximum_seconds_ && queue_.size () > 0)
|
||||
{
|
||||
NODELET_WARN ("[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
|
||||
(*queue_.begin ()).first.toSec (), fabs ( ( (*queue_.begin ()).first - cloud->header.stamp).toSec () ));
|
||||
queue_.erase (queue_.begin ());
|
||||
}
|
||||
}
|
||||
|
||||
// Push back new data
|
||||
queue_[cloud->header.stamp].push_back (cloud);
|
||||
if ((int)queue_[cloud->header.stamp].size () >= input_messages_)
|
||||
{
|
||||
// Concatenate together and publish
|
||||
std::vector<PointCloudConstPtr> &clouds = queue_[cloud->header.stamp];
|
||||
PointCloud cloud_out = *clouds[0];
|
||||
|
||||
// Resize the output dataset
|
||||
int data_size = cloud_out.data.size ();
|
||||
int nr_fields = cloud_out.fields.size ();
|
||||
int nr_points = cloud_out.width * cloud_out.height;
|
||||
for (size_t i = 1; i < clouds.size (); ++i)
|
||||
{
|
||||
assert (clouds[i]->data.size () / (clouds[i]->width * clouds[i]->height) == clouds[i]->point_step);
|
||||
|
||||
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height)
|
||||
{
|
||||
NODELET_ERROR ("[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
|
||||
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
|
||||
break;
|
||||
}
|
||||
// Point step must increase with the length of each new field
|
||||
cloud_out.point_step += clouds[i]->point_step;
|
||||
// Resize data to hold all clouds
|
||||
data_size += clouds[i]->data.size ();
|
||||
|
||||
// Concatenate fields
|
||||
cloud_out.fields.resize (nr_fields + clouds[i]->fields.size ());
|
||||
int delta_offset = cloud_out.fields[nr_fields - 1].offset + pcl::getFieldSize (cloud_out.fields[nr_fields - 1].datatype);
|
||||
for (size_t d = 0; d < clouds[i]->fields.size (); ++d)
|
||||
{
|
||||
cloud_out.fields[nr_fields + d] = clouds[i]->fields[d];
|
||||
cloud_out.fields[nr_fields + d].offset += delta_offset;
|
||||
}
|
||||
nr_fields += clouds[i]->fields.size ();
|
||||
}
|
||||
// Recalculate row_step
|
||||
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
|
||||
cloud_out.data.resize (data_size);
|
||||
|
||||
// Iterate over each point and perform the appropriate memcpys
|
||||
int point_offset = 0;
|
||||
for (int cp = 0; cp < nr_points; ++cp)
|
||||
{
|
||||
for (size_t i = 0; i < clouds.size (); ++i)
|
||||
{
|
||||
// Copy each individual point
|
||||
memcpy (&cloud_out.data[point_offset], &clouds[i]->data[cp * clouds[i]->point_step], clouds[i]->point_step);
|
||||
point_offset += clouds[i]->point_step;
|
||||
}
|
||||
}
|
||||
pub_output_.publish (boost::make_shared<const PointCloud> (cloud_out));
|
||||
queue_.erase (cloud->header.stamp);
|
||||
}
|
||||
|
||||
// Clean the queue to avoid overflowing
|
||||
if (maximum_queue_size_ > 0)
|
||||
{
|
||||
while ((int)queue_.size () > maximum_queue_size_)
|
||||
queue_.erase (queue_.begin ());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateFieldsSynchronizer, PointCloudConcatenateFieldsSynchronizer, nodelet::Nodelet);
|
||||
|
||||
57
pcl_ros/src/pcl_ros/io/io.cpp
Normal file
57
pcl_ros/src/pcl_ros/io/io.cpp
Normal file
@ -0,0 +1,57 @@
|
||||
/*
|
||||
* 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: io.cpp 35361 2011-01-20 04:34:49Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
//#include <pcl_ros/subscriber.h>
|
||||
#include <nodelet_topic_tools/nodelet_mux.h>
|
||||
#include <nodelet_topic_tools/nodelet_demux.h>
|
||||
|
||||
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2, message_filters::Subscriber<sensor_msgs::PointCloud2> > NodeletMUX;
|
||||
//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
|
||||
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
|
||||
|
||||
//#include "pcd_io.cpp"
|
||||
//#include "bag_io.cpp"
|
||||
//#include "concatenate_fields.cpp"
|
||||
//#include "concatenate_data.cpp"
|
||||
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NodeletMUX, NodeletMUX, nodelet::Nodelet);
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX, NodeletDEMUX, nodelet::Nodelet);
|
||||
//PLUGINLIB_DECLARE_CLASS (pcl, NodeletDEMUX_ROS, NodeletDEMUX_ROS, nodelet::Nodelet);
|
||||
|
||||
175
pcl_ros/src/pcl_ros/io/pcd_io.cpp
Normal file
175
pcl_ros/src/pcl_ros/io/pcd_io.cpp
Normal file
@ -0,0 +1,175 @@
|
||||
/*
|
||||
* 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_io.cpp 35812 2011-02-08 00:05:03Z rusu $
|
||||
*
|
||||
*/
|
||||
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <pcl_ros/io/pcd_io.h>
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDReader::onInit ()
|
||||
{
|
||||
ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
|
||||
// Provide a latched topic
|
||||
ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
|
||||
|
||||
private_nh.getParam ("publish_rate", publish_rate_);
|
||||
private_nh.getParam ("tf_frame", tf_frame_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - publish_rate : %f\n"
|
||||
" - tf_frame : %s",
|
||||
getName ().c_str (),
|
||||
publish_rate_, tf_frame_.c_str ());
|
||||
|
||||
PointCloud2Ptr output_new;
|
||||
output_ = boost::make_shared<PointCloud2> ();
|
||||
output_new = boost::make_shared<PointCloud2> ();
|
||||
|
||||
// Wait in a loop until someone connects
|
||||
do
|
||||
{
|
||||
ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
|
||||
ros::spinOnce ();
|
||||
ros::Duration (0.01).sleep ();
|
||||
}
|
||||
while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
|
||||
|
||||
std::string file_name;
|
||||
|
||||
while (private_nh.ok ())
|
||||
{
|
||||
// Get the current filename parameter. If no filename set, loop
|
||||
if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
|
||||
{
|
||||
ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
|
||||
ros::Duration (0.01).sleep ();
|
||||
ros::spinOnce ();
|
||||
continue;
|
||||
}
|
||||
|
||||
// If the filename parameter holds a different value than the last one we read
|
||||
if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
|
||||
{
|
||||
NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
|
||||
file_name = file_name_;
|
||||
PointCloud2 cloud;
|
||||
if (impl_.read (file_name_, cloud) < 0)
|
||||
{
|
||||
NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
|
||||
return;
|
||||
}
|
||||
output_ = boost::make_shared<PointCloud2> (cloud);
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
output_->header.frame_id = tf_frame_;
|
||||
}
|
||||
|
||||
// We do not publish empty data
|
||||
if (output_->data.size () == 0)
|
||||
continue;
|
||||
|
||||
if (publish_rate_ == 0)
|
||||
{
|
||||
if (output_ != output_new)
|
||||
{
|
||||
NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
pub_output.publish (output_);
|
||||
output_new = output_;
|
||||
}
|
||||
ros::Duration (0.01).sleep ();
|
||||
}
|
||||
else
|
||||
{
|
||||
NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
|
||||
output_->header.stamp = ros::Time::now ();
|
||||
pub_output.publish (output_);
|
||||
|
||||
ros::Duration (publish_rate_).sleep ();
|
||||
}
|
||||
|
||||
ros::spinOnce ();
|
||||
// Update parameters from server
|
||||
private_nh.getParam ("publish_rate", publish_rate_);
|
||||
private_nh.getParam ("tf_frame", tf_frame_);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDWriter::onInit ()
|
||||
{
|
||||
ros::NodeHandle pnh = getMTPrivateNodeHandle ();
|
||||
sub_input_ = pnh.subscribe ("input", 1, &PCDWriter::input_callback, this);
|
||||
// ---[ Optional parameters
|
||||
pnh.getParam ("filename", file_name_);
|
||||
pnh.getParam ("binary_mode", binary_mode_);
|
||||
|
||||
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
|
||||
" - filename : %s\n"
|
||||
" - binary_mode : %s",
|
||||
getName ().c_str (),
|
||||
file_name_.c_str (), (binary_mode_) ? "true" : "false");
|
||||
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
|
||||
{
|
||||
if (!isValid (cloud))
|
||||
return;
|
||||
|
||||
getMTPrivateNodeHandle ().getParam ("filename", file_name_);
|
||||
|
||||
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
|
||||
|
||||
std::string fname;
|
||||
if (file_name_.empty ())
|
||||
fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
|
||||
else
|
||||
fname = file_name_;
|
||||
impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
|
||||
|
||||
NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
|
||||
}
|
||||
|
||||
typedef pcl_ros::PCDReader PCDReader;
|
||||
typedef pcl_ros::PCDWriter PCDWriter;
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PCDReader, PCDReader, nodelet::Nodelet);
|
||||
PLUGINLIB_DECLARE_CLASS (pcl, PCDWriter, PCDWriter, nodelet::Nodelet);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user