Merge branch 'refactor/publish-data-ros' into 'master'

refactor: Move all publish_data calls to the ROS wrapper

Closes #50

See merge request ruvu/mcl!71

ruvu_mcl/CMakeLists.txt

@@ -92,8 +92,11 @@ add_library(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+# bug in erf_inv_initializer under valgrind
+# https://svn.boost.org/trac10/ticket/10005
+target_compile_definitions(${PROJECT_NAME} PRIVATE BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS)
 target_compile_warnings(${PROJECT_NAME} ${EXTRA_FLAGS})
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 add_executable(node
   src/main.cpp

ruvu_mcl/src/cloud_publisher.cpp

@@ -4,6 +4,7 @@
 
 #include <algorithm>
 #include <utility>
+#include <vector>
 
 #include "./particle_filter.hpp"
 #include "ros/node_handle.h"
@@ -17,7 +18,7 @@ CloudPublisher::CloudPublisher(ros::NodeHandle nh, ros::NodeHandle private_nh)
 {
 }
 
-void CloudPublisher::publish(const std_msgs::Header & header, const ParticleFilter & pf)
+void CloudPublisher::publish(const std_msgs::Header & header, const std::vector<Particle> & pf)
 {
   constexpr double length = 0.1;
   constexpr double width = 0.02;
@@ -30,14 +31,14 @@ void CloudPublisher::publish(const std_msgs::Header & header, const ParticleFilt
   m.action = visualization_msgs::Marker::MODIFY;
   m.pose.orientation = tf2::toMsg(tf2::Quaternion::getIdentity());
   m.scale.x = width;
-  m.points.reserve(pf.particles.size() * 2);
-  m.colors.reserve(pf.particles.size() * 2);
+  m.points.reserve(pf.size() * 2);
+  m.colors.reserve(pf.size() * 2);
 
   double max_weight = 0;
-  for (const auto & p : pf.particles) max_weight = std::max(max_weight, p.weight);
+  for (const auto & p : pf) max_weight = std::max(max_weight, p.weight);
 
   tf2::Vector3 p1{length / 2, 0, 0};
-  for (const auto & particle : pf.particles) {
+  for (const auto & particle : pf) {
     std_msgs::ColorRGBA c;
     c.a = particle.weight / max_weight;
     c.b = 1;

ruvu_mcl/src/cloud_publisher.hpp

@@ -2,12 +2,14 @@
 
 #pragma once
 
+#include <vector>
+
 #include "ros/message_forward.h"
 #include "ros/publisher.h"
 
 namespace ros
 {
-class NodeHandle;
+class Particle;
 }
 namespace std_msgs
 {
@@ -16,13 +18,13 @@ ROS_DECLARE_MESSAGE(Header)
 
 namespace ruvu_mcl
 {
-class ParticleFilter;
+class Particle;
 
 class CloudPublisher
 {
 public:
   CloudPublisher(ros::NodeHandle nh, ros::NodeHandle private_nh);
-  void publish(const std_msgs::Header & header, const ParticleFilter & pf);
+  void publish(const std_msgs::Header & header, const std::vector<Particle> & pf);
 
 private:
   ros::Publisher cloud_pub_;

ruvu_mcl/src/mcl.cpp

@@ -2,7 +2,6 @@
 
 #include "./mcl.hpp"
 
-#include <algorithm>
 #include <memory>
 
 #include "./adaptive/fixed.hpp"
@@ -17,7 +16,6 @@
 #include "./sensor_models/landmark_likelihood_field_model.hpp"
 #include "./sensor_models/likelihood_field_model.hpp"
 #include "ros/node_handle.h"
-#include "std_msgs/UInt32.h"
 #include "tf2/utils.h"
 
 constexpr auto name = "mcl";
@@ -37,12 +35,10 @@ std::unique_ptr<Laser> create_laser_model(
 }
 
 Mcl::Mcl(ros::NodeHandle nh, ros::NodeHandle private_nh)
-: cloud_pub_(nh, private_nh),
-  count_pub_(private_nh.advertise<std_msgs::UInt32>("count", 1)),
-  pose_pub_(private_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1)),
-  config_(),
+: config_(),
   rng_(std::make_unique<Rng>()),
   last_odom_pose_(),
+  last_filter_update_(),
   filter_(),
   model_(nullptr),
   map_(nullptr),
@@ -80,13 +76,8 @@ void Mcl::configure(const Config & config)
     throw std::logic_error("no adaptive algorithm configured");
 
   if (filter_.particles.empty()) {
-    auto p = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
-    p->header.stamp = ros::Time::now();
-    p->header.frame_id = config_.global_frame_id;
-    tf2::toMsg(config.initial_pose.getOrigin(), p->pose.pose.position);
-    p->pose.pose.orientation = tf2::toMsg(config.initial_pose.getRotation());
-    std::copy(config.initial_cov.begin(), config.initial_cov.end(), p->pose.covariance.begin());
-    initial_pose_cb(p);
+    PoseWithCovariance p{config.initial_pose, config.initial_cov};
+    initial_pose_cb(ros::Time::now(), p);
   }
 }
 
@@ -122,13 +113,7 @@ bool Mcl::scan_cb(const LaserData & scan, const tf2::Transform & odom_pose)
       resampler_->resample(&filter_, needed_particles);
   }
 
-  // Create output
-  auto ps = filter_.get_pose_with_covariance_stamped(scan.header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = scan.header.stamp;
+  last_filter_update_ = scan.header.stamp;
   return true;
 }
 
@@ -168,14 +153,7 @@ bool Mcl::landmark_cb(const LandmarkList & landmarks, const tf2::Transform & odo
       resampler_->resample(&filter_, needed_particles);
   }
 
-  // Create output
-  auto ps =
-    filter_.get_pose_with_covariance_stamped(landmarks.header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = landmarks.header.stamp;
+  last_filter_update_ = landmarks.header.stamp;
   return true;
 }
 
@@ -193,16 +171,17 @@ void Mcl::landmark_list_cb(const LandmarkList & landmarks)
   landmark_model_ = nullptr;
 }
 
-void Mcl::initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose)
+void Mcl::initial_pose_cb(const ros::Time & stamp, const PoseWithCovariance & initial_pose)
 {
-  const auto & p = initial_pose->pose;
+  const auto & p = initial_pose.pose;
+  const auto & covariance = initial_pose.covariance;
   ROS_INFO_NAMED(
-    name, "initial pose received %.3f %.3f %.3f, spawning %lu new particles", p.pose.position.x,
-    p.pose.position.y, tf2::getYaw(p.pose.orientation), config_.max_particles);
+    name, "initial pose received %.3f %.3f %.3f, spawning %lu new particles", p.getOrigin().x(),
+    p.getOrigin().y(), tf2::getYaw(p.getRotation()), config_.max_particles);
 
-  auto dx = rng_->normal_distribution(p.pose.position.x, p.covariance[0 * 6 + 0]);
-  auto dy = rng_->normal_distribution(p.pose.position.y, p.covariance[1 * 6 + 1]);
-  auto dt = rng_->normal_distribution(tf2::getYaw(p.pose.orientation), p.covariance[5 * 6 + 5]);
+  auto dx = rng_->normal_distribution(p.getOrigin().x(), covariance[0 * 6 + 0]);
+  auto dy = rng_->normal_distribution(p.getOrigin().y(), covariance[1 * 6 + 1]);
+  auto dt = rng_->normal_distribution(tf2::getYaw(p.getRotation()), covariance[5 * 6 + 5]);
 
   filter_.particles.clear();
   for (size_t i = 0; i < config_.max_particles; ++i) {
@@ -212,13 +191,7 @@ void Mcl::initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr
     filter_.particles.emplace_back(tf2::Transform{q, p}, 1. / config_.max_particles);
   }
 
-  auto ps =
-    filter_.get_pose_with_covariance_stamped(initial_pose->header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = initial_pose->header.stamp;
+  last_filter_update_ = stamp;
 }
 
 void Mcl::request_nomotion_update()
@@ -237,15 +210,15 @@ bool Mcl::odometry_update(
   if (!last_odom_pose_) {
     ROS_INFO_NAMED(name, "first odometry update, recording the odom pose");
     last_odom_pose_ = odom_pose;
-    last_pose_.stamp_ = header.stamp;
+    last_filter_update_ = header.stamp;
     // call should_process to record the frame_id
     should_process(tf2::Transform::getIdentity(), {measurement_type, header.frame_id});
     return true;
   }
 
-  if (header.stamp <= last_pose_.stamp_) {
+  if (header.stamp <= last_filter_update_) {
     ROS_DEBUG_STREAM_NAMED(
-      name, "skipping out-of-order measurement, " << header.stamp << " <= " << last_pose_.stamp_);
+      name, "skipping out-of-order measurement, " << header.stamp << " <= " << last_filter_update_);
     return false;
   }
 
@@ -290,15 +263,6 @@ bool Mcl::should_process(const tf2::Transform & diff, const MeasurementKey & mea
   }
 }
 
-void Mcl::publish_data(const geometry_msgs::PoseWithCovarianceStamped & ps)
-{
-  cloud_pub_.publish(ps.header, filter_);
-  std_msgs::UInt32 count;
-  count.data = filter_.particles.size();
-  count_pub_.publish(count);
-  pose_pub_.publish(ps);
-}
-
 std::ostream & operator<<(std::ostream & out, const Mcl::MeasurementType & measurement_type)
 {
   switch (measurement_type) {

ruvu_mcl/src/mcl.hpp

@@ -2,21 +2,21 @@
 
 #pragma once
 
+#include <boost/shared_ptr.hpp>
+#include <map>
 #include <memory>
 #include <string>
+#include <vector>
 
-#include "./cloud_publisher.hpp"
 #include "./config.hpp"
 #include "./particle_filter.hpp"
 #include "ros/message_forward.h"
-#include "ros/publisher.h"
-#include "tf2/transform_datatypes.h"
+#include "ros/time.h"
 
 // forward declare
 namespace ros
 {
 class NodeHandle;
-class Time;
 }  // namespace ros
 namespace tf2
 {
@@ -30,6 +30,10 @@ namespace nav_msgs
 {
 ROS_DECLARE_MESSAGE(OccupancyGrid)
 }
+namespace std_msgs
+{
+ROS_DECLARE_MESSAGE(Header)
+}
 
 namespace ruvu_mcl
 {
@@ -51,13 +55,17 @@ class Mcl
 
   void configure(const Config & config);
   const Config & config() const { return config_; }
-  const tf2::Transform & pose() const { return last_pose_; }
+  const PoseWithCovariance get_pose_with_covariance() const
+  {
+    return filter_.get_pose_with_covariance();
+  }
+  const std::vector<Particle> & particles() const { return filter_.particles; }
 
   bool scan_cb(const LaserData & scan, const tf2::Transform & odom_pose);
   bool landmark_cb(const LandmarkList & landmarks, const tf2::Transform & odom_pose);
   void map_cb(const nav_msgs::OccupancyGridConstPtr & map);
   void landmark_list_cb(const LandmarkList & landmarks);
-  void initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose);
+  void initial_pose_cb(const ros::Time & stamp, const PoseWithCovariance & initial_pose);
   void request_nomotion_update();
 
 private:
@@ -76,18 +84,12 @@ class Mcl
     const std_msgs::Header & header, const MeasurementType & measurement_type,
     tf2::Transform odom_pose);
   bool should_process(const tf2::Transform & diff, const MeasurementKey & measurment_key);
-  void publish_data(const geometry_msgs::PoseWithCovarianceStamped & ps);
-
-  // data output
-  CloudPublisher cloud_pub_;
-  ros::Publisher count_pub_;
-  ros::Publisher pose_pub_;
 
   // internals
   Config config_;
   std::shared_ptr<Rng> rng_;
   std::optional<tf2::Transform> last_odom_pose_;
-  tf2::Stamped<tf2::Transform> last_pose_;
+  ros::Time last_filter_update_;
   ParticleFilter filter_;
   std::unique_ptr<MotionModel> model_;
   nav_msgs::OccupancyGridConstPtr map_;