34 #ifndef DEPTH_IMAGE_TO_LASERSCAN 35 #define DEPTH_IMAGE_TO_LASERSCAN 37 #include <sensor_msgs/Image.h> 38 #include <sensor_msgs/LaserScan.h> 66 sensor_msgs::LaserScanPtr
convert_msg(
const sensor_msgs::ImageConstPtr& depth_msg,
67 const sensor_msgs::CameraInfoConstPtr& info_msg);
154 bool use_point(
const float new_value,
const float old_value,
const float range_min,
const float range_max)
const;
171 const sensor_msgs::LaserScanPtr& scan_msg,
const int& scan_height)
const{
173 float center_x = cam_model.
cx();
174 float center_y = cam_model.
cy();
178 float constant_x = unit_scaling / cam_model.
fx();
179 float constant_y = unit_scaling / cam_model.
fy();
181 const T* depth_row =
reinterpret_cast<const T*
>(&depth_msg->data[0]);
182 int row_step = depth_msg->step /
sizeof(T);
184 int offset = (int)(cam_model.
cy()-scan_height/2);
185 depth_row += offset*row_step;
187 for(
int v = offset; v < offset+
scan_height_; v++, depth_row += row_step){
188 for (
int u = 0; u < (int)depth_msg->width; u++)
190 T depth = depth_row[u];
193 double th = -atan2((
double)(u - center_x) * constant_x, unit_scaling);
194 int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
198 double x = (u - center_x) * depth * constant_x;
202 r = sqrt(pow(x, 2.0) + pow(z, 2.0));
206 if(
use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
207 scan_msg->ranges[index] = r;
sensor_msgs::LaserScanPtr convert_msg(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
void set_output_frame(const std::string output_frame_id)
image_geometry::PinholeCameraModel cam_model_
image_geometry helper class for managing sensor_msgs/CameraInfo messages.
void convert(const sensor_msgs::ImageConstPtr &depth_msg, const image_geometry::PinholeCameraModel &cam_model, const sensor_msgs::LaserScanPtr &scan_msg, const int &scan_height) const
double angle_between_rays(const cv::Point3d &ray1, const cv::Point3d &ray2) const
bool use_point(const float new_value, const float old_value, const float range_min, const float range_max) const
float scan_time_
Stores the time between scans.
void set_scan_height(const int scan_height)
float range_min_
Stores the current minimum range to use.
float range_max_
Stores the current maximum range to use.
int scan_height_
Number of pixel rows to use when producing a laserscan from an area.
void set_scan_time(const float scan_time)
double magnitude_of_ray(const cv::Point3d &ray) const
void set_range_limits(const float range_min, const float range_max)
std::string output_frame_id_
Output frame_id for each laserscan. This is likely NOT the camera's frame_id.