color_laser.cpp
Go to the documentation of this file.
1 
34 #include <arpa/inet.h>
35 
37 
38 //
39 // Anonymous namespace for locally scoped symbols
40 namespace {
41 
42  const uint32_t laser_cloud_step = 16;
43 
44 } // namespace
45 
46 namespace multisense_ros {
47 
49  ros::NodeHandle& nh
50 ):
51  color_image_(),
52  camera_info_(),
53  color_laser_pointcloud_(),
54  color_laser_publisher_(),
55  color_image_sub_(),
56  laser_pointcloud_sub_(),
57  camera_info_sub_(),
58  node_handle_(nh),
59  data_lock_(),
60  image_channels_(3)
61 {
62  //
63  // Initialize point cloud structure
64 
65  color_laser_pointcloud_.is_bigendian = (htonl(1) == 1);
66  color_laser_pointcloud_.is_dense = true;
67  color_laser_pointcloud_.height = 1;
68  color_laser_pointcloud_.point_step = laser_cloud_step;
69 
70  color_laser_pointcloud_.fields.resize(4);
71  color_laser_pointcloud_.fields[0].name = "x";
72  color_laser_pointcloud_.fields[0].offset = 0;
73  color_laser_pointcloud_.fields[0].count = 1;
74  color_laser_pointcloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
75  color_laser_pointcloud_.fields[1].name = "y";
76  color_laser_pointcloud_.fields[1].offset = 4;
77  color_laser_pointcloud_.fields[1].count = 1;
78  color_laser_pointcloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
79  color_laser_pointcloud_.fields[2].name = "z";
80  color_laser_pointcloud_.fields[2].offset = 8;
81  color_laser_pointcloud_.fields[2].count = 1;
82  color_laser_pointcloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
83  color_laser_pointcloud_.fields[3].name = "rgb";
84  color_laser_pointcloud_.fields[3].offset = 12;
85  color_laser_pointcloud_.fields[3].count = 1;
86  color_laser_pointcloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
87 
88  color_laser_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("lidar_points2_color",
89  10,
90  boost::bind(&ColorLaser::startStreaming, this),
91  boost::bind(&ColorLaser::stopStreaming, this));
92 
93 }
94 
96 {
97 }
98 
100  const sensor_msgs::Image::ConstPtr& message
101 )
102 {
103  boost::mutex::scoped_lock lock(data_lock_);
104 
105  //
106  // Images are assumed to be 8 bit.
107 
108  image_channels_ = message->data.size() / (message->height * message->width);
109 
110  if (image_channels_ > 3)
111  {
112  ROS_ERROR("Unsupported number of color channels: %d", image_channels_);
113  }
114 
115  color_image_ = *message;
116 }
117 
119  const sensor_msgs::CameraInfo::ConstPtr& message
120 )
121 {
122  boost::mutex::scoped_lock lock(data_lock_);
123 
124  camera_info_ = *message;
125 }
126 
128  sensor_msgs::PointCloud2::Ptr message
129 )
130 {
131  boost::mutex::scoped_lock lock(data_lock_);
132 
133  //
134  // Make sure the associated camera_info/color image is within 2 seconds
135  // of the current point cloud message
136 
137  if (message->header.stamp - color_image_.header.stamp > ros::Duration(2) ||
138  message->header.stamp - camera_info_.header.stamp > ros::Duration(2))
139  {
140  return;
141  }
142 
143  color_laser_pointcloud_.header = message->header;
144 
145  //
146  // Here we assume that our the sizeof our intensity field is the same as
147  // the sizeof our new rgb color field.
148 
149  color_laser_pointcloud_.data.resize(message->data.size());
150  float* colorPointCloudDataP = reinterpret_cast<float*>(&(color_laser_pointcloud_.data[0]));
151 
152  //
153  // Iterate over all the points in the point cloud, Use the camera projection
154  // matrix to project each point into the left camera image,
155  // colorize the point with the corresponding image point in the left
156  // camera. If the point does not project into the camera image
157  // do not add it to the point cloud.
158 
159  float* pointCloudDataP = reinterpret_cast<float*>(&(message->data[0]));
160 
161  uint32_t height = message->height;
162  uint32_t width = message->width;
163  uint32_t cloudStep = message->point_step / message->fields.size();
164 
165  uint32_t validPoints = 0;
166  for( uint32_t index = 0 ; index < height * width ;
167  ++index, pointCloudDataP += cloudStep)
168  {
169  float x = pointCloudDataP[0];
170  float y = pointCloudDataP[1];
171  float z = pointCloudDataP[2];
172 
173  //
174  // Invalid points from the laser will have a distance of 60m.
175  // Since these points have the laser calibration applied to them
176  // add in a 2m buffer for filtering out invalid points
177 
178  if (sqrt(pow(x,2) + pow(y,2) + pow(z,2)) > 58.0)
179  {
180  continue;
181  }
182 
183 
184  //
185  // Compute the (u,v) camera coordinates corresponding to our laser
186  // point
187 
188  //
189  // (fx*x + cx*z)/z
190  uint32_t u = (camera_info_.P[0] * x + camera_info_.P[2] * z) / z;
191  //
192  // (fy*y + cy*z)/z
193  uint32_t v = (camera_info_.P[5] * y + camera_info_.P[6] * z) / z;
194 
195 
196 
197  //
198  // If our computed (u, v) point projects into the image use its
199  // color value and add it to the color pointcloud message
200 
201  if (u < color_image_.width && v < color_image_.height && u >= 0 && v >= 0)
202  {
203 
204  colorPointCloudDataP[0] = x;
205  colorPointCloudDataP[1] = y;
206  colorPointCloudDataP[2] = z;
207 
208  uint8_t* colorChannelP = reinterpret_cast<uint8_t*>(&colorPointCloudDataP[3]);
209 
210  //
211  // Image data is assumed to be BRG and stored continuously in memory
212  // both of which are the case for color images from the MultiSense
213 
214  uint8_t* imageDataP = &color_image_.data[(image_channels_ * v * color_image_.width) + (image_channels_ * u)];
215 
216  switch(image_channels_)
217  {
218  case 3:
219  colorChannelP[2] = imageDataP[2];
220  case 2:
221  colorChannelP[1] = imageDataP[1];
222  case 1:
223  colorChannelP[0] = imageDataP[0];
224  }
225 
226  //
227  // If we are dealing with a grayscale image then copy
228  // the intensity value to all 3 channels
229 
230  if (image_channels_ == 1)
231  {
232  colorChannelP[2] = imageDataP[0];
233  colorChannelP[1] = imageDataP[0];
234  }
235 
236  colorPointCloudDataP += cloudStep;
237  ++validPoints;
238  }
239 
240  }
241 
242  color_laser_pointcloud_.data.resize(validPoints * laser_cloud_step);
243  color_laser_pointcloud_.width = validPoints;
244  color_laser_pointcloud_.row_step = validPoints * laser_cloud_step;
245 
247 }
248 
250 {
251  if (color_image_sub_.getNumPublishers() == 0 &&
254  {
255  color_image_sub_ = node_handle_.subscribe("image_rect_color",
256  10,
258  this);
259 
260  camera_info_sub_ = node_handle_.subscribe("camera_info",
261  10,
263  this);
264 
265  laser_pointcloud_sub_ = node_handle_.subscribe("lidar_points2",
266  10,
268  this);
269  }
270 
271 }
272 
274 {
276  {
280  }
281 }
282 
283 } // namespace
284 
285 int main(int argc, char** argv)
286 {
287 
288  try
289  {
290  ros::init(argc, argv, "color_laser_publisher");
291 
292  ros::NodeHandle nh;
293 
294  multisense_ros::ColorLaser colorLaserPublisher(nh);
295 
296  ros::spin();
297  }
298  catch(std::exception& e)
299  {
300  ROS_ERROR("%s", e.what());
301  return 1;
302  }
303 
304  return 0;
305 }
306 
307 
ros::Subscriber color_image_sub_
Definition: color_laser.h:92
ros::Publisher color_laser_publisher_
Definition: color_laser.h:87
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::Image color_image_
Definition: color_laser.h:79
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_
Definition: color_laser.h:80
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSCPP_DECL void spin(Spinner &spinner)
void laserPointCloudCallback(sensor_msgs::PointCloud2::Ptr message)
ColorLaser(ros::NodeHandle &nh)
Definition: color_laser.cpp:48
ros::Subscriber laser_pointcloud_sub_
Definition: color_laser.h:93
void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &message)
uint32_t getNumPublishers() const
ros::NodeHandle node_handle_
Definition: color_laser.h:99
TFSIMD_FORCE_INLINE const tfScalar & x() const
void colorImageCallback(const sensor_msgs::Image::ConstPtr &message)
Definition: color_laser.cpp:99
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_
Definition: color_laser.h:94
sensor_msgs::PointCloud2 color_laser_pointcloud_
Definition: color_laser.h:82
#define ROS_ERROR(...)


multisense_ros
Author(s):
autogenerated on Wed Jan 8 2020 03:37:59