39 #include <sensor_msgs/PointCloud2.h> 41 #include <pcl/point_cloud.h> 52 void callback(
const sensor_msgs::PointCloud2::ConstPtr& pc)
55 PointCloudT::Ptr cloud = boost::make_shared<PointCloudT>();
62 invalid.x = invalid.y = invalid.z = std::numeric_limits<float>::quiet_NaN();
63 PointCloudT::Ptr cloud_out = boost::make_shared<PointCloudT>(width, height, invalid);
64 cloud_out->is_dense =
false;
66 for (
size_t i = 0; i < cloud->size(); i++)
68 const PointT &p = cloud->points[i];
72 if (col < 0 || col >= width || row < 0 || row >= height)
74 ROS_ERROR(
"Invalid coordinates: (%d, %d) is outside (0, 0)..(%zu, %zu)!", col, row, width, height);
77 (*cloud_out)[row * width + col] = p;
80 sensor_msgs::PointCloud2::Ptr
msg = boost::make_shared<sensor_msgs::PointCloud2>();
82 msg->header = pc->header;
86 int main(
int argc,
char **argv)
88 ros::init(argc, argv,
"sick_ldmrs_make_organized");
97 pub = nh.
advertise<sensor_msgs::PointCloud2>(
"organized", 1);
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())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
ROSCPP_DECL void spin(Spinner &spinner)
double angular_resolution_
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)
void callback(const sensor_msgs::PointCloud2::ConstPtr &pc)
sick_ldmrs_msgs::SICK_LDMRS_Point PointT
int main(int argc, char **argv)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
pcl::PointCloud< PointT > PointCloudT