Merge branch 'feature/interface-improvements' into 'master'

feat: Improve MclRos interface

See merge request ruvu/mcl!77

ruvu_mcl/src/config.cpp

@@ -42,7 +42,7 @@ Config::Config(const ruvu_mcl::AMCLConfig & config)
     model = m;
   } else {
     std::ostringstream ss;
-    ss << "motion model " << config.odom_model_type << " is not yet implemented";
+    ss << "motion model '" << config.odom_model_type << "' is not yet implemented";
     throw std::runtime_error(ss.str());
   }
 
@@ -69,7 +69,7 @@ Config::Config(const ruvu_mcl::AMCLConfig & config)
     laser = c;
   } else {
     std::ostringstream ss;
-    ss << "sensor model " << config.laser_model_type << " is not yet implemented";
+    ss << "sensor model '" << config.laser_model_type << "' is not yet implemented";
     throw std::runtime_error(ss.str());
   }
 
@@ -89,7 +89,7 @@ Config::Config(const ruvu_mcl::AMCLConfig & config)
     landmark = c;
   } else {
     std::ostringstream ss;
-    ss << "sensor model " << config.laser_model_type << " is not yet implemented";
+    ss << "landmark sensor model '" << config.laser_model_type << "' is not yet implemented";
     throw std::runtime_error(ss.str());
   }
 

ruvu_mcl/src/mcl.cpp

@@ -34,9 +34,13 @@ std::unique_ptr<Laser> create_laser_model(
     throw std::logic_error("no laser model configured");
 }
 
-Mcl::Mcl(ros::NodeHandle nh, ros::NodeHandle private_nh)
+Mcl::Mcl() : Mcl(std::make_shared<Rng>()) {}
+
+Mcl::Mcl(uint_fast32_t seed) : Mcl(std::make_shared<Rng>(seed)) {}
+
+Mcl::Mcl(const std::shared_ptr<Rng> & rng)
 : config_(),
-  rng_(std::make_unique<Rng>()),
+  rng_(rng),
   last_odom_pose_(),
   last_filter_update_(),
   filter_(),

ruvu_mcl/src/mcl.hpp

@@ -14,10 +14,6 @@
 #include "ros/time.h"
 
 // forward declare
-namespace ros
-{
-class NodeHandle;
-}  // namespace ros
 namespace tf2
 {
 class Transform;
@@ -50,7 +46,8 @@ class Rng;
 class Mcl
 {
 public:
-  Mcl(ros::NodeHandle nh, ros::NodeHandle private_nh);
+  Mcl();
+  Mcl(std::uint_fast32_t seed);
   ~Mcl();  // to handle forward declares
 
   void configure(const Config & config);
@@ -80,6 +77,8 @@ class Mcl
 
   friend std::ostream & operator<<(std::ostream & out, const MeasurementType & measurement_type);
 
+  Mcl(const std::shared_ptr<Rng> & rng);
+
   bool odometry_update(
     const std_msgs::Header & header, const MeasurementType & measurement_type,
     tf2::Transform odom_pose);

ruvu_mcl/src/mcl_ros.cpp

@@ -17,10 +17,35 @@ constexpr auto name = "mcl_ros";
 
 namespace ruvu_mcl
 {
+geometry_msgs::PoseWithCovariance to_msg(PoseWithCovariance pose_with_covariance)
+{
+  geometry_msgs::PoseWithCovariance ps;
+  tf2::toMsg(pose_with_covariance.pose.getOrigin(), ps.pose.position);
+  tf2::convert(pose_with_covariance.pose.getRotation(), ps.pose.orientation);
+  assert(pose_with_covariance.covariance.size() == ps.covariance.size());
+  std::copy(
+    pose_with_covariance.covariance.begin(), pose_with_covariance.covariance.end(),
+    ps.covariance.begin());
+  return ps;
+}
+
 MclRos::MclRos(
   ros::NodeHandle nh, ros::NodeHandle private_nh,
   const std::shared_ptr<const tf2_ros::Buffer> & buffer)
-: mcl_(nh, private_nh),
+: mcl_(),
+  buffer_(buffer),
+  transform_br_(),
+  last_tf_broadcast_(),
+  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))
+{
+}
+
+MclRos::MclRos(
+  ros::NodeHandle nh, ros::NodeHandle private_nh,
+  const std::shared_ptr<const tf2_ros::Buffer> & buffer, uint_fast32_t seed)
+: mcl_(seed),
   buffer_(buffer),
   transform_br_(),
   last_tf_broadcast_(),
@@ -34,7 +59,13 @@ MclRos::~MclRos() = default;
 
 void MclRos::configure(const ruvu_mcl::AMCLConfig & config) { mcl_.configure(Config{config}); }
 
-void MclRos::scan_cb(const sensor_msgs::LaserScanConstPtr & scan)
+const geometry_msgs::PoseWithCovariance MclRos::get_pose_with_covariance() const
+{
+  auto ps = mcl_.get_pose_with_covariance();
+  return to_msg(ps);
+}
+
+bool MclRos::scan_cb(const sensor_msgs::LaserScanConstPtr & scan)
 {
   tf2::Transform tf;
   {
@@ -49,7 +80,7 @@ void MclRos::scan_cb(const sensor_msgs::LaserScanConstPtr & scan)
     odom_pose = get_odom_pose(data.header.stamp);
   } catch (const tf2::TransformException & e) {
     ROS_WARN_NAMED(name, "failed to compute odom pose, skipping measurement (%s)", e.what());
-    return;
+    return false;
   }
 
   auto updated = mcl_.scan_cb(data, odom_pose);
@@ -61,9 +92,10 @@ void MclRos::scan_cb(const sensor_msgs::LaserScanConstPtr & scan)
   } else if (!last_tf_broadcast_.header.frame_id.empty()) {  // Verify last_tf_broadcast_ is set
     broadcast_last_tf(data.header.stamp);
   }
+  return updated;
 }
 
