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

Apply ament_uncrustify --reformat (#297)

Browse Source 
Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly
2020-08-06 15:28:29 -04:00
committed by GitHub
parent 0ac6810688
commit 63cee139f1
88 changed files with 6019 additions and 5318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pcl_ros/tools/bag_to_pcd.cpp
+46 -55
View File
@@ -61,14 +61,15 @@ typedef PointCloud::ConstPtr PointCloudConstPtr;
/* ---[ */
int
main (int argc, char** argv)
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> [<target_frame>]" << std::endl;
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
return (-1);
ros::init(argc, argv, "bag_to_pcd");
if (argc < 4) {
std::cerr << "Syntax is: " << argv[0] <<
" <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" <<
std::endl;
return -1;
}
// TF
@@ -79,87 +80,77 @@ int
rosbag::View view;
rosbag::View::iterator view_it;
try
{
bag.open (argv[1], rosbag::bagmode::Read);
}
catch (rosbag::BagException)
{
try {
bag.open(argv[1], rosbag::bagmode::Read);
} catch (rosbag::BagException) {
std::cerr << "Error opening file " << argv[1] << std::endl;
return (-1);
return -1;
}
view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2"));
view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage"));
view_it = view.begin ();
view.addQuery(bag, rosbag::TypeQuery("sensor_msgs/PointCloud2"));
view.addQuery(bag, rosbag::TypeQuery("tf/tfMessage"));
view.addQuery(bag, rosbag::TypeQuery("tf2_msgs/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::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);
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;
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);
ros::Duration r(0.001);
// Loop over the whole bag file
while (view_it != view.end ())
{
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
{
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)
{
PointCloudConstPtr cloud = view_it->instantiate<PointCloud>();
if (cloud == NULL) {
++view_it;
continue;
}
// If a target_frame was specified
if(argc > 4)
{
if (argc > 4) {
// Transform it
if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
{
++view_it;
continue;
if (!pcl_ros::transformPointCloud(argv[4], *cloud, cloud_t, tf_listener)) {
++view_it;
continue;
}
}
else
{
} else {
// Else, don't transform it
cloud_t = *cloud;
}
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::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);
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);
return 0;
}
/* ]--- */
pcl_ros/tools/convert_pcd_to_image.cpp
+17 -21
View File
@@ -56,41 +56,37 @@
/* ---[ */
int
main (int argc, char **argv)
main(int argc, char ** argv)
{
ros::init (argc, argv, "image_publisher");
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
ros::Publisher image_pub = nh.advertise <sensor_msgs::Image> ("output", 1);
ros::Publisher image_pub = nh.advertise<sensor_msgs::Image>("output", 1);
if (argc != 2)
{
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);
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());
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 ();
ros::Rate loop_rate(5);
while (nh.ok()) {
image_pub.publish(image);
ros::spinOnce();
loop_rate.sleep();
}
return (0);
return 0;
}
/* ]--- */
pcl_ros/tools/convert_pointcloud_to_image.cpp
+23 -23
View File
@@ -54,37 +54,37 @@ class PointCloudToImage
{
public:
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
cloud_cb(const sensor_msgs::PointCloud2ConstPtr & cloud)
{
if (cloud->height <= 1)
{
if (cloud->height <= 1) {
ROS_ERROR("Input point cloud is not organized, ignoring!");
return;
}
try
{
pcl::toROSMsg (*cloud, image_); //convert the cloud
try {
pcl::toROSMsg(*cloud, image_); //convert the cloud
image_.header = cloud->header;
image_pub_.publish (image_); //publish our cloud image
}
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
} catch (std::runtime_error & e) {
ROS_ERROR_STREAM(
"Error in converting cloud to image message: " <<
e.what());
}
}
PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
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);
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 );
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
@@ -95,10 +95,10 @@ private:
};
int
main (int argc, char **argv)
main(int argc, char ** argv)
{
ros::init (argc, argv, "convert_pointcloud_to_image");
ros::init(argc, argv, "convert_pointcloud_to_image");
PointCloudToImage pci; //this loads up the node
ros::spin (); //where she stops nobody knows
ros::spin(); //where she stops nobody knows
return 0;
}
pcl_ros/tools/pcd_to_pointcloud.cpp
+83 -84
View File
@@ -60,113 +60,112 @@ using namespace std;
class PCDGenerator
{
protected:
string tf_frame_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
public:
protected:
string tf_frame_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
// ROS messages
sensor_msgs::PointCloud2 cloud_;
public:
// ROS messages
sensor_msgs::PointCloud2 cloud_;
string file_name_, cloud_topic_;
double wait_;
string file_name_, cloud_topic_;
double wait_;
pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_;
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
////////////////////////////////////////////////////////////////////////////////
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());
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;
}
////////////////////////////////////////////////////////////////////////////////
// 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 = wait_ * 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();
////////////////////////////////////////////////////////////////////////////////
// Spin (!)
bool spin ()
{
int nr_points = cloud_.width * cloud_.height;
string fields_list = pcl::getFieldsList (cloud_);
double interval = wait_ * 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
{
// check once a second if there is any subscriber
ros::Duration (1).sleep ();
continue;
}
std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
if (interval == 0) // We only publish once if a 0 seconds interval is given
{
// Give subscribers 3 seconds until point cloud decays... a little ugly!
ros::Duration (3.0).sleep ();
break;
}
if (pub_.getNumSubscribers() > 0) {
ROS_DEBUG("Publishing data to %d subscribers.", pub_.getNumSubscribers());
pub_.publish(cloud_);
} else {
// check once a second if there is any subscriber
ros::Duration(1).sleep();
continue;
}
std::this_thread::sleep_for(std::chrono::microseconds(static_cast<uint32_t>(interval)));
if (interval == 0) { // We only publish once if a 0 seconds interval is given
// Give subscribers 3 seconds until point cloud decays... a little ugly!
ros::Duration(3.0).sleep();
break;
}
return (true);
}
return true;
}
};
/* ---[ */
int
main (int argc, char** argv)
main(int argc, char ** argv)
{
if (argc < 2)
{
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
return (-1);
if (argc < 2) {
std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" <<
std::endl;
return -1;
}
ros::init (argc, argv, "pcd_to_pointcloud");
ros::init(argc, argv, "pcd_to_pointcloud");
PCDGenerator c;
c.file_name_ = string (argv[1]);
c.file_name_ = string(argv[1]);
// check if publishing interval is given
if (argc == 2)
{
c.wait_ = 0;
}
else
{
c.wait_ = atof (argv[2]);
}
if (c.start () == -1)
{
ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
return (-1);
if (argc == 2) {
c.wait_ = 0;
} else {
c.wait_ = atof(argv[2]);
}
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);
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;
}
/* ]--- */
pcl_ros/tools/pointcloud_to_pcd.cpp
+97 -100
View File
@@ -64,124 +64,121 @@ Cloud Data) file format.
**/
class PointCloudToPCD
{
protected:
ros::NodeHandle nh_;
protected:
ros::NodeHandle nh_;
private:
std::string prefix_;
bool binary_;
bool compressed_;
std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
private:
std::string prefix_;
bool binary_;
bool compressed_;
std::string fixed_frame_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
public:
string cloud_topic_;
public:
string cloud_topic_;
ros::Subscriber sub_;
ros::Subscriber sub_;
////////////////////////////////////////////////////////////////////////////////
// Callback
void
cloud_cb (const boost::shared_ptr<const pcl::PCLPointCloud2>& cloud)
{
if ((cloud->width * cloud->height) == 0)
////////////////////////////////////////////////////////////////////////////////
// Callback
void
cloud_cb(const boost::shared_ptr<const pcl::PCLPointCloud2> & 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());
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;
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 ());
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;
ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO ("Data saved to %s", ss.str ().c_str ());
pcl::PCDWriter writer;
if(binary_)
{
if(compressed_)
{
writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
}
else
{
writer.writeBinary (ss.str (), *cloud, v, q);
}
}
else
{
writer.writeASCII (ss.str (), *cloud, v, q, 8);
}
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>();
}
////////////////////////////////////////////////////////////////////////////////
PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_)
{
// 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_);
}
std::stringstream ss;
ss << prefix_ << cloud->header.stamp << ".pcd";
ROS_INFO("Data saved to %s", ss.str().c_str());
priv_nh.getParam ("fixed_frame", fixed_frame_);
priv_nh.getParam ("binary", binary_);
priv_nh.getParam ("compressed", compressed_);
if(binary_)
{
if(compressed_)
{
ROS_INFO_STREAM ("Saving as binary compressed PCD");
}
else
{
ROS_INFO_STREAM ("Saving as binary PCD");
}
}
else
{
ROS_INFO_STREAM ("Saving as binary PCD");
}
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 ());
pcl::PCDWriter writer;
if (binary_) {
if (compressed_) {
writer.writeBinaryCompressed(ss.str(), *cloud, v, q);
} else {
writer.writeBinary(ss.str(), *cloud, v, q);
}
} else {
writer.writeASCII(ss.str(), *cloud, v, q, 8);
}
}
////////////////////////////////////////////////////////////////////////////////
PointCloudToPCD()
: binary_(false), compressed_(false), tf_listener_(tf_buffer_)
{
// 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_);
}
priv_nh.getParam("fixed_frame", fixed_frame_);
priv_nh.getParam("binary", binary_);
priv_nh.getParam("compressed", compressed_);
if (binary_) {
if (compressed_) {
ROS_INFO_STREAM("Saving as binary compressed PCD");
} else {
ROS_INFO_STREAM("Saving as binary PCD");
}
} else {
ROS_INFO_STREAM("Saving as binary PCD");
}
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)
main(int argc, char ** argv)
{
ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
ros::init(argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
PointCloudToPCD b;
ros::spin ();
ros::spin();
return (0);
return 0;
}
/* ]--- */