bugfix: check_path.cpp

Signed-off-by: Daisuke Sato <[email protected]>

cabot_bt/plugins/condition/check_path.cpp

@@ -105,7 +105,16 @@ void CheckPathCondition::normalize_path(nav_msgs::msg::Path & path)
       }
     }
   }
-  temp.push_back(path.poses[path.poses.size() - 1]);
+  auto tx = temp[temp.size() - 1].pose.position.x;
+  auto ty = temp[temp.size() - 1].pose.position.y;
+  auto px = path.poses[path.poses.size() - 1].pose.position.x;
+  auto py = path.poses[path.poses.size() - 1].pose.position.y;
+  RCLCPP_INFO(
+    node_->get_logger(), "hypot (%.5f, %.5f) -> (%.5f, %.5f) %.5f",
+    tx, ty, px, py, std::hypot(px - tx, py - ty));
+  if (std::hypot(px - tx, py - ty) > MIN_DIST / 2) {
+    temp.push_back(path.poses[path.poses.size() - 1]);
+  }
   path.poses = temp;
 }
 

cabot_bt/test/data/path2.yaml

@@ -0,0 +1,36 @@
+header:
+  stamp:
+    sec: 0
+    nanosec: 0
+  frame_id: local/odom
+poses:
+- header:
+    stamp:
+      sec: 0
+      nanosec: 0
+    frame_id: local/odom
+  pose:
+    position:
+      x: 2.3530254342857315
+      y: -3.4121318032810586
+      z: 0.0
+    orientation:
+      x: 0.0
+      y: 0.0
+      z: 0.9674952892025127
+      w: 0.25288903766463733
+- header:
+    stamp:
+      sec: 0
+      nanosec: 0
+    frame_id: local/odom
+  pose:
+    position:
+      x: -0.2632573734885906
+      y: -1.944118087492098
+      z: 0.0
+    orientation:
+      x: 0.0
+      y: 0.0
+      z: 0.9674952892025127
+      w: 0.25288903766463733