6: Check TODOs/FIXMEs

Author: simon-eisenmann-driveblocks

planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp

@@ -83,8 +83,8 @@ void HMINode::PublishMission_(std::string mission)
     missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT;
   }
 
-  // TODO(simon.eisenmann@driveblocks.ai): Change deadline parameter
-  missionMessage.deadline = 1000;
+  missionMessage.deadline = 1000;  // This parameter can be changed if needed (it will be set by the
+                                   // software in the future).
 
   mission_publisher_->publish(missionMessage);
 }

planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp

@@ -248,8 +248,10 @@ class MissionPlannerNode : public rclcpp::Node
   Direction target_lane_ = stay;
   Direction mission_ = stay;
   int retry_attempts_ = 0;
+  int recenter_counter_ = 0;
   bool lane_change_trigger_success_ = true;
   Direction lane_change_direction_ = stay;
+  float deadline_target_lane_ = 1000;
 
   lanelet::BasicPoint2d goal_point_;
   std::vector<int> ego_lane_;
@@ -262,9 +264,10 @@ class MissionPlannerNode : public rclcpp::Node
   float projection_distance_on_goallane_;
   int retrigger_attempts_max_;
   std::string local_map_frame_;
+  int recenter_period_;
 
   // Unique ID for each marker
-  int centerline_marker_id_ = 0;
+  ID centerline_marker_id_;
 };
 }  // namespace autoware::mapless_architecture
 

planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml

@@ -5,3 +5,4 @@
       projection_distance_on_goallane: 20.0 # [m] projection distance of goal point
       retrigger_attempts_max: 10 # number of attempts for triggering a lane change
       local_map_frame: map # Identifier of local map frame. Currently, there is no way to set global ROS params https://github.com/ros2/ros2cli/issues/778 -> This param has to be set in the mission converter also!
+      recenter_period: 10 # recenter goal point after 10 odometry updates

planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp

@@ -93,7 +93,7 @@ MissionPlannerNode::MissionPlannerNode(const rclcpp::NodeOptions & options)
   tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
 
   // Set ros parameters (DEFAULT values will be overwritten by external
-  // parameter file if exists)
+  // parameter file)
   distance_to_centerline_threshold_ =
     declare_parameter<float>("distance_to_centerline_threshold", 0.2);
   RCLCPP_INFO(
@@ -113,6 +113,13 @@ MissionPlannerNode::MissionPlannerNode(const rclcpp::NodeOptions & options)
 
   local_map_frame_ = declare_parameter<std::string>("local_map_frame", "map");
   RCLCPP_INFO(this->get_logger(), "Local map frame identifier: %s", local_map_frame_.c_str());
+
+  recenter_period_ = declare_parameter<int>("recenter_period", 10);
+  RCLCPP_INFO(
+    this->get_logger(),
+    "After this number of odometry updates the goal point (used for lane change) is recentered (on "
+    "the centerline): %d",
+    recenter_period_);
 }
 
 void MissionPlannerNode::CallbackLocalMapMessages_(
@@ -233,9 +240,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_(
       break;
   }
 
-  lanes.deadline_target_lane =
-    std::numeric_limits<double>::infinity();  // TODO(simon.eisenmann@driveblocks.ai):
-                                              // Change this value
+  lanes.deadline_target_lane = deadline_target_lane_;
 
   // Create driving corridors and add them to the MissionLanesStamped message
   lanes.ego_lane = CreateDrivingCorridor(ego_lane_, converted_lanelets);
@@ -308,14 +313,22 @@ void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry
     goal_point_.x() = target_pose.get_x();
     goal_point_.y() = target_pose.get_y();
 
-    // Re-center updated goal point to lie on centerline (to get rid of issues
+    // Recenter updated goal point to lie on centerline (to get rid of issues
     // with a less accurate odometry update which could lead to loosing the
-    // goal lane)
-    const lanelet::BasicPoint2d target_point_2d = RecenterGoalPoint(goal_point_, current_lanelets_);
+    // goal lane), recenter only after a certain number of steps (recenter_period_) to reduce the
+    // calling frequency
+    if (recenter_counter_ >= recenter_period_) {
+      const lanelet::BasicPoint2d target_point_2d =
+        RecenterGoalPoint(goal_point_, current_lanelets_);
 
-    // Overwrite goal point
-    goal_point_.x() = target_point_2d.x();
-    goal_point_.y() = target_point_2d.y();
+      // Overwrite goal point
+      goal_point_.x() = target_point_2d.x();
+      goal_point_.y() = target_point_2d.y();
+
+      recenter_counter_ = 0;
+    } else {
+      recenter_counter_++;
+    }
 
     // --- Start of debug visualization
     // Create marker and publish it
@@ -405,6 +418,8 @@ void MissionPlannerNode::CallbackMissionMessages_(const autoware_planning_msgs::
       RCLCPP_INFO(this->get_logger(), "Mission does not match.");
   }
 
+  deadline_target_lane_ = msg.deadline;
+
   return;
 }
 
