34 #include <arpa/inet.h> 42 const uint32_t laser_cloud_step = 16;
53 color_laser_pointcloud_(),
54 color_laser_publisher_(),
56 laser_pointcloud_sub_(),
100 const sensor_msgs::Image::ConstPtr& message
108 image_channels_ = message->data.size() / (message->height * message->width);
119 const sensor_msgs::CameraInfo::ConstPtr& message
128 sensor_msgs::PointCloud2::Ptr message
159 float* pointCloudDataP =
reinterpret_cast<float*
>(&(message->data[0]));
161 uint32_t height = message->height;
162 uint32_t width = message->width;
163 uint32_t cloudStep = message->point_step / message->fields.size();
165 uint32_t validPoints = 0;
166 for( uint32_t index = 0 ; index < height * width ;
167 ++index, pointCloudDataP += cloudStep)
169 float x = pointCloudDataP[0];
170 float y = pointCloudDataP[1];
171 float z = pointCloudDataP[2];
178 if (sqrt(pow(x,2) + pow(y,2) + pow(z,2)) > 58.0)
201 if (u <
color_image_.width && v < color_image_.height && u >= 0 && v >= 0)
204 colorPointCloudDataP[0] = x;
205 colorPointCloudDataP[1] = y;
206 colorPointCloudDataP[2] = z;
208 uint8_t* colorChannelP =
reinterpret_cast<uint8_t*
>(&colorPointCloudDataP[3]);
219 colorChannelP[2] = imageDataP[2];
221 colorChannelP[1] = imageDataP[1];
223 colorChannelP[0] = imageDataP[0];
232 colorChannelP[2] = imageDataP[0];
233 colorChannelP[1] = imageDataP[0];
236 colorPointCloudDataP += cloudStep;
285 int main(
int argc,
char** argv)
290 ros::init(argc, argv,
"color_laser_publisher");
298 catch(std::exception& e)
ros::Subscriber color_image_sub_
ros::Publisher color_laser_publisher_
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::Image color_image_
int main(int argc, char **argv)
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)
sensor_msgs::CameraInfo camera_info_
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
void laserPointCloudCallback(sensor_msgs::PointCloud2::Ptr message)
ColorLaser(ros::NodeHandle &nh)
ros::Subscriber laser_pointcloud_sub_
void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &message)
uint32_t getNumPublishers() const
ros::NodeHandle node_handle_
TFSIMD_FORCE_INLINE const tfScalar & x() const
void colorImageCallback(const sensor_msgs::Image::ConstPtr &message)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE const tfScalar & z() const
uint32_t getNumSubscribers() const
ros::Subscriber camera_info_sub_
sensor_msgs::PointCloud2 color_laser_pointcloud_