48 #include <pcl/io/io.h> 49 #include <pcl/io/pcd_io.h> 50 #include <pcl/point_types.h> 78 cloud_topic_ =
"cloud_pcd";
79 pub_.
advertise (nh_, cloud_topic_.c_str (), 1);
80 private_nh_.
param(
"frame_id", tf_frame_, std::string(
"/base_link"));
81 ROS_INFO (
"Publishing data on topic %s with frame_id %s.", nh_.
resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
91 cloud_.header.frame_id = tf_frame_;
99 int nr_points = cloud_.width * cloud_.height;
101 double interval = wait_ * 1e+6;
104 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 ());
140 std::cerr <<
"Syntax is: " << argv[0] <<
" <file.pcd> [publishing_interval (in seconds)]" << std::endl;
144 ros::init (argc, argv,
"pcd_to_pointcloud");
155 c.
wait_ = atof (argv[2]);
158 if (c.
start () == -1)
160 ROS_ERROR (
"Could not load file %s. Exiting.", argv[1]);
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
std::string resolveName(const std::string &name, bool remap=true) const
pcl_ros::Publisher< sensor_msgs::PointCloud2 > pub_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_DEBUG_ONCE(...)
sensor_msgs::PointCloud2 cloud_
int main(int argc, char **argv)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void publish(const sensor_msgs::PointCloud2Ptr &point_cloud) const
uint32_t getNumSubscribers() const
ros::NodeHandle private_nh_
void advertise(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size)