diff --git a/README.md b/README.md
index 80b53fb40c6b8bf730b424dc9ab6d6b80a8cfa24..d75a0a4f0642fedeca4aef8ed3b786af19185cd6 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,3 @@
-# wolf_ros_laser
+# WOLF ROS Laser Package
+For installation guide and code documentation, please visit the [documentation website](http://www.iri.upc.edu/wolf).
diff --git a/include/publisher_falko.h b/include/publisher_falko.h
index 7ea67d1d9da140c46ce0a5a8faae650e1b77bed5..e4a965e387773e06fad6bedadb6c9cfdd396a925 100644
--- a/include/publisher_falko.h
+++ b/include/publisher_falko.h
@@ -98,7 +98,6 @@ class PublisherFalko : public Publisher
std::map<FrameBasePtr, VectorComposite> map_frame_states;
};
-WOLF_REGISTER_PUBLISHER(PublisherFalko)
} // namespace wolf
#endif
diff --git a/include/publisher_laser_map.h b/include/publisher_laser_map.h
index f8ab4878ed54113b62d074c46dbfe5335f789096..1af60603fbdc5e6d76f042ec00ead531066540af 100644
--- a/include/publisher_laser_map.h
+++ b/include/publisher_laser_map.h
@@ -43,7 +43,8 @@
#include "publisher.h"
-using namespace wolf;
+namespace wolf
+{
struct ScanData
{
@@ -127,4 +128,5 @@ inline Eigen::Vector2i PublisherLaserMap::vectorToCell(const Eigen::DenseBase<T>
return cell;
}
+}
#endif
diff --git a/include/publisher_odom_icp.h b/include/publisher_odom_icp.h
index ebc230b5b8cdd1b2eda6f657eedeb870911a2baa..1c60816d10d6d4d37b3bd5683c6da02e526b4550 100644
--- a/include/publisher_odom_icp.h
+++ b/include/publisher_odom_icp.h
@@ -60,7 +60,6 @@ class PublisherOdomIcp: public Publisher
};
-WOLF_REGISTER_PUBLISHER(PublisherOdomIcp)
}
#endif
diff --git a/include/subscriber_laser2d.h b/include/subscriber_laser2d.h
index 284ed3d1d7228705f77ebc8dbe05c2d1504e91f6..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,8 +75,11 @@ 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)
}
#endif
diff --git a/src/publisher_falko.cpp b/src/publisher_falko.cpp
index 0d1de4fb36c596929f6316e15ee9d4a8faad96a1..53481413f881c738c5812eb1e8458775f2b33a2e 100644
--- a/src/publisher_falko.cpp
+++ b/src/publisher_falko.cpp
@@ -377,4 +377,5 @@ void PublisherFalko::getPosition(Eigen::Vector3d &p, Eigen::Quaterniond &q, Vect
Eigen::AngleAxisd(_state['O'](0) + sensor_->getO()->getState()(0), Eigen::Vector3d::UnitZ()));
}
+WOLF_REGISTER_PUBLISHER(PublisherFalko)
} // namespace wolf
diff --git a/src/publisher_laser_map.cpp b/src/publisher_laser_map.cpp
index e5df8118bc89f29c2a5e70a39e781ea794524cff..e1bb8019fae198e6739ce5a82e865303ec9aa1a3 100644
--- a/src/publisher_laser_map.cpp
+++ b/src/publisher_laser_map.cpp
@@ -31,6 +31,9 @@
#include <limits>
+namespace wolf
+{
+
PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
const ParamsServer& _server,
const ProblemPtr _problem) :
@@ -521,3 +524,4 @@ void PublisherLaserMap::updatePcMsg()
}
WOLF_REGISTER_PUBLISHER(PublisherLaserMap)
+}
\ No newline at end of file
diff --git a/src/publisher_odom_icp.cpp b/src/publisher_odom_icp.cpp
index e0a9aa7a7b59abce61ad50a6dd4e50acffc92df6..c9ed4ffb0c6b8af35be2c3ee82b1454227f7b456 100644
--- a/src/publisher_odom_icp.cpp
+++ b/src/publisher_odom_icp.cpp
@@ -94,4 +94,5 @@ void PublisherOdomIcp::publishDerived()
publisher_.publish(msg_);
}
+WOLF_REGISTER_PUBLISHER(PublisherOdomIcp)
}
diff --git a/src/subscriber_laser2d.cpp b/src/subscriber_laser2d.cpp
index 973a4753a2be3add69220154266daac4458563de..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,72 +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)
}