diff --git a/include/subscriber_laser2d.h b/include/subscriber_laser2d.h
index 02a7b6167abe811a056c43c599e4ebec66d6e872..d2be96ca55137372e554364fa10acfed077cb7d7 100644
--- a/include/subscriber_laser2d.h
+++ b/include/subscriber_laser2d.h
@@ -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);
};
}
diff --git a/src/subscriber_laser2d.cpp b/src/subscriber_laser2d.cpp
index 40b9fdfaf1fa39ff5931628d0b28ff42a8eda919..6bf2130194cc3c25681cf87bf9a38a317cb2ea97 100644
--- a/src/subscriber_laser2d.cpp
+++ b/src/subscriber_laser2d.cpp
@@ -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,73 +54,84 @@ 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_)
+ }
+}
+
WOLF_REGISTER_SUBSCRIBER(SubscriberLaser2d)
}