62 data_->loadConfigFile(node, private_nh);
63 private_nh.
param(
"model", model, std::string(
"RS16"));
68 srv_ = boost::make_shared<dynamic_reconfigure::Server<rslidar_pointcloud::CloudNodeConfig> >(private_nh);
69 dynamic_reconfigure::Server<rslidar_pointcloud::CloudNodeConfig>::CallbackType
f;
87 pcl::PointCloud<pcl::PointXYZI>::Ptr outPoints(
new pcl::PointCloud<pcl::PointXYZI>);
89 outPoints->header.frame_id = scanMsg->header.frame_id;
93 outPoints->height = 16;
94 outPoints->width = 24 * (int)scanMsg->packets.size();
95 outPoints->is_dense =
false;
96 outPoints->resize(outPoints->height * outPoints->width);
98 else if (model ==
"RS32")
100 outPoints->height = 32;
101 outPoints->width = 12 * (int)scanMsg->packets.size();
102 outPoints->is_dense =
false;
103 outPoints->resize(outPoints->height * outPoints->width);
108 data_->block_num = 0;
109 for (
size_t i = 0; i < scanMsg->packets.size(); ++i)
111 data_->unpack(scanMsg->packets[i], outPoints);
113 sensor_msgs::PointCloud2 outMsg;
void processScan(const rslidar_msgs::rslidarScan::ConstPtr &scanMsg)
Callback for raw scan messages.
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void callback(rslidar_pointcloud::CloudNodeConfig &config, uint32_t level)
ros::Subscriber rslidar_scan_
boost::shared_ptr< dynamic_reconfigure::Server< rslidar_pointcloud::CloudNodeConfig > > srv_
Pointer to dynamic reconfigure service srv_.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Convert(ros::NodeHandle node, ros::NodeHandle private_nh)
Constructor.
boost::shared_ptr< rslidar_rawdata::RawData > data_
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)