Skip to content
Snippets Groups Projects
Commit cae12848 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

upside_down optional param

parent 7f30db39
No related branches found
No related tags found
1 merge request!4new release
Hide whitespace changes
Inline Side-by-side
include/subscriber_laser2d.h
+ 5
− 1
View file @ cae12848
......@@ -61,7 +61,7 @@ namespace wolf
class SubscriberLaser2d : public Subscriber
{
protected:
bool laser_intrinsics_set_;
bool laser_params_set_, laser_params_from_msg_, upside_down_;
laserscanutils::LaserScanParams params_;
SensorLaser2dPtr sensor_laser_;
......@@ -75,6 +75,10 @@ class SubscriberLaser2d : public Subscriber
void initialize(ros::NodeHandle& nh, const std::string& topic) override;
void callback(const sensor_msgs::LaserScan::ConstPtr& msg);
protected:
void checkConsistentScanParams(const unsigned int& ranges_size);
};
WOLF_REGISTER_SUBSCRIBER(SubscriberLaser2d)
......
src/subscriber_laser2d.cpp
+ 72
− 60
View file @ cae12848
......@@ -28,13 +28,14 @@ namespace wolf
SubscriberLaser2d::SubscriberLaser2d(const std::string& _unique_name,
const ParamsServer& _server,
const SensorBasePtr _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr),
laser_params_set_(false)
{
sensor_laser_ = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_);
assert(sensor_laser_ != nullptr && "SubscriberLaser2d: sensor provided is not of type SensorLaser2d");
laser_intrinsics_set_ = not _server.getParam<bool>(prefix_ + "/load_params_from_msg");
params_ = sensor_laser_->getScanParams();
laser_params_from_msg_ = not _server.getParam<bool>(prefix_ + "/load_params_from_msg");
upside_down_ = getParamWithDefault<bool>(_server, prefix_ + "/upside_down", false);
}
......@@ -53,72 +54,83 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
return;
}
CaptureLaser2dPtr new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
msg->header.stamp.nsec),
sensor_ptr_,
msg->ranges);
//Currently this line is just to bypass the ROS "auto config". from the ROS msg. Maybe we want to explore
//getting the params from the msg instead of the yaml file in the future.
//laser_intrinsics_set_ = true;
if (not laser_intrinsics_set_)
// Set parameters if not set
if (not laser_params_set_)
{
WOLF_DEBUG("SubscriberLaser2d: Loading params from msg:",
"\n\tangle_min = ", msg->angle_min,
"\n\tangle_max = ", msg->angle_max,
"\n\tangle_increment = ", msg->angle_increment,
"\n\ttime_increment = ", msg->time_increment,
"\n\trange_min = ", msg->range_min,
"\n\trange_max = ", msg->range_max);
params_.angle_min_ = msg->angle_min;
params_.angle_max_ = msg->angle_max;
params_.angle_step_ = msg->angle_increment;
params_.scan_time_ = msg->time_increment;
params_.range_min_ = msg->range_min;
params_.range_max_ = msg->range_max;
if (std::abs(params_.angle_min_ + (msg->ranges.size()-1) * params_.angle_step_ - params_.angle_max_) > params_.angle_step_ / 2)
// load from msg
if (laser_params_from_msg_)
{
WOLF_WARN("SubscriberLaser2d: Loading params from msg:",
"\n\tangle_min = ", params_.angle_min_,
"\n\tangle_max = ", params_.angle_max_,
"\n\tangle_increment = ", params_.angle_step_,
"\n\ttime_increment = ", params_.scan_time_,
"\n\trange_min = ", params_.range_min_,
"\n\trange_max = ", params_.range_max_);
WOLF_WARN("SubscriberLaser2d: angle_min, angle_max, angle_increment not coherent with ranges.size(): \n",
"params_.angle_min_ + (msg->ranges.size()-1) * params_.angle_step_ - params_.angle_max_ = ",
params_.angle_min_ + (msg->ranges.size()-1) * params_.angle_step_ - params_.angle_max_,
" - Skipping capture...");
params_.angle_step_ = (params_.angle_max_-params_.angle_min_)/(msg->ranges.size()-1);
WOLF_WARN("new_angle_step_ = ", params_.angle_step_)
WOLF_DEBUG("SubscriberLaser2d: Loading params from msg:",
"\n\tangle_min = ", msg->angle_min,
"\n\tangle_max = ", msg->angle_max,
"\n\tangle_increment = ", msg->angle_increment,
"\n\ttime_increment = ", msg->time_increment,
"\n\trange_min = ", msg->range_min,
"\n\trange_max = ", msg->range_max);
if (not upside_down_)
{
params_.angle_min_ = msg->angle_min;
params_.angle_max_ = msg->angle_max;
}
else
{
params_.angle_min_ = -msg->angle_max;
params_.angle_max_ = -msg->angle_min;
}
params_.angle_step_ = msg->angle_increment;
params_.scan_time_ = msg->time_increment;
params_.range_min_ = msg->range_min;
params_.range_max_ = msg->range_max;
}
// load from sensor
else
params_ = sensor_laser_->getScanParams();
// check consistency between angles and ranges size
checkConsistentScanParams(msg->ranges.size());
sensor_laser_->setScanParams(params_);
laser_intrinsics_set_ = true;
laser_params_set_ = true;
}
// check coherent params
// Create and process capture
CaptureLaser2dPtr new_capture;
if (not upside_down_)
new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
msg->header.stamp.nsec),
sensor_ptr_,
msg->ranges);
else
{
if (std::abs(params_.angle_min_ - msg->angle_min) > 1e-4 or
std::abs(params_.angle_max_ - msg->angle_max) > 1e-4 or
std::abs(params_.angle_step_ - msg->angle_increment) > 1e-4 or
std::abs(params_.range_min_ - msg->range_min) > 1e-4 or
std::abs(params_.range_max_ - msg->range_max) > 1e-4)
{
WOLF_ERROR("SubscriberLaser2d: Any param in msg different than sensor param! Skipping capture...");
WOLF_ERROR_COND(std::abs(params_.angle_min_ - msg->angle_min) > 1e-4, "\tmsg.angle_min: ", msg->angle_min, " - params_.angle_min_: ", params_.angle_min_, " - error: ", std::abs(params_.angle_min_ - msg->angle_min));
WOLF_ERROR_COND(std::abs(params_.angle_max_ - msg->angle_max) > 1e-4, "\tmsg.angle_max: ", msg->angle_max, " - params_.angle_max_: ", params_.angle_max_, " - error: ", std::abs(params_.angle_max_ - msg->angle_max));
WOLF_ERROR_COND(std::abs(params_.angle_step_- msg->angle_increment) > 1e-4, "\tmsg.angle_increment: ", msg->angle_increment, " - params_.angle_step_: ", params_.angle_step_, " - error: ", std::abs(params_.angle_step_ - msg->angle_increment));
WOLF_ERROR_COND(std::abs(params_.range_min_ - msg->range_min) > 1e-9, "\tmsg.range_min: ", msg->range_min, " - params_.range_min_: ", params_.range_min_, " - error: ", std::abs(params_.range_min_ - msg->range_min));
WOLF_ERROR_COND(std::abs(params_.range_max_ - msg->range_max) > 1e-9, "\tmsg.range_max: ", msg->range_max, " - params_.range_max_: ", params_.range_max_, " - error: ", std::abs(params_.range_max_ - msg->range_max));
return;
}
std::vector<float> ranges_reversed(msg->ranges.rbegin(), msg->ranges.rend());
new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
msg->header.stamp.nsec),
sensor_ptr_,
ranges_reversed);
}
new_capture->process();
}
void SubscriberLaser2d::checkConsistentScanParams(const unsigned int& ranges_size)
{
if (std::abs(params_.angle_min_ + (ranges_size-1) * params_.angle_step_ - params_.angle_max_) > params_.angle_step_ / 2)
{
WOLF_WARN("SubscriberLaser2d: Loaded params:",
"\n\tangle_min = ", params_.angle_min_,
"\n\tangle_max = ", params_.angle_max_,
"\n\tangle_increment = ", params_.angle_step_,
"\n\ttime_increment = ", params_.scan_time_,
"\n\trange_min = ", params_.range_min_,
"\n\trange_max = ", params_.range_max_);
WOLF_WARN("SubscriberLaser2d: angle_min, angle_max, angle_increment not coherent with ranges.size(): \n",
"params_.angle_min_ + (msg->ranges.size()-1) * params_.angle_step_ - params_.angle_max_ = ",
params_.angle_min_ + (ranges_size-1) * params_.angle_step_ - params_.angle_max_,
" - computing new consistent angle_step...");
params_.angle_step_ = (params_.angle_max_-params_.angle_min_)/(ranges_size-1);
WOLF_WARN("new_angle_step_ = ", params_.angle_step_)
}
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment