Commit cae12848 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

upside_down optional param

parent 7f30db39
...@@ -61,7 +61,7 @@ namespace wolf ...@@ -61,7 +61,7 @@ namespace wolf
class SubscriberLaser2d : public Subscriber class SubscriberLaser2d : public Subscriber
{ {
protected: protected:
bool laser_intrinsics_set_; bool laser_params_set_, laser_params_from_msg_, upside_down_;
laserscanutils::LaserScanParams params_; laserscanutils::LaserScanParams params_;
SensorLaser2dPtr sensor_laser_; SensorLaser2dPtr sensor_laser_;
...@@ -75,6 +75,10 @@ class SubscriberLaser2d : public Subscriber ...@@ -75,6 +75,10 @@ class SubscriberLaser2d : public Subscriber
void initialize(ros::NodeHandle& nh, const std::string& topic) override; void initialize(ros::NodeHandle& nh, const std::string& topic) override;
void callback(const sensor_msgs::LaserScan::ConstPtr& msg); void callback(const sensor_msgs::LaserScan::ConstPtr& msg);
protected:
void checkConsistentScanParams(const unsigned int& ranges_size);
}; };
WOLF_REGISTER_SUBSCRIBER(SubscriberLaser2d) WOLF_REGISTER_SUBSCRIBER(SubscriberLaser2d)
......
...@@ -28,13 +28,14 @@ namespace wolf ...@@ -28,13 +28,14 @@ namespace wolf
SubscriberLaser2d::SubscriberLaser2d(const std::string& _unique_name, SubscriberLaser2d::SubscriberLaser2d(const std::string& _unique_name,
const ParamsServer& _server, const ParamsServer& _server,
const SensorBasePtr _sensor_ptr) 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_); sensor_laser_ = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_);
assert(sensor_laser_ != nullptr && "SubscriberLaser2d: sensor provided is not of type SensorLaser2d"); assert(sensor_laser_ != nullptr && "SubscriberLaser2d: sensor provided is not of type SensorLaser2d");
laser_intrinsics_set_ = not _server.getParam<bool>(prefix_ + "/load_params_from_msg"); laser_params_from_msg_ = not _server.getParam<bool>(prefix_ + "/load_params_from_msg");
params_ = sensor_laser_->getScanParams(); upside_down_ = getParamWithDefault<bool>(_server, prefix_ + "/upside_down", false);
} }
...@@ -53,72 +54,83 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg) ...@@ -53,72 +54,83 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
return; return;
} }
CaptureLaser2dPtr new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec, // Set parameters if not set
msg->header.stamp.nsec), if (not laser_params_set_)
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_)
{ {
WOLF_DEBUG("SubscriberLaser2d: Loading params from msg:", // load from msg
"\n\tangle_min = ", msg->angle_min, if (laser_params_from_msg_)
"\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)
{ {
WOLF_WARN("SubscriberLaser2d: Loading params from msg:", WOLF_DEBUG("SubscriberLaser2d: Loading params from msg:",
"\n\tangle_min = ", params_.angle_min_, "\n\tangle_min = ", msg->angle_min,
"\n\tangle_max = ", params_.angle_max_, "\n\tangle_max = ", msg->angle_max,
"\n\tangle_increment = ", params_.angle_step_, "\n\tangle_increment = ", msg->angle_increment,
"\n\ttime_increment = ", params_.scan_time_, "\n\ttime_increment = ", msg->time_increment,
"\n\trange_min = ", params_.range_min_, "\n\trange_min = ", msg->range_min,
"\n\trange_max = ", params_.range_max_); "\n\trange_max = ", msg->range_max);
WOLF_WARN("SubscriberLaser2d: angle_min, angle_max, angle_increment not coherent with ranges.size(): \n", if (not upside_down_)
"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_, params_.angle_min_ = msg->angle_min;
" - Skipping capture..."); params_.angle_max_ = msg->angle_max;
params_.angle_step_ = (params_.angle_max_-params_.angle_min_)/(msg->ranges.size()-1); }
else
WOLF_WARN("new_angle_step_ = ", params_.angle_step_) {
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_); 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 else
{ {
if (std::abs(params_.angle_min_ - msg->angle_min) > 1e-4 or std::vector<float> ranges_reversed(msg->ranges.rbegin(), msg->ranges.rend());
std::abs(params_.angle_max_ - msg->angle_max) > 1e-4 or new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
std::abs(params_.angle_step_ - msg->angle_increment) > 1e-4 or msg->header.stamp.nsec),
std::abs(params_.range_min_ - msg->range_min) > 1e-4 or sensor_ptr_,
std::abs(params_.range_max_ - msg->range_max) > 1e-4) ranges_reversed);
{
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;
}
} }
new_capture->process(); 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_)
}
}
} }
Supports Markdown
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