@@ -548,8 +563,7 @@ void MissionPlannerNode::CheckIfGoalPointShouldBeReset_(
 {
   // Check if goal point should be reset: If the x value of the goal point is
   // negative, then the point is behind the vehicle and must be therefore reset.
-  if (goal_point_.x() < 0 && mission_ != stay) {  // TODO(simon.eisenmann@driveblocks.ai): Maybe
-                                                  // remove condition mission_ != stay
+  if (goal_point_.x() < 0 && mission_ != stay) {
     // Find the index of the lanelet containing the goal point
     int goal_index =
       FindOccupiedLaneletID(converted_lanelets, goal_point_);  // Returns -1 if no match
@@ -763,12 +777,7 @@ void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor(
   centerline_marker.ns = "centerline";
 
   // Unique ID
-  if (centerline_marker_id_ == std::numeric_limits<int>::max()) {
-    // Handle overflow
-    centerline_marker_id_ = 0;
-  } else {
-    centerline_marker.id = centerline_marker_id_++;
-  }
+  centerline_marker.id = centerline_marker_id_.ReturnIDAndIncrement();
 
   centerline_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
   centerline_marker.action = visualization_msgs::msg::Marker::ADD;

planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp

@@ -66,6 +66,16 @@ class Pose2D
   double psi_;
 };
 
+class ID
+{
+public:
+  ID() : value_(0) {}
+  unsigned int ReturnIDAndIncrement();
+
+private:
+  unsigned int value_;
+};
+
 /**
  * Represent a 2D pose (pose_prev) in a new origin / coordinate system, which
  * is given in relation to the previous coordinate system / origin

planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp

@@ -229,8 +229,6 @@ std::vector<int> GetRelevantAdjacentLanelets(
     ids_relevant_successors = ids_adjacent_lanelets;
   }
 
-  // FIXME: avoid that list is empty -> has to be -1 if no relevant lanelet
-  // exists, this fixes an issue that originates in the lanelet converter; should be fixed there!
   if (ids_relevant_successors.empty()) {
     ids_relevant_successors.push_back(-1);
   }
@@ -664,4 +662,13 @@ void CalculatePredecessors(std::vector<LaneletConnection> & lanelet_connections)
   }
 }
 
+unsigned int ID::ReturnIDAndIncrement()
+{
+  if (value_ == std::numeric_limits<unsigned int>::max()) {
+    RCLCPP_WARN(rclcpp::get_logger("ID"), "ID overflow");
+    value_ = 0;  // Reset value_ to 0
+  }
+  return value_++;
+}
+
 }  // namespace autoware::mapless_architecture

planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp

@@ -15,6 +15,7 @@
 #ifndef AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_
 #define AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_
 
+#include "autoware/local_mission_planner_common/helper_functions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp"
@@ -201,6 +202,9 @@ class MissionLaneConverterNode : public rclcpp::Node
   // ROS parameters
   float target_speed_;
   std::string local_map_frame_;
+
+  // Unique ID for each marker
+  ID marker_id_;
 };
 }  // namespace autoware::mapless_architecture
 

planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp

@@ -161,7 +161,6 @@ void MissionLaneConverterNode::MissionLanesCallback_(
   path_publisher_->publish(path_msg);
   path_publisher_global_->publish(path_msg_global);
 
-  // TODO(thomas.herrmann@driveblocks.ai): outsource this to a separate method
   // Clear all markers in scene
   visualization_msgs::msg::Marker msg_marker;
   msg_marker.header = msg_mission.header;
@@ -239,8 +238,12 @@ MissionLaneConverterNode::ConvertMissionToTrajectory(
         trj_msg, path_msg, trj_vis, path_center_vis, msg.ego_lane.centerline);
 
       // Fill path bounds left and right
-      CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1);
-      CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2);
+      CreatePathBound_(
+        path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left,
+        marker_id_.ReturnIDAndIncrement());
+      CreatePathBound_(
+        path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right,
+        marker_id_.ReturnIDAndIncrement());
       break;
     case -1:
       // Lane change to the left
@@ -249,18 +252,24 @@ MissionLaneConverterNode::ConvertMissionToTrajectory(
 
       // Fill path bounds left and right
       CreatePathBound_(
-        path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left, 1);
-      CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2);
+        path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left,
+        marker_id_.ReturnIDAndIncrement());
+      CreatePathBound_(
+        path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right,
+        marker_id_.ReturnIDAndIncrement());
       break;
     case 1:
       // Lane change to the right
       CreateMotionPlannerInput_(
         trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right[0].centerline);
 
       // Fill path bounds left and right
-      CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1);
       CreatePathBound_(
-        path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right, 2);
+        path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left,
+        marker_id_.ReturnIDAndIncrement());
+      CreatePathBound_(
+        path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right,
+        marker_id_.ReturnIDAndIncrement());
       break;
     case -2:
       // Take exit left
@@ -269,18 +278,24 @@ MissionLaneConverterNode::ConvertMissionToTrajectory(
 
       // Fill path bounds left and right
       CreatePathBound_(
-        path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left, 1);
-      CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2);
+        path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left,
+        marker_id_.ReturnIDAndIncrement());
+      CreatePathBound_(
+        path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right,
+        marker_id_.ReturnIDAndIncrement());
       break;
     case 2:
       // Take exit right
       CreateMotionPlannerInput_(
         trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right.back().centerline);
 
       // Fill path bounds left and right
-      CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1);
       CreatePathBound_(
-        path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, 2);
+        path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left,
+        marker_id_.ReturnIDAndIncrement());
+      CreatePathBound_(
+        path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right,
+        marker_id_.ReturnIDAndIncrement());
       break;
 
     default:
@@ -374,7 +389,6 @@ void MissionLaneConverterNode::CreatePathBound_(
     }
 
     // Add last added point to a marker message for debugging
-    // FIXME: probably no unique ids for multiple calls?
     AddPointVisualizationMarker_(path_vis, bound_path.back().x, bound_path.back().y, id_marker);
   }
 
@@ -456,8 +470,6 @@ void MissionLaneConverterNode::AddHeadingToTrajectory_(
   return;
 }
 
-// TODO(thomas.herrmann@driveblocks.ai): store the latest odometry message here and then re-use in
-// the output conversion
 void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg)
 {
   // Store current odometry information