feat(mission_planner): add random id to route (autowarefoundation#2746)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

planning/mission_planner/src/mission_planner/arrival_checker.cpp

@@ -18,9 +18,6 @@
 
 #include <tf2/utils.h>
 
-// TODO(Takagi, Isamu): remove when modified goal is always published
-#include <memory>
-
 namespace mission_planner
 {
 
@@ -31,44 +28,48 @@ ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node
   distance_ = node->declare_parameter<double>("arrival_check_distance");
   duration_ = node->declare_parameter<double>("arrival_check_duration");
 
-  sub_goal_ = node->create_subscription<autoware_planning_msgs::msg::PoseWithUuidStamped>(
+  sub_goal_ = node->create_subscription<PoseWithUuidStamped>(
     "input/modified_goal", 1,
-    [this](const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr msg) {
-      goal_pose_ = msg;
-    });
+    [this](const PoseWithUuidStamped::ConstSharedPtr msg) { modify_goal(*msg); });
+}
+
+void ArrivalChecker::set_goal()
+{
+  // Ignore the modified goal after the route is cleared.
+  goal_with_uuid_ = std::nullopt;
 }
 
-void ArrivalChecker::reset_goal()
+void ArrivalChecker::set_goal(const PoseWithUuidStamped & goal)
 {
-  // Disable checking until the modified goal is received.
-  goal_pose_.reset();
+  // Ignore the modified goal for the previous route using uuid.
+  goal_with_uuid_ = goal;
 }
 
-// TODO(Takagi, Isamu): remove when modified goal is always published
-void ArrivalChecker::reset_goal(const autoware_planning_msgs::msg::PoseWithUuidStamped & goal)
+void ArrivalChecker::modify_goal(const PoseWithUuidStamped & modified_goal)
 {
-  const auto pose = std::make_shared<autoware_planning_msgs::msg::PoseWithUuidStamped>();
-  *pose = goal;
-  goal_pose_ = pose;
+  if (!goal_with_uuid_) {
+    return;
+  }
+  if (goal_with_uuid_.value().uuid.uuid != modified_goal.uuid.uuid) {
+    return;
+  }
+  set_goal(modified_goal);
 }
 
-bool ArrivalChecker::is_arrived(const geometry_msgs::msg::PoseStamped & pose) const
+bool ArrivalChecker::is_arrived(const PoseStamped & pose) const
 {
-  // Check if the modified goal is received.
-  if (goal_pose_ == nullptr) {
+  if (!goal_with_uuid_) {
     return false;
   }
-  geometry_msgs::msg::PoseStamped goal;
-  goal.header = goal_pose_->header;
-  goal.pose = goal_pose_->pose;
+  const auto goal = goal_with_uuid_.value();
 
-  // Check frame_id.
+  // Check frame id
   if (goal.header.frame_id != pose.header.frame_id) {
     return false;
   }
 
   // Check distance.
-  if (distance_ < tier4_autoware_utils::calcDistance2d(pose, goal)) {
+  if (distance_ < tier4_autoware_utils::calcDistance2d(pose.pose, goal.pose)) {
     return false;
   }
 

planning/mission_planner/src/mission_planner/arrival_checker.hpp

@@ -28,18 +28,21 @@ namespace mission_planner
 class ArrivalChecker
 {
 public:
+  using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped;
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
   explicit ArrivalChecker(rclcpp::Node * node);
-  void reset_goal();
-  void reset_goal(const autoware_planning_msgs::msg::PoseWithUuidStamped & goal);
-  bool is_arrived(const geometry_msgs::msg::PoseStamped & pose) const;
+  void set_goal();
+  void set_goal(const PoseWithUuidStamped & goal);
+  bool is_arrived(const PoseStamped & pose) const;
 
 private:
   double distance_;
   double angle_;
   double duration_;
-  autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr goal_pose_;
-  rclcpp::Subscription<autoware_planning_msgs::msg::PoseWithUuidStamped>::SharedPtr sub_goal_;
+  std::optional<PoseWithUuidStamped> goal_with_uuid_;
+  rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_goal_;
   motion_utils::VehicleStopChecker vehicle_stop_checker_;
+  void modify_goal(const PoseWithUuidStamped & modified_goal);
 };
 
 }  // namespace mission_planner

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -16,12 +16,10 @@
 
 #include <autoware_adapi_v1_msgs/srv/set_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
-
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
+
+#include <array>
+#include <random>
 
 namespace
 {
@@ -48,6 +46,14 @@ LaneletSegment convert(const LaneletSegment & s)
   return segment;
 }
 
+std::array<uint8_t, 16> generate_random_id()
+{
+  static std::independent_bits_engine<std::mt19937, 8, uint8_t> engine(std::random_device{}());
+  std::array<uint8_t, 16> id;
+  std::generate(id.begin(), id.end(), std::ref(engine));
+  return id;
+}
+
 }  // namespace
 
 namespace mission_planner
@@ -113,21 +119,18 @@ PoseStamped MissionPlanner::transform_pose(const PoseStamped & input)
 
 void MissionPlanner::change_route()
 {
-  arrival_checker_.reset_goal();
+  arrival_checker_.set_goal();
   // TODO(Takagi, Isamu): publish an empty route here
 }
 
 void MissionPlanner::change_route(const LaneletRoute & route)
 {
-  // TODO(Takagi, Isamu): replace when modified goal is always published
-  // arrival_checker_.reset_goal();
   PoseWithUuidStamped goal;
   goal.header = route.header;
   goal.pose = route.goal_pose;
   goal.uuid = route.uuid;
 
-  arrival_checker_.reset_goal(goal);
-
+  arrival_checker_.set_goal(goal);
   pub_route_->publish(route);
   pub_marker_->publish(planner_->visualize(route));
 }
@@ -170,13 +173,14 @@ void MissionPlanner::on_set_route(
 
   // Convert route.
   LaneletRoute route;
-  route.header.stamp = req->header.stamp;
-  route.header.frame_id = map_frame_;
   route.start_pose = odometry_->pose.pose;
   route.goal_pose = transform_pose(pose).pose;
   for (const auto & segment : req->segments) {
     route.segments.push_back(convert(segment));
   }
+  route.header.stamp = req->header.stamp;
+  route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
 
   // Update route.
   change_route(route);
@@ -226,6 +230,7 @@ void MissionPlanner::on_set_route_points(
   }
   route.header.stamp = req->header.stamp;
   route.header.frame_id = map_frame_;
+  route.uuid.uuid = generate_random_id();
 
   // Update route.
   change_route(route);