我参照了下面这个链接，https://blog.csdn.net/qq_39266065/article/details/108714973?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-12.control#commentBox
对代码进行配置修改，但是修改完运行后就出现如下错误：
但是我找不到问题所在。我的代码如下所示：
1、DepthImageToLaserScanROS.h
#ifndef DEPTH_IMAGE_TO_LASERSCAN_ROS
#define DEPTH_IMAGE_TO_LASERSCAN_ROS
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <boost/thread/mutex.hpp>
#include <dynamic_reconfigure/server.h>
#include <depthimage_to_laserscan/DepthConfig.h>
#include <depthimage_to_laserscan/DepthImageToLaserScan.h>
namespace depthimage_to_laserscan
{
class DepthImageToLaserScanROS
{
public:
DepthImageToLaserScanROS(ros::NodeHandle& n, ros::NodeHandle& pnh);
~DepthImageToLaserScanROS();
private:
/**
* Callback for image_transport
*
* Synchronized callback for depth image and camera info. Publishes laserscan at the end of this callback.
*
* @param depth_msg Image provided by image_transport.
* @param info_msg CameraInfo provided by image_transport.
*
*/
void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg);
/**
* Callback that is called when there is a new subscriber.
*
* Will not subscribe to the image and camera_info until we have a subscriber for our LaserScan (lazy subscribing).
*
*/
void connectCb(const ros::SingleSubscriberPublisher& pub);
/**
* Callback called when a subscriber unsubscribes.
*
* If all current subscribers of our LaserScan stop listening, stop subscribing (lazy subscribing).
*
*/
void disconnectCb(const ros::SingleSubscriberPublisher& pub);
/**
* Dynamic reconfigure callback.
*
* Callback that is used to set each of the parameters insde the DepthImageToLaserScan object.
*
* @param config Dynamic Reconfigure object.
* @param level Dynamic Reconfigure level.
*
*/
void reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level);
ros::NodeHandle pnh_; ///< Private nodehandle used to generate the transport hints in the connectCb.
image_transport::ImageTransport it_; ///< Subscribes to synchronized Image CameraInfo pairs.
image_transport::CameraSubscriber sub_; ///< Subscriber for image_transport
ros::Publisher pub_; ///< Publisher for output LaserScan messages
dynamic_reconfigure::Server<depthimage_to_laserscan::DepthConfig> srv_; ///< Dynamic reconfigure server
depthimage_to_laserscan::DepthImageToLaserScan dtl_; ///< Instance of the DepthImageToLaserScan conversion class.
boost::mutex connect_mutex_; ///< Prevents the connectCb and disconnectCb from being called until everything is initialized.
};
}; // depthimage_to_laserscan
#endif
2、DepthImageToLaserScan.h
#ifndef DEPTH_IMAGE_TO_LASERSCAN
#define DEPTH_IMAGE_TO_LASERSCAN
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>
#include <depthimage_to_laserscan/depth_traits.h>
#include <sstream>
#include <limits.h>
#include <math.h>
namespace depthimage_to_laserscan
{
class DepthImageToLaserScan
{
public:
DepthImageToLaserScan();
~DepthImageToLaserScan();
/**
* Converts the information in a depth image (sensor_msgs::Image) to a sensor_msgs::LaserScan.
*
* This function converts the information in the depth encoded image (UInt16 or Float32 encoding) into
* a sensor_msgs::LaserScan as accurately as possible. To do this, it requires the synchornized Image/CameraInfo
* pair associated with the image.
*
* @param depth_msg UInt16 or Float32 encoded depth image.
* @param info_msg CameraInfo associated with depth_msg
* @return sensor_msgs::LaserScanPtr for the center row(s) of the depth image.
*
*/
sensor_msgs::LaserScanPtr convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg);
/**
* Sets the scan time parameter.
*
* This function stores the desired value for scan_time. In sensor_msgs::LaserScan, scan_time is defined as
* "time between scans [seconds]". This value is not easily calculated from consquetive messages, and is thus
* left to the user to set correctly.
*
* @param scan_time The value to use for outgoing sensor_msgs::LaserScan.
*
*/
void set_scan_time(const float scan_time);
/**
* Sets the minimum and maximum range for the sensor_msgs::LaserScan.
*
* range_min is used to determine how close of a value to allow through when multiple radii correspond to the same
* angular increment. range_max is used to set the output message.
*
* @param range_min Minimum range to assign points to the laserscan, also minimum range to use points in the output scan.
* @param range_max Maximum range to use points in the output scan.
*
*/
void set_range_limits(const float range_min, const float range_max);
/**
* Sets the number of image rows to use in the output LaserScan.
*
* scan_height is the number of rows (pixels) to use in the output. This will provide scan_height number of radii for each
* angular increment. The output scan will output the closest radius that is still not smaller than range_min. This function
* can be used to vertically compress obstacles into a single LaserScan.
*
* @param scan_height Number of pixels centered around the center of the image to compress into the LaserScan.
*
*/
void set_scan_height(const int scan_height);
/**
* Sets the frame_id for the output LaserScan.
*
* Output frame_id for the LaserScan. Will probably NOT be the same frame_id as the depth image.
* Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while the camera uses 'camera_depth_optical_frame'.
*
* @param output_frame_id Frame_id to use for the output sensor_msgs::LaserScan.
*
*/
void set_output_frame(const std::string& output_frame_id);
private:
/**
* Computes euclidean length of a cv::Point3d (as a ray from origin)
*
* This function computes the length of a cv::Point3d assumed to be a vector starting at the origin (0,0,0).
*
* @param ray The ray for which the magnitude is desired.
* @return Returns the magnitude of the ray.
*
*/
double magnitude_of_ray(const cv::Point3d& ray) const;
/**
* Computes the angle between two cv::Point3d
*
* Computes the angle of two cv::Point3d assumed to be vectors starting at the origin (0,0,0).
* Uses the following equation: angle = arccos(a*b/(|a||b|)) where a = ray1 and b = ray2.
*
* @param ray1 The first ray
* @param ray2 The second ray
* @return The angle between the two rays (in radians)
*
*/
double angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const;
/**
* Determines whether or not new_value should replace old_value in the LaserScan.
*
* Uses the values of range_min, and range_max to determine if new_value is a valid point. Then it determines if
* new_value is 'more ideal' (currently shorter range) than old_value.
*
* @param new_value The current calculated range.
* @param old_value The current range in the output LaserScan.
* @param range_min The minimum acceptable range for the output LaserScan.
* @param range_max The maximum acceptable range for the output LaserScan.
* @return If true, insert new_value into the output LaserScan.
*
*/
bool use_point(const float new_value, const float old_value, const float range_min, const float range_max) const;
/**
* Converts the depth image to a laserscan using the DepthTraits to assist.
*
* This uses a method to inverse project each pixel into a LaserScan angular increment. This method first projects the pixel
* forward into Cartesian coordinates, then calculates the range and angle for this point. When multiple points coorespond to
* a specific angular measurement, then the shortest range is used.
*
* @param depth_msg The UInt16 or Float32 encoded depth message.
* @param cam_model The image_geometry camera model for this image.
* @param scan_msg The output LaserScan.
* @param scan_height The number of vertical pixels to feed into each angular_measurement.
*
*/
template<typename T>
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{
// Use correct principal point from calibration
const float center_x = cam_model.cx();
const float center_y = cam_model.cy();
// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
const double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
const float constant_x = unit_scaling / cam_model.fx();
const float constant_y = unit_scaling / cam_model.fy(); // line 181
const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
const int row_step = depth_msg->step / sizeof(T);
const int offset = (int)(center_y - scan_height/2);
depth_row += offset*row_step; // Offset to center of image
for(int v = offset; v < offset+scan_height_; ++v, depth_row += row_step){
for (int u = 0; u < (int)depth_msg->width; ++u) // Loop over each pixel in row
{
const T depth = depth_row[u];
double r = depth; // Assign to pass through NaNs and Infs
const double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
const int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
// Calculate in XYZ
double x = (u - center_x) * depth * constant_x;
double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
// Calculate actual distance
r = hypot(x, z);
double y = (v - center_y) * depth * constant_y; //line 205
if(y<ythresh_min_||y>ythresh_max_)
{
r = std::numeric_limits<float>::quiet_NaN();
continue;
}
}
// Determine if this point should be used.
if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
scan_msg->ranges[index] = r;
}
}
}
}
image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages.
float scan_time_; ///< Stores the time between scans.
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.
std::string output_frame_id_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id.
};
}; // depthimage_to_laserscan
#endif
3、DepthImageToLaserScan.cpp
#include <depthimage_to_laserscan/DepthImageToLaserScan.h>
using namespace depthimage_to_laserscan;
DepthImageToLaserScan::DepthImageToLaserScan()
: scan_time_(1./30.)
, range_min_(0.45)
, range_max_(10.0)
, scan_height_(1)
{
}
DepthImageToLaserScan::~DepthImageToLaserScan(){
void set_y_thresh(const float ythresh_min, const float ythresh_max);
float ythresh_min_;
float ythresh_max_;
}
double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{
return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0));
}
double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const{
const double dot_product = ray1.x*ray2.x + ray1.y*ray2.y + ray1.z*ray2.z;
const double magnitude1 = magnitude_of_ray(ray1);
const double magnitude2 = magnitude_of_ray(ray2);;
return acos(dot_product / (magnitude1 * magnitude2));
}
bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{
// Check for NaNs and Infs, a real number within our limits is more desirable than these.
const bool new_finite = std::isfinite(new_value);
const bool old_finite = std::isfinite(old_value);
// Infs are preferable over NaNs (more information)
if(!new_finite && !old_finite){ // Both are not NaN or Inf.
if(!std::isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
return true;
}
return false; // Do not replace old_value
}
// If not in range, don't bother
const bool range_check = range_min <= new_value && new_value <= range_max;
if(!range_check){
return false;
}
if(!old_finite){ // New value is in range and finite, use it.
return true;
}
// Finally, if they are both numerical and new_value is closer than old_value, use new_value.
const bool shorter_check = new_value < old_value;
return shorter_check;
}
sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg){
// Set camera model
cam_model_.fromCameraInfo(info_msg);
// Calculate angle_min and angle_max by measuring angles between the left ray, right ray, and optical center ray
cv::Point2d raw_pixel_left(0, cam_model_.cy());
cv::Point2d rect_pixel_left = cam_model_.rectifyPoint(raw_pixel_left);
cv::Point3d left_ray = cam_model_.projectPixelTo3dRay(rect_pixel_left);
cv::Point2d raw_pixel_right(depth_msg->width-1, cam_model_.cy());
cv::Point2d rect_pixel_right = cam_model_.rectifyPoint(raw_pixel_right);
cv::Point3d right_ray = cam_model_.projectPixelTo3dRay(rect_pixel_right);
cv::Point2d raw_pixel_center(cam_model_.cx(), cam_model_.cy());
cv::Point2d rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center);
cv::Point3d center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center);
const double angle_max = angle_between_rays(left_ray, center_ray);
const double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image
// Fill in laserscan message
sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
scan_msg->header = depth_msg->header;
if(output_frame_id_.length() > 0){
scan_msg->header.frame_id = output_frame_id_;
}
scan_msg->angle_min = angle_min;
scan_msg->angle_max = angle_max;
scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (depth_msg->width - 1);
scan_msg->time_increment = 0.0;
scan_msg->scan_time = scan_time_;
scan_msg->range_min = range_min_;
scan_msg->range_max = range_max_;
// Check scan_height vs image_height
if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){
std::stringstream ss;
ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height.";
throw std::runtime_error(ss.str());
}
// Calculate and fill the ranges
const uint32_t ranges_size = depth_msg->width;
scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());
if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
{
convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
}
else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
}
else
{
std::stringstream ss;
ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
throw std::runtime_error(ss.str());
}
return scan_msg;
}
void DepthImageToLaserScan::set_scan_time(const float scan_time){
scan_time_ = scan_time;
}
void DepthImageToLaserScan::set_range_limits(const float range_min, const float range_max){
range_min_ = range_min;
range_max_ = range_max;
}
void DepthImageToLaserScan::set_scan_height(const int scan_height){
scan_height_ = scan_height;
}
void DepthImageToLaserScan::set_output_frame(const std::string& output_frame_id){
output_frame_id_ = output_frame_id;
}
void DepthImageToLaserScan::set_y_thresh(const float ythresh_min, const float ythresh_max){
ythresh_min_ = ythresh_min;
ythresh_max_ = ythresh_max;
}
4、DepthImageToLaserScanROS.cpp
#include <depthimage_to_laserscan/DepthImageToLaserScanROS.h>
using namespace depthimage_to_laserscan;
DepthImageToLaserScanROS::DepthImageToLaserScanROS(ros::NodeHandle& n, ros::NodeHandle& pnh):pnh_(pnh), it_(n), srv_(pnh) {
boost::mutex::scoped_lock lock(connect_mutex_);
// Dynamic Reconfigure
dynamic_reconfigure::Server<depthimage_to_laserscan::DepthConfig>::CallbackType f;
f = boost::bind(&DepthImageToLaserScanROS::reconfigureCb, this, _1, _2);
srv_.setCallback(f);
// Lazy subscription to depth image topic
pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1));
}
DepthImageToLaserScanROS::~DepthImageToLaserScanROS(){
sub_.shutdown();
}
void DepthImageToLaserScanROS::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg){
try
{
sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg);
pub_.publish(scan_msg);
}
catch (std::runtime_error& e)
{
ROS_ERROR_THROTTLE(1.0, "Could not convert depth image to laserscan: %s", e.what());
}
}
void DepthImageToLaserScanROS::connectCb(const ros::SingleSubscriberPublisher& pub) {
boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) {
ROS_DEBUG("Connecting to depth topic.");
image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
sub_ = it_.subscribeCamera("image", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
}
}
void DepthImageToLaserScanROS::disconnectCb(const ros::SingleSubscriberPublisher& pub) {
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) {
ROS_DEBUG("Unsubscribing from depth topic.");
sub_.shutdown();
}
}
void DepthImageToLaserScanROS::reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level){
dtl_.set_scan_time(config.scan_time);
dtl_.set_range_limits(config.range_min, config.range_max);
dtl_.set_scan_height(config.scan_height);
dtl_.set_output_frame(config.output_frame_id);
dtl_.set_y_thresh(config.ythresh_min, config.ythresh_max);
}
