multihypotheses working now

Signed-off-by: JesusSilvaUtrera <jesus.silva@ekumenlabs.com>

Author: JesusSilvaUtrera

localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/mh_amcl.hpp

@@ -157,11 +157,11 @@ class MH_AMCL_Node : public rclcpp_lifecycle::LifecycleNode {
   std::shared_ptr<mh_amcl::MapMatcher> map_matcher_;
 
   // Callbacks
-  void map_callback(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg);
+  void map_callback(nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);
   void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg);
   void initpose_callback(
-      const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
-          &pose_msg);
+    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
+        pose_msg);
 };
 
 } // namespace mh_amcl

localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/utils.hpp

@@ -67,8 +67,8 @@ std_msgs::msg::ColorRGBA getColor(Color color_id, double alpha = 1.0);
  * frame to the global frame
  */
 std::tuple<double, double>
-mapToWorld(std::shared_ptr<beluga_ros::OccupancyGrid> costmap, int localX,
-           int localY);
+mapToWorld(std::shared_ptr<beluga_ros::OccupancyGrid> costmap, int local_x,
+           int local_y);
 
 /**
  * @brief Function to transform the coordinates of a point from global frame to
@@ -77,7 +77,7 @@ mapToWorld(std::shared_ptr<beluga_ros::OccupancyGrid> costmap, int localX,
  */
 std::tuple<int, int>
 worldToMapNoBounds(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-                   double worldX, double worldY);
+                   double world_x, double world_y);
 
 /**
  * @brief Function to transform the coordinates of a point from global frame to
@@ -87,7 +87,7 @@ worldToMapNoBounds(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
  */
 std::tuple<int, int>
 worldToMapEnforceBounds(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-                        double worldX, double worldY);
