diff --git a/CHANGELOG.md b/CHANGELOG.md index 763a047..b58ff03 100644 --- a/CHANGELOG.md +++ b/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) \ No newline at end of file +- [ ] Cleanly handle end-of-path situations (see loop path @ end) diff --git a/config/planner.yaml b/config/planner.yaml new file mode 100644 index 0000000..2ee9ffd --- /dev/null +++ b/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 diff --git a/include/ap1/planning/frames.hpp b/include/ap1/planning/frames.hpp index 3c9b91d..6c2921a 100644 --- a/include/ap1/planning/frames.hpp +++ b/include/ap1/planning/frames.hpp @@ -20,7 +20,7 @@ namespace ap1::planning::frames { struct RouteF { - std::vector route; + std::vector route; std::vector 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 \ No newline at end of file +#endif // AP1_PLANNING_FRAMES_HPP diff --git a/include/ap1/planning/math_utils.hpp b/include/ap1/planning/math_utils.hpp index a436e70..af31f39 100644 --- a/include/ap1/planning/math_utils.hpp +++ b/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 diff --git a/include/ap1/planning/planner_node.hpp b/include/ap1/planning/planner_node.hpp index dc6baaf..a489195 100644 --- a/include/ap1/planning/planner_node.hpp +++ b/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. diff --git a/include/ap1/planning/waypoint_utils.hpp b/include/ap1/planning/waypoint_utils.hpp index 574e78a..5cc0f52 100644 --- a/include/ap1/planning/waypoint_utils.hpp +++ b/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& navigable_waypoints); +int locate_closest_waypoint(const ap1::math::vec2f& target_location, + const std::vector& 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 generate_waypoint_sequence(const std::vector& waypoints, const int to, - const std::vector& fallback_path); +std::vector generate_waypoint_sequence(const std::vector& waypoints, const int to, + const std::vector& fallback_path); /** * @brief Finds the next waypoint in the path. @@ -73,7 +73,7 @@ std::vector generate_waypoint_sequence(const std::vector& waypoint * * @return long */ -long find_next_waypoint_idx(const std::vector& centerline); +long find_next_waypoint_idx(const std::vector& centerline); } // namespace ap1::planning diff --git a/src/behaviours.cpp b/src/behaviours.cpp index 42294d7..5b41cc2 100644 --- a/src/behaviours.cpp +++ b/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; diff --git a/src/event_generator.cpp b/src/event_generator.cpp index 4a5f0c6..94e8b36 100644 --- a/src/event_generator.cpp +++ b/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 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> 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 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; } diff --git a/src/frames.cpp b/src/frames.cpp index b55bce9..fb7fd84 100644 --- a/src/frames.cpp +++ b/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; diff --git a/src/math_utils.cpp b/src/math_utils.cpp index 656c84e..0857c09 100644 --- a/src/math_utils.cpp +++ b/src/math_utils.cpp @@ -1,8 +1,7 @@ #include "ap1/planning/math_utils.hpp" #include -// 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 diff --git a/src/planner_node.cpp b/src/planner_node.cpp index 85f3ceb..9bd8483 100644 --- a/src/planner_node.cpp +++ b/src/planner_node.cpp @@ -38,37 +38,59 @@ 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( - "/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( - // "/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( - "/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( - "/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( - "/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( - "/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("/ap1/planning/state", 1); - this->target_path_pub_ = create_publisher("/ap1/planning/target_path", 1); + this->state_pub_ = create_publisher( + this->get_parameter("topics.state").as_string(), 1); + this->target_path_pub_ = create_publisher( + this->get_parameter("topics.target_path").as_string(), 1); this->speed_profile_pub_ = - create_publisher("/ap1/planning/speed_profile", 1); + create_publisher( + this->get_parameter("topics.speed_profile").as_string(), 1); // # Create Planning Loop @ rate_hz timer_ = create_wall_timer( @@ -76,6 +98,11 @@ PlannerNode::PlannerNode(double rate_hz, std::string transitions_path) [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,7 +111,7 @@ 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." @@ -92,6 +119,25 @@ void PlannerNode::planning_loop_callback() 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 \ No newline at end of file +} // namespace ap1::planning diff --git a/src/waypoint_utils.cpp b/src/waypoint_utils.cpp index 6f89704..5ebbfa2 100644 --- a/src/waypoint_utils.cpp +++ b/src/waypoint_utils.cpp @@ -8,8 +8,8 @@ namespace ap1::planning { int locate_closest_waypoint( - const vec2f& target_location, - const std::vector& navigable_waypoints + const ap1::math::vec2f& target_location, + const std::vector& 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 generate_waypoint_sequence( - const std::vector& waypoints, +std::vector generate_waypoint_sequence( + const std::vector& waypoints, const int to, - const std::vector& fallback_path + const std::vector& fallback_path ) { // Check if waypoints is empty OR to == -1 (no valid target) @@ -45,7 +45,7 @@ std::vector generate_waypoint_sequence( } // Create a new vector to hold the sequence - std::vector sequence; + std::vector 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 generate_waypoint_sequence( * * @return size_t */ -long find_next_waypoint_idx(const std::vector& centerline) +long find_next_waypoint_idx(const std::vector& 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; }