diff --git a/include/subscriber_laser2d.h b/include/subscriber_laser2d.h
index 8b2454949f3ebfa315d26e52908ebca01d54597a..ddaec8f273ca5b20164c1478c8e307394b937fb8 100644
--- a/include/subscriber_laser2d.h
+++ b/include/subscriber_laser2d.h
@@ -41,6 +41,8 @@ class SubscriberLaser2d : public Subscriber
{
protected:
bool laser_intrinsics_set_;
+ laserscanutils::LaserScanParams params_;
+ SensorLaser2dPtr sensor_laser_;
public:
// Constructor
diff --git a/src/subscriber_laser2d.cpp b/src/subscriber_laser2d.cpp
index 603483a8dc21d0fea63cd691636f40a4006b5aa3..2feb5364bba322343012a751b5a6ea6091eebf5c 100644
--- a/src/subscriber_laser2d.cpp
+++ b/src/subscriber_laser2d.cpp
@@ -9,7 +9,11 @@ SubscriberLaser2d::SubscriberLaser2d(const std::string& _unique_name,
const SensorBasePtr _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr)
{
- laser_intrinsics_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();
}
@@ -22,7 +26,10 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
setLastStamp(msg->header.stamp);
- CaptureLaser2dPtr new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, msg->ranges);
+ CaptureLaser2dPtr new_capture = std::make_shared<CaptureLaser2d>(TimeStamp(msg->header.stamp.sec,
+ msg->header.stamp.nsec),
+ sensor_ptr_,
+ msg->ranges);
// auto n = std::min(msg->ranges.size(), new_capture->getScan().ranges_raw_.size());
// auto total = 0;
// for(int i = 0; i < n; ++i)
@@ -37,41 +44,76 @@ void SubscriberLaser2d::callback(const sensor_msgs::LaserScan::ConstPtr& msg)
// std::cout << "Total error " << total << "\n";
// auto sensor_laser_ptr = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_);
- // auto params = sensor_laser_ptr->getScanParams();
-
- // WOLF_INFO("MSG ", msg->angle_increment, " PARAMS ", params.angle_step_);
- // WOLF_INFO("MSG ", msg->angle_max, " PARAMS " , params.angle_max_);
- // WOLF_INFO("MSG ", msg->angle_min, " PARAMS " , params.angle_min_);
- // WOLF_INFO("MSG ", msg->range_max, " PARAMS " , params.range_max_);
- // WOLF_INFO("MSG ", msg->range_min, " PARAMS " , params.range_min_);
+ // params_ = sensor_laser_ptr->getScanParams();
+ // WOLF_INFO("MSG ", msg->angle_increment, " PARAMS ", params_.angle_step_);
+ // WOLF_INFO("MSG ", msg->angle_max, " PARAMS " , params_.angle_max_);
+ // WOLF_INFO("MSG ", msg->angle_min, " PARAMS " , params_.angle_min_);
+ // WOLF_INFO("MSG ", msg->range_max, " PARAMS " , params_.range_max_);
+ // WOLF_INFO("MSG ", msg->range_min, " PARAMS " , params_.range_min_);
+ // WOLF_INFO("CaptureLaser ranges: ", new_capture->getScan().ranges_raw_.size());
// std::cout << "Cap Seq " << new_capture->id() << " " << msg->header.seq << "\n";
+
//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 (!laser_intrinsics_set_)
+ if (not laser_intrinsics_set_)
{
- auto sensor_laser_ptr = std::dynamic_pointer_cast<SensorLaser2d>(sensor_ptr_);
- assert(sensor_laser_ptr != nullptr);
- laserscanutils::LaserScanParams params = sensor_laser_ptr->getScanParams();
-
- // params.angle_min_ = msg->angle_min;
- // params.angle_max_ = msg->angle_max;
- WOLF_DEBUG("angle min msg", msg->angle_min, " angle max msg", msg->angle_max);
- // params.angle_min_ = 0.4;
- // params.angle_max_ = 1.44;
- WOLF_DEBUG("angle min ", params.angle_min_, " angle max ", params.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;
- params.range_std_dev_ = 0.05; // TODO: get from param
- params.angle_std_dev_ = 0.05; // TODO: get from param
-
- sensor_laser_ptr->setScanParams(params);
+ 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;
+
+ sensor_laser_->setScanParams(params_);
laser_intrinsics_set_ = true;
- ROS_INFO("LASER 2d: Intrinsics parameters 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);
+
+ WOLF_INFO("SubscriberLaser2d: Parameters loaded from msg:",
+ "\n\tangle_min_ = ", params_.angle_min_,
+ "\n\tangle_max_ = ", params_.angle_max_,
+ "\n\tangle_step_ = ", params_.angle_step_,
+ "\n\tscan_time_ = ", params_.scan_time_,
+ "\n\trange_min_ = ", params_.range_min_,
+ "\n\trange_max_ = ", params_.range_max_,
+ "\n\trange_std_dev_ = ", params_.range_std_dev_, " (not taken from msg)",
+ "\n\tangle_std_dev_ = ", params_.angle_std_dev_, " (not taken from msg)");
+ }
+ // check coherent params
+ 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;
+ }
+ }
+
+ if (std::abs(msg->angle_min + (msg->ranges.size()-1) * msg->angle_increment - msg->angle_max) > msg->angle_increment / 2)
+ {
+ WOLF_ERROR("SubscriberLaser2d: angle_min, angle_max, angle_increment not coherent with ranges.size(): \n",
+ "msg->angle_min + (msg->ranges.size()-1) * msg->angle_increment - msg->angle_max = ",
+ msg->angle_min + (msg->ranges.size()-1) * msg->angle_increment - msg->angle_max,
+ " - Skipping capture...");
+ return;
}
new_capture->process();