+                        double world_x, double world_y);
 
 /**
  * @brief extracts the X, Y coordinates and the yaw angle from a Sophus::SE2d

localization/beluga_demo_mh_amcl/rviz/custom_rviz.rviz

@@ -10,10 +10,8 @@ Panels:
         - /Sensors1
         - /Localization1
         - /Localization1/ParticleCloud1/Topic1
-        - /Markers1
-        - /Markers1/Mapped landmarks1/Topic1
       Splitter Ratio: 0.6411764621734619
-    Tree Height: 472
+    Tree Height: 486
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -247,11 +245,6 @@ Visualization Manager:
               ParticleCloud: true
               Value: true
               World map: true
-            Markers:
-              Detected landmarks: true
-              Mapped landmarks: true
-              Spotlight detections: true
-              Value: true
             RobotModel: false
             Sensors:
               Front camera: true
@@ -327,56 +320,14 @@ Visualization Manager:
             bodies: true
             heads: true
           Topic:
-            Depth: 5
+            Depth: 10
             Durability Policy: Volatile
-            History Policy: Keep Last
+            History Policy: Keep All
             Reliability Policy: Best Effort
             Value: /particle_markers
           Value: true
       Enabled: true
       Name: Localization
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Image
-          Enabled: true
-          Max Value: 1
-          Median window: 5
-          Min Value: 0
-          Name: Spotlight detections
-          Normalize Range: true
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /landmarks/spotlight_detections
-          Value: true
-        - Class: rviz_default_plugins/MarkerArray
-          Enabled: true
-          Name: Mapped landmarks
-          Namespaces:
-            {}
-          Topic:
-            Depth: 5
-            Durability Policy: Transient Local
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /landmarks/feature_map_markers
-          Value: true
-        - Class: rviz_default_plugins/MarkerArray
-          Enabled: true
-          Name: Detected landmarks
-          Namespaces:
-            {}
-          Topic:
-            Depth: 5
-            Durability Policy: System Default
-            History Policy: Keep Last
-            Reliability Policy: System Default
-            Value: /landmarks/landmark_detection_markers
-          Value: true
-      Enabled: true
-      Name: Markers
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -453,11 +404,9 @@ Window Geometry:
   Height: 1043
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000019b00000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000259000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002800530070006f0074006c006900670068007400200064006500740065006300740069006f006e0073010000029a0000011a0000002800ffffff00000001000001de00000379fc0200000007fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001800460072006f006e0074002000630061006d006500720061010000003b000001300000002800fffffffb0000000c00430061006d00650072006101000001710000012b0000002800fffffffb0000000a0049006d006100670065010000003b000001330000000000000000fb0000000c00430061006d0065007200610100000174000001330000000000000000fb0000000a0056006900650077007301000002a200000112000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003fb0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
-  Spotlight detections:
-    collapsed: false
   Time:
     collapsed: false
   Tool Properties:

localization/beluga_demo_mh_amcl/src/mh_amcl/map_matcher.cpp

@@ -183,11 +183,11 @@ float MapMatcher::match(
 
   for (int i = 0; i < scan.size(); i = i + scale) {
     tf2::Vector3 test_point = transform * scan[i];
-    auto [gi, gj] =
+    auto [local_x, local_y] =
         utils::worldToMapNoBounds(costmap, test_point.x(), test_point.y());
 
-    if (gi > 0 && gj > 0 && gi < costmap->width() && gj < costmap->height() &&
-        costmap->data_at(gi, gj) == utils::LETHAL_OBSTACLE) {
+    if (local_x > 0 && local_y > 0 && local_x < costmap->width() && local_y < costmap->height() &&
+        costmap->data_at(local_x, local_y) == utils::LETHAL_OBSTACLE) {
       hits++;
     }
     total++;

localization/beluga_demo_mh_amcl/src/mh_amcl/mh_amcl.cpp

@@ -288,7 +288,7 @@ void MH_AMCL_Node::predict() {
 }
 
 void MH_AMCL_Node::map_callback(
-    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr &msg) {
+    nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) {
   // Every time a new map is sent, update the costmap and the map matcher
   costmap_ = std::make_shared<beluga_ros::OccupancyGrid>(msg);
   map_matcher_ = std::make_shared<mh_amcl::MapMatcher>(msg);
@@ -312,9 +312,6 @@ void MH_AMCL_Node::correct() {
   }
 
   last_time_ = last_laser_->header.stamp;
-
-  RCLCPP_INFO_STREAM(get_logger(),
-                      "Correct [" << (now() - start).seconds() << " secs]");
 }
 
 void MH_AMCL_Node::reseed() {
@@ -323,15 +320,11 @@ void MH_AMCL_Node::reseed() {
   for (auto &particles : particles_population_) {
     particles->reseed();
   }
-
-  RCLCPP_INFO_STREAM(get_logger(), "==================Reseed ["
-                                        << (now() - start).seconds()
-                                        << " secs]");
 }
 
 void MH_AMCL_Node::initpose_callback(
-    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
-        &pose_msg) {
+    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr
+      pose_msg) {
   // Having a new initial pose retriggers the algorithm
   if (pose_msg->header.frame_id == "map") {
     RCLCPP_INFO(get_logger(), "Map initial pose received!");
@@ -517,10 +510,10 @@ void MH_AMCL_Node::manage_hypotheses() {
 
 signed char MH_AMCL_Node::get_cost(const geometry_msgs::msg::Pose &pose) {
   // Get the corresponding cost from the costmap for a specific pose
-  auto [i, j] =
+  auto [local_x, local_y] =
       utils::worldToMapNoBounds(costmap_, pose.position.x, pose.position.y);
 
-  std::optional<signed char> opt = costmap_->data_at(i, j);
+  std::optional<signed char> opt = costmap_->data_at(local_x, local_y);
   if (opt.has_value()) {
     return static_cast<unsigned char>(opt.value());
   } else {

localization/beluga_demo_mh_amcl/src/mh_amcl/particles_distribution.cpp

@@ -371,7 +371,6 @@ void ParticlesDistribution::correct_once(
     p.hits = p.hits / static_cast<float>(scan.ranges.size());
     quality_ = std::max(quality_, p.hits);
   }
-  RCLCPP_WARN_STREAM(parent_node_->get_logger(), "Quality computed: " << quality_);
 }
 
 tf2::Transform ParticlesDistribution::get_tranform_to_read(

localization/beluga_demo_mh_amcl/src/mh_amcl/utils.cpp

@@ -115,58 +115,62 @@ std_msgs::msg::ColorRGBA getColor(Color color_id, double alpha) {
 }
 
 std::tuple<double, double>
-mapToWorld(std::shared_ptr<beluga_ros::OccupancyGrid> costmap, int localX,
-           int localY) {
-  double worldX = costmap->origin().translation().x() +
-                  (localX + 0.5) * costmap->resolution();
-  double worldY = costmap->origin().translation().y() +
-                  (localY + 0.5) * costmap->resolution();
-  return std::make_tuple(worldX, worldY);
+mapToWorld(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
+           int local_x,
+           int local_y)
+{
+  // map origin in world frame
+  double origin_x = costmap->origin().translation().x();
+  double origin_y = costmap->origin().translation().y();
+  double res     = costmap->resolution();
+
+  // use the origin + the center of the cell (local + 0.5) times the resolution of each cell
+  double world_x = origin_x + (local_x + 0.5) * res;
+  double world_y = origin_y + (local_y + 0.5) * res;
+  return std::make_tuple(world_x, world_y);
 }
 
 std::tuple<int, int>
 worldToMapNoBounds(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-                   double worldX, double worldY) {
-  // No clamping or bounds checking is done here, result might be negative or
-  // exceed the grid limits
-  int localX = static_cast<int>((worldX + costmap->origin().translation().x()) /
-                                costmap->resolution());
-  int localY = static_cast<int>((worldY + costmap->origin().translation().y()) /
-                                costmap->resolution());
-  return std::make_tuple(localX, localY);
+                   double world_x,
+                   double world_y)
+{
+  double origin_x = costmap->origin().translation().x();
+  double origin_y = costmap->origin().translation().y();
+  double res     = costmap->resolution();
+
+  // subtract origin, divide by resolution, floor to get cell index
+  int local_x = static_cast<int>(std::floor((world_x - origin_x) / res));
+  int local_y = static_cast<int>(std::floor((world_y - origin_y) / res));
+  return std::make_tuple(local_x, local_y);
 }
 
 std::tuple<int, int>
 worldToMapEnforceBounds(std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-                        double worldX, double worldY) {
-  int localX;
-  int localY;
-
-  // If the world coordinates given are inferior to the costmap origin, start on
-  // the origin of the grid
-  if (worldX < costmap->origin().translation().x() ||
-      worldY < costmap->origin().translation().y())
-    localX = 0;
-  localY = 0;
-
-  localX = static_cast<int>((worldX + costmap->origin().translation().x()) /
-                            costmap->resolution());
-  localY = static_cast<int>((worldY + costmap->origin().translation().y()) /
-                            costmap->resolution());
-
-  // Clamp the indices within grid dimensions:
-  if (localX < 0) {
-    localX = 0;
-  } else if (localX >= costmap->width()) {
-    localX = costmap->width() - 1;
-  }
-  if (localY < 0) {
-    localY = 0;
-  } else if (localY >= costmap->height()) {
-    localY = costmap->height() - 1;
+                        double world_x,
+                        double world_y)
+{
+  double origin_x = costmap->origin().translation().x();
+  double origin_y = costmap->origin().translation().y();
+  double res     = costmap->resolution();
+  int width      = costmap->width();
+  int height     = costmap->height();
+
+  // again subtract origin, divide by resolution, floor to get cell index
+  int local_x = static_cast<int>(std::floor((world_x - origin_x) / res));
+  int local_y = static_cast<int>(std::floor((world_y - origin_y) / res));
+
+  // if completely before the map origin, snap to (0,0)
+  if (world_x < origin_x || world_y < origin_y) {
+    local_x = 0;
+    local_y = 0;
   }
 
-  return std::make_tuple(localX, localY);
+  // clamp into [0_width-1], [0_height-1]
+  local_x = std::clamp(local_x, 0, width  - 1);
+  local_y = std::clamp(local_y, 0, height - 1);
+
+  return std::make_tuple(local_x, local_y);
 }
 
 std::tuple<double, double, double>