-void MclRos::landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks)
+bool MclRos::landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks)
 {
   tf2::Transform tf;
   {
@@ -78,7 +110,7 @@ void MclRos::landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks)
     odom_pose = get_odom_pose(data.header.stamp);
   } catch (const tf2::TransformException & e) {
     ROS_WARN_NAMED(name, "failed to compute odom pose, skipping measurement (%s)", e.what());
-    return;
+    return false;
   }
 
   auto updated = mcl_.landmark_cb(data, odom_pose);
@@ -90,6 +122,7 @@ void MclRos::landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks)
   } else if (!last_tf_broadcast_.header.frame_id.empty()) {  // Verify last_tf_broadcast_ is set
     broadcast_last_tf(data.header.stamp);
   }
+  return updated;
 }
 
 void MclRos::map_cb(const nav_msgs::OccupancyGridConstPtr & map) { mcl_.map_cb(map); }
@@ -174,12 +207,7 @@ void MclRos::publish_data(const ros::Time & stamp, const PoseWithCovariance & po
 
   geometry_msgs::PoseWithCovarianceStamped ps;
   ps.header = header;
-  tf2::toMsg(pose_with_covariance.pose.getOrigin(), ps.pose.pose.position);
-  tf2::convert(pose_with_covariance.pose.getRotation(), ps.pose.pose.orientation);
-  assert(pose_with_covariance.covariance.size() == ps.pose.covariance.size());
-  std::copy(
-    pose_with_covariance.covariance.begin(), pose_with_covariance.covariance.end(),
-    ps.pose.covariance.begin());
+  ps.pose = to_msg(pose_with_covariance);
   pose_pub_.publish(ps);
 }
 }  // namespace ruvu_mcl

ruvu_mcl/src/mcl_ros.hpp

@@ -19,7 +19,7 @@ class Buffer;
 }
 namespace geometry_msgs
 {
-ROS_DECLARE_MESSAGE(PoseWithCovarianceStamped)
+ROS_DECLARE_MESSAGE(PoseWithCovariance)
 }
 namespace nav_msgs
 {
@@ -44,12 +44,16 @@ class MclRos
   MclRos(
     ros::NodeHandle nh, ros::NodeHandle private_nh,
     const std::shared_ptr<const tf2_ros::Buffer> & buffer);
+  MclRos(
+    ros::NodeHandle nh, ros::NodeHandle private_nh,
+    const std::shared_ptr<const tf2_ros::Buffer> & buffer, std::uint_fast32_t seed);
   ~MclRos();  // to handle forward declares
 
   void configure(const ruvu_mcl::AMCLConfig & config);
+  const geometry_msgs::PoseWithCovariance get_pose_with_covariance() const;
 
-  void scan_cb(const sensor_msgs::LaserScanConstPtr & scan);
-  void landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks);
+  bool scan_cb(const sensor_msgs::LaserScanConstPtr & scan);
+  bool landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks);
   void map_cb(const nav_msgs::OccupancyGridConstPtr & map);
   void landmark_list_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks);
   void initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose);