Tweaks across planning: resolved all TODOs

CHANGELOG.md

@@ -21,13 +21,13 @@
 
 ### v0.2.1 - WIP
 - [ ] Re-add target_speed control
-- [ ] Move `math_utils` unti `ap1::planning::math` namespace.
-- [ ] Add age check on incoming packets
-- [ ] Move intial state publish to init instead of updating every planning frame
+- [x] Move `math_utils` unti ~~`ap1::planning::math`~~ `ap1::math` namespace.
+- [x] Add age check on incoming packets
+- [x] Move intial state publish to init instead of updating every planning frame
 
 ## v0.3 | Controlled Navigation
 ### v0.3.0 - WIP
 - [ ] Add emergency stop
 - [ ] Support intersections and navigate through them
 - [ ] Navigate to a determined position safely
-- [ ] Cleanly handle end-of-path situations (see loop path @ end)
+- [ ] Cleanly handle end-of-path situations (see loop path @ end)

config/planner.yaml

@@ -0,0 +1,11 @@
+planner_node:
+  ros__parameters:
+    topics:
+      lanes:         /ap1/mapping/lanes
+      target_speed:  /ap1/control/target_speed
+      odometer:      /ap1/mapping/odometer
+      entities:      /ap1/mapping/entities
+      speed:         /ap1/actuation/speed
+      state:         /ap1/planning/state
+      target_path:   /ap1/planning/target_path
+      speed_profile: /ap1/planning/speed_profile

include/ap1/planning/frames.hpp

@@ -20,7 +20,7 @@
 namespace ap1::planning::frames {
 struct RouteF
 {
-    std::vector<ap1::planning::vec2f> route;
+    std::vector<ap1::math::vec2f> route;
     std::vector<float> speed_profile;
 };
 
@@ -39,4 +39,4 @@ struct MapF
 void unwrap_route_f(const RouteF& route, ap1_msgs::msg::TargetPathStamped &path, ap1_msgs::msg::SpeedProfileStamped &speed_profile);
 } // namespace ap1::planning::frames
 
-#endif // AP1_PLANNING_FRAMES_HPP
+#endif // AP1_PLANNING_FRAMES_HPP

include/ap1/planning/math_utils.hpp

@@ -1,7 +1,7 @@
 #ifndef AP1_PLANNING_MATH_UTILS_HPP
 #define AP1_PLANNING_MATH_UTILS_HPP
 
-namespace ap1::planning {
+namespace ap1::math {
 /**
  * @brief 2D vector with floating point coordinates
  *
@@ -31,6 +31,6 @@ float distance(const vec2f& next, const vec2f& target);
  */
 float magnitude(const vec2f& v);
 float magnitude(float x, float y);
-} // ap1::planning
+} // ap1::math
 
 #endif // AP1_PLANNING_MATH_UTILS_HPP

include/ap1/planning/planner_node.hpp

@@ -84,6 +84,9 @@ class PlannerNode : public rclcpp::Node
     void on_entities(const EntityStateArray::SharedPtr entities);
     void on_target_location(const geometry_msgs::msg::Point::SharedPtr loc);
 
+    // Age Check
+    static constexpr double DATA_TTL_SEC = 0.5;
+
     /**
      * @brief Planning loop callback runs rate_hz times per second.
      * This callback is responsible for sending commands & updates to control.

include/ap1/planning/waypoint_utils.hpp

@@ -28,8 +28,8 @@ namespace ap1::planning
  *   - Lane waypoints are at: [(8, 0), (9, 0), (10, 0), (11, 0)]
  *   - Function returns 2 (index of waypoint at (10, 0), closest to target)
  */
-int locate_closest_waypoint(const vec2f& target_location,
-                            const std::vector<vec2f>& navigable_waypoints);
+int locate_closest_waypoint(const ap1::math::vec2f& target_location,
+                            const std::vector<ap1::math::vec2f>& navigable_waypoints);
 
 /**
  * @brief Generate a sequence of waypoints from current position to destination
@@ -61,8 +61,8 @@ int locate_closest_waypoint(const vec2f& target_location,
  *   - fallback_path = [(5, 2)] (direct to target)
  *   - Returns [(5, 2)] (car drives directly to target)
  */
-std::vector<vec2f> generate_waypoint_sequence(const std::vector<vec2f>& waypoints, const int to,
-                                              const std::vector<vec2f>& fallback_path);
+std::vector<ap1::math::vec2f> generate_waypoint_sequence(const std::vector<ap1::math::vec2f>& waypoints, const int to,
+                                              const std::vector<ap1::math::vec2f>& fallback_path);
 
 /**
  * @brief Finds the next waypoint in the path.
@@ -73,7 +73,7 @@ std::vector<vec2f> generate_waypoint_sequence(const std::vector<vec2f>& waypoint
  *
  * @return long
  */
-long find_next_waypoint_idx(const std::vector<vec2f>& centerline);
+long find_next_waypoint_idx(const std::vector<ap1::math::vec2f>& centerline);
 
 } // namespace ap1::planning
 

src/behaviours.cpp

@@ -14,6 +14,7 @@
 #include "ap1_msgs/msg/lane_boundaries.hpp"
 
 using namespace ap1::planning;
+using namespace ap1::math;
 
 using ap1_msgs::msg::EntityState;
 using ap1_msgs::msg::EntityStateArray;

src/event_generator.cpp

@@ -47,40 +47,49 @@ ap1::planning::EventGenerator::EventGenerator() {}
 /**
  * Get the event generator to figure out all the events in the frame.
  * There are 4 events to check, see ap1::planning::fsm::Event for all options.
- *
- * TODO: This data flow is super clunky and ugly and should be fixed.
  */
 std::vector<Event> ap1::planning::EventGenerator::update(
     const MapF& frame,
     fsm::StateContext &ctx,
     rclcpp::Time now
-) {
-    // output var
+)
+{
+    // Determine the time that has been taken to stop so far.
+    const rclcpp::Duration stop_duration = ctx.stop_entry_time.has_value()
+        ? now - ctx.stop_entry_time.value()  // Given the recorded entry time.
+        : rclcpp::Duration::from_seconds(0);
+
+    // Determine the distance that has been driven through.
+    const double drive_through_distance = ctx.drive_through_start_distance
+            .has_value()
+        ? frame.odometer - ctx.drive_through_start_distance.value()
+            // Given the entry distance.
+        : 0.0;
+
+    // Check which events have been triggered.
+    const std::vector<std::pair<bool, Event>> checks = {
+        { 
+            sign_is_close(frame.entities),
+            Event::SignDetected
+        },
+        { 
+            drive_through_distance > DRIVE_THROUGH_DISTANCE,
+            Event::DriveThruDistanceCovered
+        },
+        {
+            frame.speed <= EPSILON,
+            Event::HasStopped
+        },
+        {
+            stop_duration > rclcpp::Duration::from_seconds(MIN_STOP_DURATION),
+            Event::StopTimeElapsed
+        },
+    };
+
+    // Add the triggered events to the event vector and return it.
     std::vector<Event> events{};
-
-    // do we see a sign?
-    if (sign_is_close(frame.entities)) events.push_back(Event::SignDetected);
-
-    // have we crossed enough distance to exit drive_through?
-    if (ctx.drive_through_start_distance.has_value()) {
-        if (frame.odometer - ctx.drive_through_start_distance.value() > DRIVE_THROUGH_DISTANCE) {
-            events.push_back(Event::DriveThruDistanceCovered);
-        }
-    }
-
-    // have we stopped?
-    if (frame.speed <= 0.f + EPSILON) {
-        events.push_back(Event::HasStopped);
+    for (const auto& [condition, event] : checks) {
+        if (condition) events.push_back(event);
     }
-
-    // has enough time passed?
-    if (ctx.stop_entry_time.has_value()) {
-        rclcpp::Duration stop_duration = now - ctx.stop_entry_time.value();
-
-        if (stop_duration > rclcpp::Duration::from_seconds(MIN_STOP_DURATION)) {
-            events.push_back(Event::StopTimeElapsed);
-        }
-    }
-
     return events;
 }

src/frames.cpp

@@ -14,7 +14,7 @@ using ap1_msgs::msg::SpeedProfileStamped;
 void ap1::planning::frames::unwrap_route_f(const RouteF& route, TargetPathStamped &path, SpeedProfileStamped& speed_profile) {
     // ROUTE WAYPOINTS
     path.path.clear();
-    for (const ap1::planning::vec2f& waypoint : route.route) {
+    for (const ap1::math::vec2f& waypoint : route.route) {
         Point p;
         p.x = waypoint.x;
         p.y = waypoint.y;

src/math_utils.cpp

@@ -1,8 +1,7 @@
 #include "ap1/planning/math_utils.hpp"
 #include <cmath>
 
-// TODO: put into ap1::planning::math
-namespace ap1::planning {
+namespace ap1::math {
 
 float distance(const vec2f& next, const vec2f& target)
 {
@@ -22,4 +21,4 @@ float magnitude(const vec2f& v) {
     return std::sqrt(v.x*v.x + v.y*v.y);
 }
 
-} // namespace ap1::planning
+} // namespace ap1::math

src/planner_node.cpp

@@ -38,44 +38,71 @@ PlannerNode::PlannerNode(double rate_hz, std::string transitions_path)
     : Node("planner_node"), rate_hz_(rate_hz),
       fsm(this->now(), fsm::VehicleState::Driving, transitions_path), event_generator()
 {
+    // Declare path parameters with defaults.
+    this->declare_parameter("topics.lanes",         
+            "/ap1/mapping/lanes");
+    //this->declare_parameter("topics.target_location",
+    //      "/ap1/control/target_location);
+    this->declare_parameter("topics.target_speed",  
+            "/ap1/control/target_speed");
+    this->declare_parameter("topics.odometer",      
+            "/ap1/mapping/odometer");
+    this->declare_parameter("topics.entities",      
+            "/ap1/mapping/entities");
+    this->declare_parameter("topics.speed",         
+            "/ap1/actuation/speed");
+    this->declare_parameter("topics.state",         
+            "/ap1/planning/state");
+    this->declare_parameter("topics.target_path",   
+            "/ap1/planning/target_path");
+    this->declare_parameter("topics.speed_profile", 
+            "/ap1/planning/speed_profile");
+
     // # Subscribe to all inputs
-    // todo: paths should be loaded from config
     this->lane_sub_ = create_subscription<LaneBoundaries>(
-        "/ap1/mapping/lanes", 1,
+        this->get_parameter("topics.lanes").as_string(), 1,
         [this](LaneBoundaries::SharedPtr msg) { this->on_lanes(msg); });
 
     // this->target_location_sub_ = create_subscription<Point>(
-    //     "/ap1/control/target_location", 1,
+    //     this->get_parameter("topics.target_location").as_string(), 1,
     //     [this](Point::SharedPtr msg) { this->on_target_location(msg); });
 
     this->target_speed_sub_ = create_subscription<FloatStamped>(
-        "/ap1/control/target_speed", 1,
+        this->get_parameter("topics.target_speed").as_string(), 1,
         [this](FloatStamped::SharedPtr msg) { this->on_target_speed(msg); });
 
     this->odometer_sub_ = create_subscription<FloatStamped>(
-        "/ap1/mapping/odometer", 1,
+        this->get_parameter("topics.odometer").as_string(), 1,
         [this](FloatStamped::SharedPtr msg) { this->on_odometer(msg); });
 
     this->entities_sub_ = create_subscription<EntityStateArray>(
-        "/ap1/mapping/entities", 1,
+        this->get_parameter("topics.entities").as_string(), 1,
         [this](EntityStateArray::SharedPtr msg) { this->on_entities(msg); });
 
     this->speed_sub_ = create_subscription<FloatStamped>(
-        "/ap1/actuation/speed", 1,
+        this->get_parameter("topics.speed").as_string(), 1,
         [this](FloatStamped::SharedPtr msg) { this->on_speed(msg); });
 
     // # Publishers
-    this->state_pub_ = create_publisher<std_msgs::msg::String>("/ap1/planning/state", 1);
-    this->target_path_pub_ = create_publisher<TargetPathStamped>("/ap1/planning/target_path", 1);
+    this->state_pub_ = create_publisher<std_msgs::msg::String>(
+        this->get_parameter("topics.state").as_string(), 1);
+    this->target_path_pub_ = create_publisher<TargetPathStamped>(
+        this->get_parameter("topics.target_path").as_string(), 1);
     this->speed_profile_pub_ =
-        create_publisher<SpeedProfileStamped>("/ap1/planning/speed_profile", 1);
+        create_publisher<SpeedProfileStamped>(
+        this->get_parameter("topics.speed_profile").as_string(), 1);
 
     // # Create Planning Loop @ rate_hz
     timer_ = create_wall_timer(
         std::chrono::duration<double>(1.0f / rate_hz),
         [this]() { this->planning_loop_callback(); });
 
     RCLCPP_INFO(this->get_logger(), "Path Planner Node initialized.");
+
+    // Publish state info once for startup.
+    std_msgs::msg::String msg;
+    msg.data = fsm::to_string(this->fsm.current_state).value();
+    this->state_pub_->publish(msg);
 }
 
 // # Methods
@@ -84,14 +111,33 @@ void PlannerNode::planning_loop_callback()
 {
     // check that we have all the necessary fields
     if (this->odometer_ == nullptr || this->current_lane_ == nullptr || this->entities_ == nullptr)
-    { // TODO: add age check here too
+    {
         RCLCPP_WARN_THROTTLE(
             this->get_logger(), *this->get_clock(), 5000,
             "1 or more necessary field is null. Skipping loop."
         );
         return;
     }
 
+    // Check that field data is not too old. 
+    const rclcpp::Time now = this->get_clock()->now();
+    const auto odometer_age = (now - this->odometer_->header.stamp).seconds();
+    const auto current_lane_age = (now - this->current_lane_->header.stamp)
+        .seconds();
+    const auto entities_age = (now - this->entities_->header.stamp).seconds();
+
+    if (odometer_age > DATA_TTL_SEC || current_lane_age > DATA_TTL_SEC 
+            || entities_age > DATA_TTL_SEC)
+    {
+        RCLCPP_WARN_THROTTLE(
+            this->get_logger(), *this->get_clock(), 5000,
+            "Stale data detected (odom: %.2fs, lane: %.2fs, entities: %.2fs). "
+            "Skipping loop.",
+            odometer_age, current_lane_age, entities_age
+        );
+        return;
+    }
+
     // assemble frame
     const frames::MapF map_f{this->speed, this->odometer_->value, this->now(), *this->current_lane_, *this->entities_};
 
@@ -128,12 +174,6 @@ void PlannerNode::planning_loop_callback()
     // publish
     target_path_pub_->publish(path);
     speed_profile_pub_->publish(speed_profile);
-
-    // TODO: move this to init somehow - no need to publish every frame
-    // Publish quick default state message for console
-    std_msgs::msg::String msg;
-    msg.data = fsm::to_string(this->fsm.current_state).value();
-    this->state_pub_->publish(msg);
 }
 
 // # Callbacks
@@ -165,4 +205,4 @@ void PlannerNode::on_entities(const EntityStateArray::SharedPtr msg)
     this->entities_ = msg;
 }
 
-} // namespace ap1::planning
+} // namespace ap1::planning

src/waypoint_utils.cpp

@@ -8,8 +8,8 @@
 namespace ap1::planning {
 
 int locate_closest_waypoint(
-    const vec2f& target_location,
-    const std::vector<vec2f>& navigable_waypoints
+    const ap1::math::vec2f& target_location,
+    const std::vector<ap1::math::vec2f>& navigable_waypoints
 ) {
     // Check if navigable_waypoints is empty
     if (navigable_waypoints.empty()) {
@@ -32,10 +32,10 @@ int locate_closest_waypoint(
     return closest_index;
 }
 
-std::vector<vec2f> generate_waypoint_sequence(
-    const std::vector<vec2f>& waypoints,
+std::vector<ap1::math::vec2f> generate_waypoint_sequence(
+    const std::vector<ap1::math::vec2f>& waypoints,
     const int to,
-    const std::vector<vec2f>& fallback_path
+    const std::vector<ap1::math::vec2f>& fallback_path
 )
 {
     // Check if waypoints is empty OR to == -1 (no valid target)
@@ -45,7 +45,7 @@ std::vector<vec2f> generate_waypoint_sequence(
     }
 
     // Create a new vector to hold the sequence
-    std::vector<vec2f> sequence;
+    std::vector<ap1::math::vec2f> sequence;
 
     // Copy waypoints from index 0 to index 'to' (inclusive)
     // This creates the path from the car's position to the target
@@ -63,10 +63,10 @@ std::vector<vec2f> generate_waypoint_sequence(
  *
  * @return size_t
  */
-long find_next_waypoint_idx(const std::vector<vec2f>& centerline)
+long find_next_waypoint_idx(const std::vector<ap1::math::vec2f>& centerline)
 {
     // find the closest waypoint to us (ahead or behind)
-    long closest_waypoint_idx = locate_closest_waypoint(vec2f{0.f, 0.f}, centerline);
+    long closest_waypoint_idx = locate_closest_waypoint(ap1::math::vec2f{0.f, 0.f}, centerline);
     if (closest_waypoint_idx == -1) {
         return -1;
     }