diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp index cb2308272d..2cb8abb8dc 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.hpp @@ -89,34 +89,34 @@ namespace basic_autonomy double buffer_ending_downtrack = 20.0; //The additional downtrack beyond requested end dist used to fit points along spline std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory }; - + /** * \brief Applies the provided speed limits to the provided speeds such that each element is capped at its corresponding speed limit if needed - * + * * \param speeds The speeds to limit * \param speed_limits The speed limits to apply. Must have the same size as speeds - * + * * \return The capped speed limits. Has the same size as speeds */ std::vector apply_speed_limits(const std::vector speeds, const std::vector speed_limits); /** * \brief Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis - * + * * \param p1 The origin point for the frame in the parent frame * \param p2 A point in the parent frame that will define the +X axis relative to p1 - * + * * \return A 2D coordinate frame transform */ Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2); /** * \brief Reduces the input points to only those points that fit within the provided time boundary - * + * * \param points The input point speed pairs to reduce * \param time_span The time span in seconds which the output points will fit within - * + * * \return The subset of points that fit within time_span */ std::vector constrain_to_time_boundary(const std::vector &points, double time_span); @@ -125,33 +125,33 @@ namespace basic_autonomy * \brief Returns the min, and its idx, from the vector of values, excluding given set of values * \param values vector of values * \param excluded set of excluded values - * + * * \return minimum value and its idx */ std::pair min_with_exclusions(const std::vector &values, const std::unordered_set &excluded); /** * \brief Applies the longitudinal acceleration limit to each point's speed - * + * * \param downtracks downtrack distances corresponding to each speed * \param curv_speeds vehicle velocity in m/s. * \param accel_limit vehicle longitudinal acceleration in m/s^2. - * + * * \return optimized speeds for each dowtrack points that satisfies longitudinal acceleration */ std::vector optimize_speed(const std::vector &downtracks, const std::vector &curv_speeds, double accel_limit); /** * \brief Method combines input points, times, orientations, and an absolute start time to form a valid carma platform trajectory - * + * * NOTE: All input vectors must be the same size. The output vector will take this size. - * + * * \param points The points in the map frame that the trajectory will follow. Units m * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s * \param yaws The orientation the vehicle should achieve at each point. Units radians * \param startTime The absolute start time which will be used to update the input relative times. Units s * \param desired_controller_plugin The name of the controller plugin for the generated trajectory. - * + * * \return A list of trajectory points built from the provided inputs. */ std::vector trajectory_from_points_times_orientations( @@ -160,12 +160,12 @@ namespace basic_autonomy /** * \brief Attaches back_distance length of points behind the future points - * + * * \param points_set all point speed pairs * \param future_points future points before which to attach the points - * \param nearest_pt_index idx of the first future_point in points_set + * \param nearest_pt_index idx of the first future_point in points_set * \param back_distance the back distance to be added, in meters - * + * * \return point speed pairs with back distance length of points in front of future points * NOTE- used to add past points to future trajectory for smooth spline calculation */ @@ -175,7 +175,7 @@ namespace basic_autonomy /** * \brief Computes a spline based on the provided points * \param basic_points The points to use for fitting the spline - * + * * \return A spline which has been fit to the provided points */ std::unique_ptr compute_fit(const std::vector &basic_points); @@ -184,7 +184,7 @@ namespace basic_autonomy * \brief Given the curvature fit, computes the curvature at the given step along the curve * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature * \param fit_curve curvature fit - * + * * \return Curvature (k = 1/r, 1/meter) */ double compute_curvature_at(const basic_autonomy::smoothing::SplineI &fit_curve, double step_along_the_curve); @@ -193,7 +193,7 @@ namespace basic_autonomy * \brief Creates geometry profile to return a point speed pair struct for LANE FOLLOW and LANE CHANGE maneuver types * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated speed * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called @@ -206,20 +206,20 @@ namespace basic_autonomy const carma_planning_msgs::msg::VehicleState& state,const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); /** - * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. + * \brief Converts a set of requested LANE_FOLLOWING maneuvers to point speed limit pairs. * \param maneuvers The list of maneuvers to convert geometry points and calculate associated speed * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * + * * \return List of centerline points paired with speed limits */ std::vector create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, - const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, + const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set& visited_lanelets); - + /** * \brief Adds extra centerline points beyond required message length to lane follow maneuver points so that there's always enough points to calculate trajectory * (BUFFER POINTS SHOULD BE REMOVED BEFORE RETURNING FINAL TRAJECTORY) @@ -228,50 +228,50 @@ namespace basic_autonomy * \param maneuvers The list of lane follow maneuvers which were converted to geometry points and associated speed * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * - * \return List of centerline points paired with speed limits returned with added buffer - */ + * + * \return List of centerline points paired with speed limits returned with added buffer + */ std::vector add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector& points_and_target_speeds, const std::vector &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config); /** * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning for a Lane following maneuver. - * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. * \param state The current state of the vehicle * \param state_time The abosolute time which the provided vehicle state corresponds to - * + * * \return A list of trajectory points to send to the carma planning stack */ std::vector compose_lanefollow_trajectory_from_path(const std::vector &points, const carma_planning_msgs::msg::VehicleState &state, - const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, + const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds& debug_msg, const DetailedTrajConfig &detailed_config); //Functions specific to lane change /** - * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. - * + * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. + * * \param maneuvers The list of maneuvers to convert * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. * If the first maneuver exceeds this then it's downtrack will be shifted to this value. - * + * * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * + * * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver */ std::vector get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config); - + /** - * \brief Creates a vector of lane change points using parameters defined. - * + * \brief Creates a vector of lane change points using parameters defined. + * * \param starting_lane_id lanelet id for where lane change plan should start * \param ending_lane_id lanelet id for where lane change plan should end * \param starting_downtrack The downtrack distance from which the lane change maneuver starts @@ -279,19 +279,19 @@ namespace basic_autonomy * \param wm Pointer to intialized world model for semantic map access * \param downsample_ratio TODO: add description * \param buffer_ending_downtrack The additional downtrack beyond requested end dist used to fit points along spline - * + * * \return A vector of geometry points as lanelet::basicpoint2d */ std::vector create_lanechange_geometry(lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double starting_downtrack, double ending_downtrack, const carma_wm::WorldModelConstPtr &wm, int downsample_ratio, double buffer_ending_downtrack); - - + + /** - * \brief Resamples a pair of basicpoint2d lines to get lines of same number of points. - * + * \brief Resamples a pair of basicpoint2d lines to get lines of same number of points. + * * \param line_1 a vector of points to be resampled * \param line_2 a vector of points to be resampled - * + * * \return A 2d vector with input lines resampled at same rate. The first iteration is the resampled line_1 and the resampled line_2 is the second iteration * Assumption here is for lane change to happen between two adjacent lanelets, they must share a lane boundary (linestring) */ @@ -299,14 +299,14 @@ namespace basic_autonomy /** * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning for a Lane following maneuver. - * - * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * + * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. * \param state The current state of the vehicle * \param state_time The abosolute time which the provided vehicle state corresponds to * \param wm The carma world model object which the vehicle is operating in. * \param ending_state_before_buffer The vehicle state before a buffer was added to the points. Used to revert the trajectory to required distance before returning. - * + * * \return A list of trajectory points to send to the carma planning stack */ std::vector compose_lanechange_trajectory_from_path( @@ -315,11 +315,11 @@ namespace basic_autonomy const DetailedTrajConfig &detailed_config); /** - * \brief Creates a Lanelet2 Linestring from a vector or points along the geometry + * \brief Creates a Lanelet2 Linestring from a vector or points along the geometry * \param starting_downtrack downtrack along route where maneuver starts * \param ending_downtrack downtrack along route where maneuver starts * \param wm Pointer to intialized world model for semantic map access - * + * * \return Points in a path from starting downtrack to ending downtrack */ std::vector create_route_geom(double starting_downtrack, int starting_lane_id, @@ -329,7 +329,7 @@ namespace basic_autonomy * \brief Given a start and end point, create a vector of points fit through a spline between the points (using a Spline library) * \param start_lanelet The lanelet from which lane change starts * \param end_lanelet The lanelet in which lane change ends - * + * * \return A linestring path from start to end fit through Spline Library */ lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet); @@ -348,12 +348,12 @@ namespace basic_autonomy GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type, int default_downsample_ratio, int turn_downsample_ratio); - + /** * \brief Given a carma type of trajectory_plan, generate autoware type of trajectory accounting for speed_lag and stopping case * Generated trajectory is meant to be used in autoware.auto's pure_pursuit library using set_trajectory() function * \param tp trajectory plan from tactical plugins - * + * * \return trajectory plan of autoware_auto_msgs type */ autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan& tp, double vehicle_response_lag); @@ -363,10 +363,36 @@ namespace basic_autonomy * \param speeds Velocity profile to shift. The first point should be the current vehicle speed * \param downtrack Distance points for each velocity point. Should have the same size as speeds and start from 0 * \param response_lag The lag in seconds before which the vehicle will not meaningfully accelerate - * + * * \return A Shifted trajectory - */ + */ std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag); + + /** + * \brief Applies a yield trajectory to the original trajectory set in response + * \param node_handler a node interface to use the logger and time + * \param req The service request containing the maneuvers to plan trajectories for and current vehicle state + * \param resp The original response containing the planned trajectory to be modified + * \param yield_client yield_client to call for + * \param yield_plugin_service_call_timeout yield client's timeout in milliseconds int + * + * \return The original response modified to contain the modified planned trajectory + */ + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( + const std::shared_ptr& node_handler, + const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req, + const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp, + const carma_ros2_utils::ClientPtr& yield_client, + int yield_plugin_service_call_timeout); + + /** + * \brief Helper function to verify if the input yield trajectory plan is valid + * \param node_handler node of which time to compare against the trajectory's time + * \param yield_plan input yield trajectory plan + * + * \return true or false + */ + bool is_valid_yield_plan(const std::shared_ptr& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan); } } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 309b2ffaf8..7174a825c0 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -25,7 +25,7 @@ namespace basic_autonomy carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config){ std::vector points_and_target_speeds; - + bool first = true; std::unordered_set visited_lanelets; @@ -33,7 +33,7 @@ namespace basic_autonomy for(const auto &maneuver : maneuvers) { double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); - + if(first){ starting_downtrack = std::min(starting_downtrack, max_starting_downtrack); first = false; @@ -53,10 +53,10 @@ namespace basic_autonomy else{ throw std::invalid_argument("This maneuver type is not supported"); } - + } - //Add buffer ending to lane follow points at the end of maneuver(s) end dist + //Add buffer ending to lane follow points at the end of maneuver(s) end dist if(maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, maneuvers, ending_state_before_buffer, detailed_config); @@ -66,7 +66,7 @@ namespace basic_autonomy } std::vector create_lanefollow_geometry(const carma_planning_msgs::msg::Maneuver &maneuver, double starting_downtrack, - const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, + const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set &visited_lanelets) { if(maneuver.type != carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ @@ -110,7 +110,7 @@ namespace basic_autonomy // Add extra lanelet to ensure there are sufficient points for buffer auto extra_following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); - + for (auto llt : wm->getRoute()->shortestPath()) { for (size_t i = 0; i < extra_following_lanelets.size(); i++) @@ -122,7 +122,7 @@ namespace basic_autonomy } } } - + if (lanelets.empty()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Detected no lanelets between starting downtrack: "<< starting_downtrack << ", and lane_following_maneuver.end_dist: "<< lane_following_maneuver.end_dist); @@ -132,10 +132,10 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Maneuver"); lanelet::BasicLineString2d downsampled_centerline; - // 400 value here is an arbitrary attempt at improving inlane-cruising performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving inlane-cruising performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing downsampled_centerline.reserve(400); - + //getLaneletsBetween is inclusive of lanelets between its two boundaries //which may return lanechange lanelets, so //exclude lanechanges and plan for only the straight part @@ -151,7 +151,7 @@ namespace basic_autonomy else { // skip all lanechanges until lane follow starts - while (curr_idx + 1 < lanelets.size() && + while (curr_idx + 1 < lanelets.size() && std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) == following_lanelets.end()) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "As there were no directly following lanelets after this, skipping lanelet id: " << lanelets[curr_idx].id()); @@ -163,7 +163,7 @@ namespace basic_autonomy // guaranteed to have at least one "straight" lanelet (e.g the last one in the list) straight_lanelets.push_back(lanelets[curr_idx]); // add all lanelets on the straight road until next lanechange - while (curr_idx + 1 < lanelets.size() && + while (curr_idx + 1 < lanelets.size() && std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) != following_lanelets.end()) { curr_idx++; @@ -171,9 +171,9 @@ namespace basic_autonomy straight_lanelets.push_back(lanelets[curr_idx]); following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]); } - + } - + for (auto l : straight_lanelets) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Processing lanelet ID: " << l.id()); @@ -185,7 +185,7 @@ namespace basic_autonomy std::string turn_direction = l.attribute("turn_direction").value(); is_turn = turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0; } - + lanelet::BasicLineString2d centerline = l.centerline2d().basicLineString(); lanelet::BasicLineString2d downsampled_points; if (is_turn) { @@ -193,7 +193,7 @@ namespace basic_autonomy } else { downsampled_points = carma_ros2_utils::containers::downsample_vector(centerline, general_config.default_downsample_ratio); } - + if(downsampled_centerline.size() != 0 && downsampled_points.size() != 0 // If this is not the first lanelet and the points are closer than 1m drop the first point to prevent overlap && lanelet::geometry::distance2d(downsampled_points.front(), downsampled_centerline.back()) <1.2){ downsampled_points = lanelet::BasicLineString2d(downsampled_points.begin() + 1, downsampled_points.end()); @@ -217,19 +217,19 @@ namespace basic_autonomy pair.speed = lane_following_maneuver.end_speed; points_and_target_speeds.push_back(pair); } - + return points_and_target_speeds; } std::vector add_lanefollow_buffer(const carma_wm::WorldModelConstPtr &wm, std::vector& points_and_target_speeds, const std::vector &maneuvers, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config){ - + double starting_route_downtrack = wm->routeTrackPos(points_and_target_speeds.front().point).downtrack; // Always try to add the maximum buffer. Even if the route ends it may still be possible to add buffered points. - // This does mean that downstream components might not be able to assume the buffer points are on the route + // This does mean that downstream components might not be able to assume the buffer points are on the route // though this is not likely to be an issue as they are buffer only double ending_downtrack = maneuvers.back().lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack; @@ -245,7 +245,7 @@ namespace basic_autonomy boost::optional delta_point; for (size_t i = 0; i < points_and_target_speeds.size(); ++i) { auto current_point = points_and_target_speeds[i].point; - + if (i == 0) { prev_point = current_point; continue; @@ -254,7 +254,7 @@ namespace basic_autonomy double delta_d = lanelet::geometry::distance2d(prev_point, current_point); dist_accumulator += delta_d; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << "current_point.y():" << current_point.y()); if (dist_accumulator > maneuvers.back().lane_following_maneuver.end_dist && !found_unbuffered_idx) { @@ -262,7 +262,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Found index unbuffered_idx at: " << unbuffered_idx); found_unbuffered_idx = true; } - + if (dist_accumulator > ending_downtrack) { max_i = i; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Max_i breaking at: i: " << i << ", max_i: " << max_i); @@ -281,7 +281,7 @@ namespace basic_autonomy delta_point = (current_point - prev_point) * 0.25; // Use a smaller step size then default to help ensure enough points are generated; } - // Create an extrapolated new point + // Create an extrapolated new point auto new_point = current_point + delta_point.get(); PointSpeedPair new_pair; @@ -296,7 +296,7 @@ namespace basic_autonomy ending_state_before_buffer.x_pos_global = points_and_target_speeds[unbuffered_idx].point.x(); ending_state_before_buffer.y_pos_global = points_and_target_speeds[unbuffered_idx].point.y(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Here ending_state_before_buffer.x_pos_global: " << ending_state_before_buffer.x_pos_global << + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Here ending_state_before_buffer.x_pos_global: " << ending_state_before_buffer.x_pos_global << ", and y_pos_global" << ending_state_before_buffer.y_pos_global); std::vector constrained_points(points_and_target_speeds.begin(), points_and_target_speeds.begin() + max_i); @@ -312,17 +312,17 @@ namespace basic_autonomy //Get starting lanelet and ending lanelets lanelet::ConstLanelet starting_lanelet = wm->getMap()->laneletLayer.get(starting_lane_id); lanelet::ConstLanelet ending_lanelet = wm->getMap()->laneletLayer.get(ending_lane_id); - + lanelet::ConstLanelets starting_lane; starting_lane.push_back(starting_lanelet); std::vector reference_centerline; - // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing reference_centerline.reserve(400); bool shared_boundary_found = false; bool is_lanechange_left = false; - + lanelet::BasicLineString2d current_lanelet_centerline = starting_lanelet.centerline2d().basicLineString(); lanelet::ConstLanelet current_lanelet = starting_lanelet; reference_centerline.insert(reference_centerline.end(), current_lanelet_centerline.begin(), current_lanelet_centerline.end()); @@ -330,7 +330,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Searching for shared boundary with starting lanechange lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id())); while(!shared_boundary_found){ //Assumption- Adjacent lanelets share lane boundary - if(current_lanelet.leftBound() == ending_lanelet.rightBound()){ + if(current_lanelet.leftBound() == ending_lanelet.rightBound()){ RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Lanelet " << std::to_string(current_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id())); is_lanechange_left = true; shared_boundary_found = true; @@ -351,14 +351,14 @@ namespace basic_autonomy throw(std::invalid_argument("No following lanelets from current lanelet reachable without a lane change, incorrectly chosen end lanelet")); } - current_lanelet = wm->getMapRoutingGraph()->following(current_lanelet, false).front(); + current_lanelet = wm->getMapRoutingGraph()->following(current_lanelet, false).front(); if(current_lanelet.id() == starting_lanelet.id()){ //Looped back to starting lanelet throw(std::invalid_argument("No lane change in path")); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Now checking for shared lane boundary with lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id())); - auto current_lanelet_linestring = current_lanelet.centerline2d().basicLineString(); - //Concatenate linestring starting from + 1 to avoid overlap + auto current_lanelet_linestring = current_lanelet.centerline2d().basicLineString(); + //Concatenate linestring starting from + 1 to avoid overlap reference_centerline.insert(reference_centerline.end(), current_lanelet_linestring.begin() + 1, current_lanelet_linestring.end()); starting_lane.push_back(current_lanelet); } @@ -368,9 +368,9 @@ namespace basic_autonomy std::vector target_lane_centerline; for(size_t i = 0;igetMapRoutingGraph()->left(starting_lane[i])){ curr_end_lanelet = wm->getMapRoutingGraph()->left(starting_lane[i]).get(); @@ -389,15 +389,15 @@ namespace basic_autonomy curr_end_lanelet = wm->getMapRoutingGraph()->adjacentRight(starting_lane[i]).get(); } } - + auto target_lane_linestring = curr_end_lanelet.centerline2d().basicLineString(); //Concatenate linestring starting from + 1 to avoid overlap target_lane_centerline.insert(target_lane_centerline.end(), target_lane_linestring.begin() + 1, target_lane_linestring.end()); - + } //Downsample centerlines - // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. + // 400 value here is an arbitrary attempt at improving performance by reducing copy operations. // Value picked based on annecdotal evidence from STOL system testing std::vector downsampled_starting_centerline; @@ -451,7 +451,7 @@ namespace basic_autonomy // Add points from the remaining length of the target lanelet to provide sufficient distance for adding buffer double dist_to_target_lane_end = lanelet::geometry::distance2d(centerline_points.back(), downsampled_target_centerline.back()); centerline_points.insert(centerline_points.end(), downsampled_target_centerline.begin() + end_index_target_centerline, downsampled_target_centerline.end()); - + // If the additional distance from the remaining length of the target lanelet does not provide more than the required // buffer_ending_downtrack, then also add points from the lanelet following the target lanelet if (dist_to_target_lane_end < buffer_ending_downtrack) { @@ -459,7 +459,7 @@ namespace basic_autonomy if(!following_lanelets.empty()){ //Arbitrarily choosing first following lanelet for buffer since points are only being used to fit spline auto following_lanelet_centerline = following_lanelets.front().centerline2d().basicLineString(); - centerline_points.insert(centerline_points.end(), following_lanelet_centerline.begin(), + centerline_points.insert(centerline_points.end(), following_lanelet_centerline.begin(), following_lanelet_centerline.end()); } } @@ -468,11 +468,11 @@ namespace basic_autonomy } std::vector> resample_linestring_pair_to_same_size(std::vector& line_1, std::vector& line_2){ - + auto start_time = std::chrono::high_resolution_clock::now(); // Start timing the execution time for planning so it can be logged std::vector> output; - + //Fit centerlines to a spline std::unique_ptr fit_curve_1 = compute_fit(line_1); // Compute splines based on curve points if (!fit_curve_1) @@ -498,17 +498,17 @@ namespace basic_autonomy //double step_threshold_line1 = (double)total_step_along_curve1 / (double)total_point_size; //TODO: are we missing some computation here? step_threshold_line1 and step_threshold_line2 are not used anywhere // and these calcs can be deleted (see below also). - + all_sampling_points_line2.reserve(1 + total_point_size * 2); std::vector downtracks_raw_line2 = carma_wm::geometry::compute_arc_lengths(line_2); //TODO: unused variable: int total_step_along_curve2 = static_cast(downtracks_raw_line2.back() / 2.0); //TODO: unused variable: double step_threshold_line2 = (double)total_step_along_curve2 / (double)total_point_size; double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory - - + + all_sampling_points_line2.reserve(1 + total_point_size * 2); - + for(size_t i = 0;i(end_time - start_time); RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "ExecutionTime for resample lane change centerlines: " << duration.count() << " milliseconds"); return output; } - + std::vector get_lanechange_points_from_maneuver(const carma_planning_msgs::msg::Maneuver &maneuver, double starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config) @@ -562,10 +562,10 @@ namespace basic_autonomy ending_state_before_buffer.x_pos_global = route_geometry[ending_pt_index].x(); ending_state_before_buffer.y_pos_global = route_geometry[ending_pt_index].y(); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "ending_state_before_buffer_:"<getRouteEndTrackPos().downtrack; if (ending_downtrack + detailed_config.buffer_ending_downtrack < route_length) @@ -593,13 +593,13 @@ namespace basic_autonomy //If current speed is above min speed, keep at current speed. Otherwise use end speed from maneuver. pair.speed = (state.longitudinal_vel > detailed_config.minimum_speed) ? state.longitudinal_vel : lane_change_maneuver.end_speed; points_and_target_speeds.push_back(pair); - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Const speed assigned:"< apply_speed_limits(const std::vector speeds, const std::vector speed_limits) @@ -639,7 +639,7 @@ namespace basic_autonomy size_t time_boundary_exclusive_index = trajectory_utils::time_boundary_index(downtracks, speeds, time_span); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "time_boundary_exclusive_index = " << time_boundary_exclusive_index); if (time_boundary_exclusive_index == 0) @@ -767,7 +767,7 @@ namespace basic_autonomy tpp.x = points[i].x(); tpp.y = points[i].y(); tpp.yaw = yaws[i]; - + tpp.controller_plugin_name = desired_controller_plugin; //tpp.planner_plugin_name //Planner plugin name is filled in the tactical plugin @@ -810,13 +810,13 @@ namespace basic_autonomy RCLCPP_WARN_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Insufficient Spline Points"); return nullptr; } - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Original basic_points size: " << basic_points.size()); std::vector resized_basic_points = basic_points; // The large the number of points, longer it takes to calculate a spline fit - // So if the basic_points vector size is large, only the first 400 points are used to compute a spline fit. + // So if the basic_points vector size is large, only the first 400 points are used to compute a spline fit. if (resized_basic_points.size() > 400) { resized_basic_points.resize(400); @@ -832,7 +832,7 @@ namespace basic_autonomy RCLCPP_WARN_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "More than half of basic points are ignored for spline fitting"); } } - + std::unique_ptr spl = std::make_unique(); spl->setPoints(resized_basic_points); @@ -965,7 +965,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Corresponding to point: x: " << all_sampling_points[buffer_pt_index].x() << ", y:" << all_sampling_points[buffer_pt_index].y()); if(nearest_pt_index + 1 >= buffer_pt_index){ - + lanelet::BasicPoint2d current_pos(state.x_pos_global, state.y_pos_global); lanelet::BasicPoint2d ending_pos(ending_state_before_buffer.x_pos_global, ending_state_before_buffer.y_pos_global); @@ -1061,8 +1061,8 @@ namespace basic_autonomy msg.lat_accel_limit = detailed_config.lateral_accel_limit; msg.lon_accel_limit = detailed_config.max_accel; msg.starting_state = state; - debug_msg = msg; - + debug_msg = msg; + return traj_points; } @@ -1139,7 +1139,7 @@ namespace basic_autonomy current_vehicle_point); // Add current vehicle position to front of future geometry points final_actual_speeds.insert(final_actual_speeds.begin(), state.longitudinal_vel); - + //Compute points to local downtracks std::vector downtracks = carma_wm::geometry::compute_arc_lengths(future_geom_points); @@ -1152,7 +1152,7 @@ namespace basic_autonomy all_sampling_points.push_back(p); - scaled_steps_along_curve += 1.0 / total_step_along_curve; + scaled_steps_along_curve += 1.0 / total_step_along_curve; } RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Got sampled points with size:" << all_sampling_points.size()); @@ -1220,7 +1220,7 @@ namespace basic_autonomy if (stopping_index != 0 && i >= stopping_index - 1) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Made it to 0, i: " << i); - + speeds[i] = 0.0; //stopping case } else @@ -1231,13 +1231,13 @@ namespace basic_autonomy std::vector lag_speeds; lag_speeds = apply_response_lag(speeds, downtracks, vehicle_response_lag); // This call requires that the first speed point be current speed to work as expected - + autoware_auto_msgs::msg::Trajectory autoware_trajectory; autoware_trajectory.header = tp.header; RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "size: " << trajectory_points.size()); - + auto max_size = std::min(99, (int)trajectory_points.size()); //NOTE: more than this size autoware auto raises exception with "Exceeded upper bound while in ACTIVE state." - //large portion of the points are not needed anyways + //large portion of the points are not needed anyways for (int i = 0; i < max_size; i++) { autoware_auto_msgs::msg::TrajectoryPoint autoware_point; @@ -1247,7 +1247,7 @@ namespace basic_autonomy autoware_point.longitudinal_velocity_mps = lag_speeds[i]; autoware_point.time_from_start = rclcpp::Duration(times[i] * 1e9); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Setting waypoint idx: " << i <<", with planner: << " << trajectory_points[i].planner_plugin_name << ", x: " << trajectory_points[i].x << + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Setting waypoint idx: " << i <<", with planner: << " << trajectory_points[i].planner_plugin_name << ", x: " << trajectory_points[i].x << ", y: " << trajectory_points[i].y << ", speed: " << lag_speeds[i]* 2.23694 << "mph"); autoware_trajectory.points.push_back(autoware_point); @@ -1256,8 +1256,8 @@ namespace basic_autonomy return autoware_trajectory; } - - std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) + + std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) { // Note first speed is assumed to be vehicle speed if (speeds.size() != downtracks.size()) { throw std::invalid_argument("Speed list and downtrack list are not the same size."); @@ -1275,6 +1275,76 @@ namespace basic_autonomy output = trajectory_utils::shift_by_lookahead(speeds, (unsigned int) lookahead_count); return output; } + + bool is_valid_yield_plan(const std::shared_ptr& node_handler, const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) + { + if (yield_plan.trajectory_points.size() < 2) + { + RCLCPP_WARN(node_handler->get_logger(), "Invalid Yield Trajectory"); + } + + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); + RCLCPP_DEBUG_STREAM(node_handler->get_logger(), "Now:" << node_handler->now().seconds()); + + if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > node_handler->now()) + { + return true; + } + else + { + RCLCPP_DEBUG(node_handler->get_logger(), "Old Yield Trajectory"); + } + + return false; + } + + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr modify_trajectory_to_yield_to_obstacles( + const std::shared_ptr& node_handler, + const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req, + const carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr& resp, + const carma_ros2_utils::ClientPtr& yield_client, + int yield_plugin_service_call_timeout) + { + RCLCPP_DEBUG(node_handler->get_logger(), "Activate Object Avoidance"); + + if (!yield_client || !yield_client->service_is_ready()) + { + throw std::runtime_error("Yield Client is not set or unavailable after configuration state of lifecycle"); + } + + RCLCPP_DEBUG(node_handler->get_logger(), "Yield Client is valid"); + + auto yield_srv = std::make_shared(); + yield_srv->initial_trajectory_plan = resp->trajectory_plan; + yield_srv->vehicle_state = req->vehicle_state; + + auto yield_resp = yield_client->async_send_request(yield_srv); + + auto future_status = yield_resp.wait_for(std::chrono::milliseconds(yield_plugin_service_call_timeout)); + + if (future_status != std::future_status::ready) + { + // Sometimes the yield plugin's service call may be unsuccessful due to its computationally expensive logic. + // However, consecutive calls can be successful, so return original trajectory for now + RCLCPP_WARN(node_handler->get_logger(), "Service request to yield plugin timed out waiting on a reply from the service server"); + return resp; + } + + RCLCPP_DEBUG(node_handler->get_logger(), "Received Traj from Yield"); + carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; + if (is_valid_yield_plan(node_handler, yield_plan)) + { + RCLCPP_DEBUG(node_handler->get_logger(), "Yield trajectory validated"); + resp->trajectory_plan = yield_plan; + } + else + { + throw std::invalid_argument("Invalid Yield Trajectory"); + } + + return resp; + } + } // namespace waypoint_generation } // basic_autonomy \ No newline at end of file diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp index 31bf7efaed..8196db4678 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -645,10 +645,10 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 1, 1); const waypoint_generation::DetailedTrajConfig config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20); carma_planning_msgs::msg::VehicleState ending_state; - - std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + + std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cwm, ending_state, state, general_config, config); - + EXPECT_EQ(points.back().speed, state.longitudinal_vel); @@ -656,9 +656,9 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) int centerline_size = shortest_path.back().centerline2d().size(); maneuver.lane_change_maneuver.end_dist = cwm->routeTrackPos(shortest_path.back().centerline2d()[centerline_size/2]).downtrack; maneuvers[0] = maneuver; - points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cwm, ending_state, state, general_config, config); - + EXPECT_TRUE(points.size() > 4); //Test resample linestring @@ -732,8 +732,8 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 1, 1); const waypoint_generation::DetailedTrajConfig config = waypoint_generation::compose_detailed_trajectory_config(0, 1, 0, 0, 0, 5, 0, 0, 20); carma_planning_msgs::msg::VehicleState ending_state; - - std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + + std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, starting_downtrack, cmw, ending_state, state, general_config, config); rclcpp::Time state_time(10 * 1e9); // Arbitrarily selected 10 seconds (converted to nanoseconds) EXPECT_EQ(points.back().speed, state.longitudinal_vel); @@ -773,9 +773,9 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) maneuver.lane_following_maneuver.end_speed = 6.7056; maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); // 4.4704 seconds converted to nanoseconds - - std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); EXPECT_EQ(visited_lanelets.size(), 3); @@ -815,12 +815,12 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); // 4.4704 seconds converted to nanoseconds // No defined lanelet ids in the maneuver msg - EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); - - // Defined lanelet ids in the maneuver msg + + // Defined lanelet ids in the maneuver msg maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); - std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); EXPECT_EQ(visited_lanelets.size(), 3); EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set @@ -828,11 +828,73 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) // Non-following lanelet id in the maneuver msg maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1202)); - EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, - starting_downtrack, wm, general_config, + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); } + + TEST(BasicAutonomyTest, test_verify_yield) + { + auto node = std::make_shared(rclcpp::NodeOptions()); + + std::vector trajectory_points; + + rclcpp::Time startTime = node->now(); + + carma_planning_msgs::msg::TrajectoryPlanPoint point_2; + point_2.x = 5.0; + point_2.y = 0.0; + point_2.target_time = startTime + rclcpp::Duration(1, 0); + point_2.lane_id = "1"; + trajectory_points.push_back(point_2); + + carma_planning_msgs::msg::TrajectoryPlanPoint point_3; + point_3.x = 10.0; + point_3.y = 0.0; + point_3.target_time = startTime + rclcpp::Duration(2, 0); + point_3.lane_id = "1"; + trajectory_points.push_back(point_3); + + + carma_planning_msgs::msg::TrajectoryPlan tp; + tp.trajectory_points = trajectory_points; + + bool res = basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp); + ASSERT_TRUE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp)); + + carma_planning_msgs::msg::TrajectoryPlan tp2; + + carma_planning_msgs::msg::TrajectoryPlanPoint point_4; + point_4.x = 5.0; + point_4.y = 0.0; + point_4.target_time = startTime + rclcpp::Duration(1, 0); + point_4.lane_id = "1"; + tp2.trajectory_points.push_back(point_4); + + ASSERT_FALSE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp2)); + + carma_planning_msgs::msg::TrajectoryPlan tp3; + + carma_planning_msgs::msg::TrajectoryPlanPoint point_5; + point_5.x = 5.0; + point_5.y = 0.0; + point_5.target_time = startTime; + point_5.lane_id = "1"; + tp3.trajectory_points.push_back(point_5); + + carma_planning_msgs::msg::TrajectoryPlanPoint point_6; + point_6.x = 10.0; + point_6.y = 0.0; + point_6.target_time = startTime + rclcpp::Duration(1, 0); + point_6.lane_id = "1"; + tp3.trajectory_points.push_back(point_6); + + ASSERT_FALSE(basic_autonomy::waypoint_generation::is_valid_yield_plan(node, tp2)); + + } + + } // namespace basic_autonomy // Run all the tests @@ -849,4 +911,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return success; -} \ No newline at end of file +} \ No newline at end of file diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 2dbf8bf905..391c43672d 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate') control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate') - + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') inlanecruising_plugin_file_path = os.path.join( @@ -52,37 +52,37 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') light_controlled_intersection_tactical_plugin_param_file = os.path.join( - get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') cooperative_lanechange_param_file = os.path.join( - get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') + get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') platoon_strategic_ihp_param_file = os.path.join( - get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') + get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') sci_strategic_plugin_file_path = os.path.join( - get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') + get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') lci_strategic_plugin_file_path = os.path.join( - get_package_share_directory('lci_strategic_plugin'), 'config/parameters.yaml') + get_package_share_directory('lci_strategic_plugin'), 'config/parameters.yaml') stop_and_dwell_strategic_plugin_container_file_path = os.path.join( - get_package_share_directory('stop_and_dwell_strategic_plugin'), 'config/parameters.yaml') - + get_package_share_directory('stop_and_dwell_strategic_plugin'), 'config/parameters.yaml') + yield_plugin_file_path = os.path.join( - get_package_share_directory('yield_plugin'), 'config/parameters.yaml') + get_package_share_directory('yield_plugin'), 'config/parameters.yaml') platoon_tactical_ihp_param_file = os.path.join( - get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') approaching_emergency_vehicle_plugin_param_file = os.path.join( get_package_share_directory('approaching_emergency_vehicle_plugin'), 'config/parameters.yaml') - + stop_controlled_intersection_tactical_plugin_file_path = os.path.join( - get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -101,7 +101,7 @@ def generate_launch_description(): plugin='inlanecruising_plugin::InLaneCruisingPluginNode', name='inlanecruising_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('inlanecruising_plugin', env_log_levels) } ], remappings = [ @@ -132,7 +132,7 @@ def generate_launch_description(): plugin='route_following_plugin::RouteFollowingPlugin', name='route_following_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) } ], remappings = [ @@ -165,7 +165,7 @@ def generate_launch_description(): plugin='approaching_emergency_vehicle_plugin::ApproachingEmergencyVehiclePlugin', name='approaching_emergency_vehicle_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('approaching_emergency_vehicle_plugin', env_log_levels) } ], remappings = [ @@ -206,7 +206,7 @@ def generate_launch_description(): plugin='stop_and_wait_plugin::StopandWaitNode', name='stop_and_wait_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) } ], remappings = [ @@ -236,7 +236,7 @@ def generate_launch_description(): plugin='sci_strategic_plugin::SCIStrategicPlugin', name='sci_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('sci_strategic_plugin', env_log_levels) } ], remappings = [ @@ -271,7 +271,7 @@ def generate_launch_description(): plugin='lci_strategic_plugin::LCIStrategicPlugin', name='lci_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('lci_strategic_plugin', env_log_levels) } ], remappings = [ @@ -306,7 +306,7 @@ def generate_launch_description(): plugin='stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin', name='stop_controlled_intersection_tactical_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_controlled_intersection_tactical_plugin', env_log_levels) } ], remappings = [ @@ -336,7 +336,7 @@ def generate_launch_description(): plugin='cooperative_lanechange::CooperativeLaneChangePlugin', name='cooperative_lanechange', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) } ], remappings = [ @@ -374,7 +374,7 @@ def generate_launch_description(): plugin='yield_plugin::YieldPluginNode', name='yield_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('yield_plugin', env_log_levels) } ], remappings = [ @@ -409,7 +409,7 @@ def generate_launch_description(): plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', name='light_controlled_intersection_tactical_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) } ], remappings = [ @@ -423,7 +423,7 @@ def generate_launch_description(): parameters=[ vehicle_config_param_file, light_controlled_intersection_tactical_plugin_param_file - ] + ] ), ] ) @@ -439,7 +439,7 @@ def generate_launch_description(): plugin='pure_pursuit_wrapper::PurePursuitWrapperNode', name='pure_pursuit_wrapper', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('pure_pursuit_wrapper', env_log_levels) } ], remappings = [ @@ -456,7 +456,7 @@ def generate_launch_description(): ), ] ) - + platooning_strategic_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_strategic_plugin_container', @@ -490,14 +490,14 @@ def generate_launch_description(): ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), ], - parameters=[ + parameters=[ platoon_strategic_ihp_param_file, vehicle_config_param_file ] ), ] ) - + platooning_tactical_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_tactical_plugin_container', @@ -537,7 +537,7 @@ def generate_launch_description(): plugin='stop_and_dwell_strategic_plugin::StopAndDwellStrategicPlugin', name='stop_and_dwell_strategic_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('stop_and_dwell_strategic_plugin', env_log_levels) } ], remappings = [ @@ -570,29 +570,31 @@ def generate_launch_description(): plugin='intersection_transit_maneuvering::IntersectionTransitManeuveringNode', name='intersection_transit_maneuvering', extra_arguments=[ - {'use_intra_process_comms': True}, + {'use_intra_process_comms': True}, {'--log-level' : GetLogLevel('intersection_transit_maneuvering', env_log_levels) } ], remappings = [], - parameters=[] + parameters=[ + vehicle_config_param_file + ] ), ] ) - return LaunchDescription([ - carma_inlanecruising_plugin_container, - carma_route_following_plugin_container, + return LaunchDescription([ + carma_inlanecruising_plugin_container, + carma_route_following_plugin_container, carma_approaching_emergency_vehicle_plugin_container, - carma_stop_and_wait_plugin_container, - carma_sci_strategic_plugin_container, + carma_stop_and_wait_plugin_container, + carma_sci_strategic_plugin_container, carma_stop_and_dwell_strategic_plugin_container, - carma_lci_strategic_plugin_container, - carma_stop_controlled_intersection_tactical_plugin_container, - carma_cooperative_lanechange_plugins_container, - carma_yield_plugin_container, - carma_light_controlled_intersection_plugins_container, - carma_pure_pursuit_wrapper_container, - #platooning_strategic_plugin_container, + carma_lci_strategic_plugin_container, + carma_stop_controlled_intersection_tactical_plugin_container, + carma_cooperative_lanechange_plugins_container, + carma_yield_plugin_container, + carma_light_controlled_intersection_plugins_container, + carma_pure_pursuit_wrapper_container, + #platooning_strategic_plugin_container, platooning_tactical_plugin_container, intersection_transit_maneuvering_container diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 36d3fa3194..9d9ee8d615 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -82,10 +82,10 @@ namespace carma_wm } lanelet::Id CARMAWorldModel::getTrafficSignalId(uint16_t intersection_id, uint8_t signal_group_id) - { + { lanelet::Id inter_id = lanelet::InvalId; lanelet::Id signal_id = lanelet::InvalId; - + if (sim_.intersection_id_to_regem_id_.find(intersection_id) != sim_.intersection_id_to_regem_id_.end()) { inter_id = sim_.intersection_id_to_regem_id_[intersection_id]; @@ -95,7 +95,7 @@ namespace carma_wm { signal_id = sim_.signal_group_to_traffic_light_id_[signal_group_id]; } - + return signal_id; } @@ -133,15 +133,15 @@ namespace carma_wm std::vector bus_stop_list; auto curr_downtrack = routeTrackPos(loc).downtrack; // shortpath is already sorted by distance - + for (const auto &ll : route_->shortestPath()) { auto bus_stops = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs(); - if (bus_stops.empty()) + if (bus_stops.empty()) { continue; } - + for (auto bus_stop : bus_stops) { auto stop_line = bus_stop->stopAndWaitLine(); @@ -335,7 +335,7 @@ namespace carma_wm if (!bounds_inclusive) // reduce bounds slightly to avoid including exact bounds { - if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001) + if (std::max(min.downtrack, start + 0.00001) > std::min(max.downtrack, end - 0.00001) || (start == end && (min.downtrack >= start || max.downtrack <= end))) { // Check for 1d intersection // No intersection so continue @@ -344,7 +344,7 @@ namespace carma_wm } else { - if (std::max(min.downtrack, start) > std::min(max.downtrack, end) + if (std::max(min.downtrack, start) > std::min(max.downtrack, end) || (start == end && (min.downtrack > start || max.downtrack < end))) { // Check for 1d intersection // No intersection so continue @@ -699,7 +699,7 @@ namespace carma_wm // Append distance to current centerline size_t offset = lineStrings.size() == 0 || lineStrings.back().size() == 0 ? - 0 : + 0 : 1; // Prevent duplicate points when concatenating. Not clear if causes an issue at lane changes if ((nextCenterline.size() <= 1) || (nextCenterline.size() <= 2 && offset == 1)) { @@ -1222,7 +1222,7 @@ namespace carma_wm std::vector light_list; auto curr_downtrack = routeTrackPos(loc).downtrack; // shortpath is already sorted by distance - + for (const auto &ll : route_->shortestPath()) { auto lights = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs(); @@ -1230,7 +1230,7 @@ namespace carma_wm { continue; } - + for (auto light : lights) { auto stop_line = light->getStopLine(ll); @@ -1380,7 +1380,7 @@ namespace carma_wm } lanelet::CarmaTrafficSignalPtr CARMAWorldModel::getTrafficSignal(const lanelet::Id& id) const - { + { auto general_regem = semantic_map_->regulatoryElementLayer.get(id); auto lanelets_general = semantic_map_->laneletLayer.findUsages(general_regem); @@ -1464,9 +1464,9 @@ namespace carma_wm RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm"), "No intersection_state_list in the newly received SPAT msg. Returning..."); return; } - + for (const auto& curr_intersection : spat_msg.intersection_state_list) - { + { for (const auto& current_movement_state : curr_intersection.movement_list) { @@ -1478,7 +1478,7 @@ namespace carma_wm } lanelet::CarmaTrafficSignalPtr curr_light = getTrafficSignal(curr_light_id); - + if (curr_light == nullptr) { continue; @@ -1518,11 +1518,11 @@ namespace carma_wm start_time_dynamic=min_end_time_converter_minute_of_year(start_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy, use_sim_time); // Accounting minute of the year auto received_state_dynamic = static_cast(current_movement_event.event_state.movement_phase_state); - + sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic)); sim_.traffic_signal_start_times_[curr_intersection.id.id][current_movement_state.signal_group].push_back( start_time_dynamic); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm"), "intersection id: " << (int)curr_intersection.id.id << ", signal: " << (int)current_movement_state.signal_group << ", start_time: " << std::to_string(lanelet::time::toSec(start_time_dynamic)) << ", end_time: " << std::to_string(lanelet::time::toSec(min_end_time_dynamic)) diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index 8beb6736dd..517ef490f2 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -25,7 +25,7 @@ namespace carma_wm { -enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP, +enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP, DIRECTION_OF_TRAVEL, STOP_RULE, CARMA_TRAFFIC_LIGHT, SIGNALIZED_INTERSECTION/* ... others */ }; // helper function that return geofence type as an enum, which makes it cleaner by allowing switch statement GeofenceType resolveGeofenceType(const std::string& rule_name) @@ -66,7 +66,7 @@ void WMListenerWorker::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::Un // After setting map evaluate the current update queue to apply any updates that arrived before the map bool more_updates_to_apply = true; while(!map_update_queue_.empty() && more_updates_to_apply) { - + auto update = std::move(map_update_queue_.front()); // Get first update map_update_queue_.pop(); // Remove update from queue @@ -119,34 +119,35 @@ void WMListenerWorker::enableUpdatesWithoutRoute() // helper function to log SignalizedIntersectionManager content void logSignalizedIntersectionManager(const carma_wm::SignalizedIntersectionManager& sim) { - for (auto pair : sim.intersection_id_to_regem_id_) + for (auto const &[intersection_id, regulatory_element_id] : sim.intersection_id_to_regem_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "inter id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), + "inter id: " << intersection_id << ", regem id: " << regulatory_element_id); } - for (auto pair : sim.signal_group_to_entry_lanelet_ids_) + for (auto const &[signal_id, entry_llt_ids] : sim.signal_group_to_entry_lanelet_ids_) { - for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + for (const auto & entry_llt_id : entry_llt_ids) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", entry llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", entry llt id: " << entry_llt_id); } } - for (auto pair : sim.signal_group_to_exit_lanelet_ids_) + for (auto const &[signal_id, exit_llt_ids] : sim.signal_group_to_exit_lanelet_ids_) { - for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + for (const auto & exit_llt_id : exit_llt_ids) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", exit llt id: " << *iter); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", exit llt id: " << exit_llt_id); } } - for (auto pair : sim.signal_group_to_traffic_light_id_) + for (auto const &[signal_id, regem_id] : sim.signal_group_to_traffic_light_id_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)pair.first << ", regem id: " << pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "signal id: " << (int)signal_id << ", regem id: " << regem_id); } } void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr geofence_msg) { RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Map Update Being Evaluated. SeqNum: " << geofence_msg->seq_id); - if (rerouting_flag_) // no update should be applied if rerouting + if (rerouting_flag_) // no update should be applied if rerouting { RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Currently new route is being processed. Queueing this update. Received seq: " << geofence_msg->seq_id << " prev seq: " << most_recent_update_msg_seq_); map_update_queue_.emplace(std::move(geofence_msg)); @@ -170,7 +171,7 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un if(geofence_msg->invalidates_route==true && world_model_->getRoute()) - { + { rerouting_flag_=true; recompute_route_flag_ = true; @@ -187,21 +188,21 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un most_recent_update_msg_seq_ = geofence_msg->seq_id; // Update current sequence count auto gf_ptr = std::shared_ptr(new carma_wm::TrafficControl); - + // convert ros msg to geofence object carma_wm::fromBinMsg(*geofence_msg, gf_ptr, world_model_->getMutableMap()); RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Processing Map Update with Geofence Id:" << gf_ptr->id_); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests addition of lanelets size: " << gf_ptr->lanelet_additions_.size()); for (auto llt : gf_ptr->lanelet_additions_) { // world model here should blindly accept the map update received RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new lanelet with id: " << llt.id()); auto left = llt.leftBound3d(); //new lanelet coming in - + // updating incoming points' memory addresses with local ones of same ids - // so that lanelet library can recognize they are same objects + // so that lanelet library can recognize they are same objects for (size_t i = 0; i < left.size(); i ++) { if (world_model_->getMutableMap()->pointLayer.exists(left[i].id())) //rewrite the memory address of new pts with that of local @@ -222,10 +223,10 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of traffic_lights_id size: " << gf_ptr->traffic_light_id_lookup_.size()); - for (auto pair : gf_ptr->traffic_light_id_lookup_) + for (auto const &[traffic_light_id, lanelet_id] : gf_ptr->traffic_light_id_lookup_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << pair.first << ", and lanelet::Id: " << pair.second); - world_model_->setTrafficLightIds(pair.first, pair.second); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Adding new pair for traffic light ids: " << traffic_light_id << ", and lanelet::Id: " << lanelet_id); + world_model_->setTrafficLightIds(traffic_light_id, lanelet_id); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " sends record of intersections size: " << gf_ptr->sim_.intersection_id_to_regem_id_.size()); @@ -236,9 +237,9 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size()); - for (auto pair : gf_ptr->remove_list_) + for (auto const &[lanelet_id, lanelet_to_remove] : gf_ptr->remove_list_) { - auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first); + auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id); // we can only check by id, if the element is there // this is only for speed optimization, as world model here should blindly accept the map update received auto regems_copy_to_check = parent_llt.regulatoryElements(); // save local copy as the regem can be deleted during iteration @@ -246,38 +247,38 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un for (auto regem: regems_copy_to_check) { // we can't use the deserialized element as its data address conflicts the one in this node - if (pair.second->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem); + if (lanelet_to_remove->id() == regem->id()) world_model_->getMutableMap()->remove(parent_llt, regem); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Regems left in lanelet after removal: " << parent_llt.regulatoryElements().size()); } - + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Geofence id" << gf_ptr->id_ << " requests update of size: " << gf_ptr->update_list_.size()); // we should extract general regem to specific type of regem the geofence specifies - for (auto pair : gf_ptr->update_list_) + for (auto const &[lanelet_id, lanelet_to_update]: gf_ptr->update_list_) { - auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(pair.first); + auto parent_llt = world_model_->getMutableMap()->laneletLayer.get(lanelet_id); - auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(pair.second->id()); + auto regemptr_it = world_model_->getMutableMap()->regulatoryElementLayer.find(lanelet_to_update->id()); // if this regem is already in the map. // This section is expected to be called to add back regulations which were previously removed by expired geofences. - if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end()) + if (regemptr_it != world_model_->getMutableMap()->regulatoryElementLayer.end()) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Reapplying previously existing element for lanelet id:" << parent_llt.id() << ", and regem id: " << regemptr_it->get()->id()); // again we should use the element with correct data address to be consistent world_model_->getMutableMap()->update(parent_llt, *regemptr_it); } - else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block. + else // Updates are treated as new regulations after the old value was removed. In both cases we enter this block. { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << pair.second->id()); - newRegemUpdateHelper(parent_llt, pair.second.get()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "New regulatory element at lanelet: " << parent_llt.id() << ", and id: " << lanelet_to_update->id()); + newRegemUpdateHelper(parent_llt, lanelet_to_update.get()); } } - + // set the Map to trigger a new route graph construction if rerouting was required by the updates and a new graph was not provided world_model_->setMap(world_model_->getMutableMap(), current_map_version_, recompute_route_flag_ && !geofence_msg->has_routing_graph ); @@ -298,9 +299,9 @@ void WMListenerWorker::mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::Un // no need to reroute again unless received invalidated msg again if (recompute_route_flag_) recompute_route_flag_ = false; - - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Finished Applying the Map Update with Geofence Id:" << gf_ptr->id_); // Call user defined map callback if (map_callback_) @@ -365,7 +366,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid region access rule"); } - + break; } case GeofenceType::DIGITAL_MINIMUM_GAP: @@ -380,7 +381,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid minimum gap rule"); } - + break; } case GeofenceType::DIRECTION_OF_TRAVEL: @@ -395,7 +396,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid direction of travel"); } - + break; } case GeofenceType::STOP_RULE: @@ -436,7 +437,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet { std::invalid_argument("Dynamic Pointer cast failed on getting valid signalized intersection"); } - + break; } default: @@ -449,8 +450,8 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane if (msg.participant_type.compare(getVehicleParticipationType()) != 0) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: " - << getVehicleParticipationType() + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph does not have matching vehicle type for world model. WM Type: " + << getVehicleParticipationType() << " graph type: " << msg.participant_type ); @@ -543,14 +544,14 @@ LaneletRoutingGraphPtr WMListenerWorker::routingGraphFromMsg(const autoware_lane // Create edge graph->addEdge( - lanelet, - map->laneletLayer.get(vertex.lanelet_or_area_ids[j]), + lanelet, + map->laneletLayer.get(vertex.lanelet_or_area_ids[j]), lanelet::routing::internal::EdgeInfo{vertex.edge_routing_costs[j], vertex.edge_routing_cost_source_ids[j], relation} ); } catch(const lanelet::NoSuchPrimitiveError& e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: " + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"),"Received routing graph specifies lanelets which do not match the current map version. Not found lanelet or area: " << vertex.lanelet_or_area_ids[j] << " Actual exception: " << e.what()); return nullptr; @@ -601,7 +602,7 @@ void WMListenerWorker::routeCallback(const carma_planning_msgs::msg::Route::Uniq while(!map_update_queue_.empty() && more_updates_to_apply) { auto update = std::move(map_update_queue_.front()); // Get first update - map_update_queue_.pop(); // Remove update from queue + map_update_queue_.pop(); // Remove update from queue if (update->map_version < current_map_version_) { // Drop any so far unapplied updates for the current map RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::WMListenerWorker"), "Apply from reroute: There were unapplied updates in carma_wm when a new map was recieved."); @@ -701,7 +702,7 @@ double WMListenerWorker::getConfigSpeedLimit() const } void WMListenerWorker::setVehicleParticipationType(std::string participant) -{ +{ //Function to load participation type into CarmaWorldModel world_model_->setVehicleParticipationType(participant); } diff --git a/inlanecruising_plugin/config/parameters.yaml b/inlanecruising_plugin/config/parameters.yaml index e4183d2647..243badbef3 100644 --- a/inlanecruising_plugin/config/parameters.yaml +++ b/inlanecruising_plugin/config/parameters.yaml @@ -9,5 +9,5 @@ curvature_moving_average_window_size: 9 # Size of the window used in the moving back_distance: 20.0 # Number of meters behind the first maneuver that need to be included in points for curvature calculation max_accel_multiplier: 0.85 # Multiplier of max_accel to bring the value under max_accel lat_accel_multiplier: 0.50 # Multiplier of lat_accel to bring the value under lat_accel TODO needs to be optimized -enable_object_avoidance: true # Activates object avoidance logic -buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points \ No newline at end of file +buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points +#enable_object_avoidance: true # Activates object avoidance logic - set in VehicleConfigParams \ No newline at end of file diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp index 078a3ea632..a61fadb661 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_config.hpp @@ -40,7 +40,8 @@ struct InLaneCruisingPluginConfig bool enable_object_avoidance = true; // Activate object avoidance logic bool publish_debug = false; // True if debug publishing will be enabled double buffer_ending_downtrack = 20.0; - + int tactical_plugin_service_call_timeout = 100; // Tactical plugin service call request timeout in milliseconds + friend std::ostream& operator<<(std::ostream& output, const InLaneCruisingPluginConfig& c) { output << "InLaneCruisingPluginConfig { " << std::endl @@ -59,6 +60,7 @@ struct InLaneCruisingPluginConfig << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "publish_debug: " << c.publish_debug << std::endl << "buffer_ending_downtrack: " << c.buffer_ending_downtrack << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl << "}" << std::endl; return output; } diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp index cd7ee62a3a..e4e88c7860 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp @@ -45,8 +45,8 @@ static const std::string ILC_LOGGER = "inlanecruising_plugin"; /** * \brief Class containing primary business logic for the In-Lane Cruising Plugin - * - */ + * + */ class InLaneCruisingPlugin { public: @@ -58,10 +58,10 @@ class InLaneCruisingPlugin * \param debug_publisher Callback which will publish a debug message. The callback defaults to no-op. * \param plugin_name Retrieved from the plugin node * \param version_id Retrieved from the plugin node - */ - InLaneCruisingPlugin(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const InLaneCruisingPluginConfig& config, + */ + InLaneCruisingPlugin(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const InLaneCruisingPluginConfig& config, const DebugPublisher& debug_publisher=[](const auto& msg){}, const std::string& plugin_name = "inlanecruising_plugin", const std::string& version_id = "v1.0"); @@ -71,28 +71,19 @@ class InLaneCruisingPlugin * \param srv_header header * \param req The service request * \param resp The service response - * - */ + * + */ void plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); /** * \brief set the yield service - * + * * \param yield_srv input yield service */ void set_yield_client(carma_ros2_utils::ClientPtr client); - /** - * \brief verify if the input yield trajectory plan is valid - * - * \param yield_plan input yield trajectory plan - * - * \return true or false - */ - bool validate_yield_plan(const carma_planning_msgs::msg::TrajectoryPlan& yield_plan) const; - carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later private: @@ -105,7 +96,7 @@ class InLaneCruisingPlugin DebugPublisher debug_publisher_; carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_; std::shared_ptr nh_; - + // Access members for unit test FRIEND_TEST(InLaneCruisingPluginTest, rostest1); }; diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp index 421c4be85a..a30d68e43f 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp @@ -33,13 +33,13 @@ namespace inlanecruising_plugin { /** * \brief ROS node for the InLaneCruisingPlugin - */ + */ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin { public: - + /** - * \brief Node constructor + * \brief Node constructor */ explicit InLaneCruisingPluginNode(const rclcpp::NodeOptions &); @@ -47,7 +47,7 @@ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin // Overrides //// carma_ros2_utils::CallbackReturn on_configure_plugin(); - + bool get_availability() override; std::string get_version_id() override final; @@ -55,8 +55,8 @@ class InLaneCruisingPluginNode : public carma_guidance_plugins::TacticalPlugin rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); void plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; private: diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 3b2c83e347..2ff6c6ee00 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -37,9 +37,9 @@ using oss = std::ostringstream; namespace inlanecruising_plugin { -InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const InLaneCruisingPluginConfig& config, +InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const InLaneCruisingPluginConfig& config, const DebugPublisher& debug_publisher, const std::string& plugin_name, const std::string& version_id) @@ -47,7 +47,7 @@ InLaneCruisingPlugin::InLaneCruisingPlugin(std::shared_ptrvehicle_state, wpg_general_config, wpg_detail_config); @@ -97,8 +97,8 @@ void InLaneCruisingPlugin::plan_trajectory_callback( original_trajectory.header.stamp = nh_->now(); original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); - original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, - req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, wpg_detail_config); // Compute the trajectory original_trajectory.initial_longitudinal_velocity = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed); @@ -106,60 +106,24 @@ void InLaneCruisingPlugin::plan_trajectory_callback( for (auto& p : original_trajectory.trajectory_points) { p.planner_plugin_name = plugin_name_; } - + + resp->trajectory_plan = original_trajectory; + // Aside from the flag, ILC should not call yield_plugin on invalid trajectories if (config_.enable_object_avoidance && original_trajectory.trajectory_points.size() >= 2) { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Activate Object Avoidance"); - - if (yield_client_ && yield_client_->service_is_ready()) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield Client is valid"); - - auto yield_srv = std::make_shared(); - yield_srv->initial_trajectory_plan = original_trajectory; - yield_srv->vehicle_state = req->vehicle_state; - - auto yield_resp = yield_client_->async_send_request(yield_srv); - - auto future_status = yield_resp.wait_for(std::chrono::milliseconds(100)); - - if (future_status == std::future_status::ready) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Received Traj from Yield"); - carma_planning_msgs::msg::TrajectoryPlan yield_plan = yield_resp.get()->trajectory_plan; - if (validate_yield_plan(yield_plan)) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield trajectory validated"); - resp->trajectory_plan = yield_plan; - } - else - { - throw std::invalid_argument("Invalid Yield Trajectory"); - } - } - else - { - throw std::invalid_argument("Unable to Call Yield Plugin"); - } - } - else - { - throw std::invalid_argument("Yield Client is unavailable"); - } - + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); } else { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Ignored Object Avoidance"); - resp->trajectory_plan = original_trajectory; + RCLCPP_DEBUG(nh_->get_logger(), "Ignored Object Avoidance"); } if (config_.publish_debug) { // Publish the debug message if in debug logging mode debug_msg_.trajectory_plan = resp->trajectory_plan; - debug_publisher_(debug_msg_); + debug_publisher_(debug_msg_); } - + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -173,27 +137,5 @@ void InLaneCruisingPlugin::set_yield_client(carma_ros2_utils::ClientPtr= 2) - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Yield Trajectory Time" << rclcpp::Time(yield_plan.trajectory_points[0].target_time).seconds()); - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Now:" << nh_->now().seconds()); - if (rclcpp::Time(yield_plan.trajectory_points[0].target_time) + rclcpp::Duration(5.0, 0) > nh_->now()) - { - return true; - } - else - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Old Yield Trajectory"); - } - } - else - { - RCLCPP_DEBUG_STREAM(nh_->get_logger(), "Invalid Yield Trajectory"); - } - return false; -} - } // namespace inlanecruising_plugin \ No newline at end of file diff --git a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp index 86ef0d129c..64672ade24 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp @@ -19,11 +19,11 @@ namespace inlanecruising_plugin { namespace std_ph = std::placeholders; - + InLaneCruisingPluginNode::InLaneCruisingPluginNode(const rclcpp::NodeOptions &options) - : carma_guidance_plugins::TacticalPlugin(options), - plugin_name_(get_plugin_name_and_ns()), - version_id_("v4.0"), + : carma_guidance_plugins::TacticalPlugin(options), + plugin_name_(get_plugin_name_and_ns()), + version_id_("v4.0"), config_(InLaneCruisingPluginConfig()) { // Declare parameters @@ -41,8 +41,9 @@ namespace inlanecruising_plugin config_.max_accel = declare_parameter("vehicle_acceleration_limit", config_.max_accel); config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); } - + carma_ros2_utils::CallbackReturn InLaneCruisingPluginNode::on_configure_plugin() { trajectory_debug_pub_ = create_publisher("debug/trajectory_planning", 1); @@ -63,32 +64,32 @@ namespace inlanecruising_plugin get_parameter("vehicle_acceleration_limit", config_.max_accel); get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); get_parameter("enable_object_avoidance", config_.enable_object_avoidance); - + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&InLaneCruisingPluginNode::parameter_update_callback, this, std_ph::_1)); RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "InLaneCruisingPlugin Params" << config_); - + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; config_.max_accel = config_.max_accel * config_.max_accel_multiplier; - + // Determine if we will enable debug publishing by checking the current log level of the node auto level = rcutils_logging_get_logger_level(get_logger().get_name()); config_.publish_debug = level == RCUTILS_LOG_SEVERITY_DEBUG; RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "InLaneCruisingPlugin Params After Accel Change" << config_); - + worker_ = std::make_shared(shared_from_this(), get_world_model(), config_, [this](const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds& msg) { trajectory_debug_pub_->publish(msg); }, plugin_name_, version_id_); - //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation + //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation yield_client_ = create_client("yield_plugin/plan_trajectory"); worker_->set_yield_client(yield_client_); - RCLCPP_INFO_STREAM(rclcpp::get_logger("inlanecruising_plugin"), "Yield Client Set"); + RCLCPP_INFO(rclcpp::get_logger("inlanecruising_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; @@ -110,9 +111,9 @@ namespace inlanecruising_plugin {"enable_object_avoidance", config_.enable_object_avoidance}}, parameters); auto error_int = update_params({ - {"default_downsample_ratio", config_.default_downsample_ratio}, - {"turn_downsample_ratio", config_.turn_downsample_ratio}, - {"speed_moving_average_window_size", config_.speed_moving_average_window_size}, + {"default_downsample_ratio", config_.default_downsample_ratio}, + {"turn_downsample_ratio", config_.turn_downsample_ratio}, + {"speed_moving_average_window_size", config_.speed_moving_average_window_size}, {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size}}, parameters); rcl_interfaces::msg::SetParametersResult result; @@ -133,8 +134,8 @@ namespace inlanecruising_plugin } void InLaneCruisingPluginNode::plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { worker_->plan_trajectory_callback(req, resp); diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp index b092a82732..f29acc149f 100644 --- a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp @@ -54,69 +54,4 @@ TEST(InLaneCruisingPluginTest, validate_eigen) ASSERT_NEAR(2.0, P_in_A.translation()[0], 0.000000001); ASSERT_NEAR(1.5, P_in_A.translation()[1], 0.000000001); ASSERT_NEAR(M_PI_2, P_in_A_rot.smallestAngle(), 0.000000001); -} - -TEST(InLaneCruisingPluginTest, test_verify_yield) -{ - InLaneCruisingPluginConfig config; - config.enable_object_avoidance = true; - std::shared_ptr wm = std::make_shared(); - auto node = std::make_shared(rclcpp::NodeOptions()); - - InLaneCruisingPlugin plugin(node, wm, config, [&](auto msg) {}); - - std::vector trajectory_points; - - rclcpp::Time startTime = node->now(); - - carma_planning_msgs::msg::TrajectoryPlanPoint point_2; - point_2.x = 5.0; - point_2.y = 0.0; - point_2.target_time = startTime + rclcpp::Duration(1, 0); - point_2.lane_id = "1"; - trajectory_points.push_back(point_2); - - carma_planning_msgs::msg::TrajectoryPlanPoint point_3; - point_3.x = 10.0; - point_3.y = 0.0; - point_3.target_time = startTime + rclcpp::Duration(2, 0); - point_3.lane_id = "1"; - trajectory_points.push_back(point_3); - - - carma_planning_msgs::msg::TrajectoryPlan tp; - tp.trajectory_points = trajectory_points; - - bool res = plugin.validate_yield_plan(tp); - ASSERT_TRUE(plugin.validate_yield_plan(tp)); - - carma_planning_msgs::msg::TrajectoryPlan tp2; - - carma_planning_msgs::msg::TrajectoryPlanPoint point_4; - point_4.x = 5.0; - point_4.y = 0.0; - point_4.target_time = startTime + rclcpp::Duration(1, 0); - point_4.lane_id = "1"; - tp2.trajectory_points.push_back(point_4); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); - - carma_planning_msgs::msg::TrajectoryPlan tp3; - - carma_planning_msgs::msg::TrajectoryPlanPoint point_5; - point_5.x = 5.0; - point_5.y = 0.0; - point_5.target_time = startTime; - point_5.lane_id = "1"; - tp3.trajectory_points.push_back(point_5); - - carma_planning_msgs::msg::TrajectoryPlanPoint point_6; - point_6.x = 10.0; - point_6.y = 0.0; - point_6.target_time = startTime + rclcpp::Duration(1, 0); - point_6.lane_id = "1"; - tp3.trajectory_points.push_back(point_6); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); - -} +} \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp index 54c929285e..9b394b14ba 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -82,20 +82,19 @@ namespace light_controlled_intersection_tactical_plugin /** * \brief Class containing primary business logic for the Light Controlled Intersection Tactical Plugin - * + * */ class LightControlledIntersectionTacticalPlugin { - private: + private: // World Model object carma_wm::WorldModelConstPtr wm_; // Config for this object Config config_; - // Logger for this object - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + std::shared_ptr nh_; // CARMA Streets Variables // timestamp for msg received from carma streets @@ -115,6 +114,7 @@ namespace light_controlled_intersection_tactical_plugin boost::optional last_case_; boost::optional is_last_case_successful_; carma_planning_msgs::msg::TrajectoryPlan last_trajectory_; + carma_ros2_utils::ClientPtr yield_client_; carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later @@ -133,7 +133,7 @@ namespace light_controlled_intersection_tactical_plugin std::string light_controlled_intersection_strategy_ = "Carma/signalized_intersection"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends /** - * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. + * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. * \param wm world_model pointer * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m @@ -144,7 +144,7 @@ namespace light_controlled_intersection_tactical_plugin * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ */ - void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp); /** @@ -153,8 +153,8 @@ namespace light_controlled_intersection_tactical_plugin * \param maneuver Maneuver associated that has starting downtrack and desired entry time * \param starting_speed Starting speed of the vehicle * \param points The set of points with raw speed limits whose speed profile to be changed. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. - * + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * */ void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); @@ -162,7 +162,7 @@ namespace light_controlled_intersection_tactical_plugin * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. * \param wm Pointer to intialized world model for semantic map access * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later * \param state The vehicle state at the time the function is called @@ -197,22 +197,29 @@ namespace light_controlled_intersection_tactical_plugin /*! * \brief LightControlledIntersectionTacticalPlugin constructor */ - LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); + LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, + std::shared_ptr nh); /** * \brief Function to process the light controlled intersection tactical plugin service call for trajectory planning * \param req The service request * \param resp The service response - */ - void planTrajectoryCB( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + */ + void planTrajectoryCB( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + /** + * \brief set the yield service + * + * \param yield_srv input yield service + */ + void set_yield_client(carma_ros2_utils::ClientPtr client); + /** * \brief Setter function to set a new config for this object * \param config The new config to be used by this object - */ + */ void setConfig(const Config& config); }; diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp index 430753e94f..ebe7786898 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp @@ -45,7 +45,9 @@ namespace light_controlled_intersection_tactical_plugin double algorithm_evaluation_period = 4.5; double lateral_accel_limit = 2.5; double vehicle_accel_limit = 2.0; - double vehicle_decel_limit = 2.0; + double vehicle_decel_limit = 2.0; + int tactical_plugin_service_call_timeout = 100; + bool enable_object_avoidance = false; // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const Config &c) @@ -70,6 +72,8 @@ namespace light_controlled_intersection_tactical_plugin << "lateral_accel_limit: " << c.lateral_accel_limit << std::endl << "vehicle_accel_limit: " << c.vehicle_accel_limit << std::endl << "vehicle_decel_limit: " << c.vehicle_decel_limit << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl + << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "}" << std::endl; return output; } diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp index 867e594c1d..0a45425be0 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp @@ -27,46 +27,47 @@ namespace light_controlled_intersection_tactical_plugin { /** * \brief ROS node for the LightControlledIntersectionTransitPluginNode - * + * */ class LightControlledIntersectionTransitPluginNode : public carma_guidance_plugins::TacticalPlugin { - private: + private: // Config for this object Config config_; + carma_ros2_utils::ClientPtr yield_client_; // Worker object std::shared_ptr worker_; public: - + /** - * \brief LightControlledIntersectionTransitPluginNode constructor + * \brief LightControlledIntersectionTransitPluginNode constructor */ explicit LightControlledIntersectionTransitPluginNode(const rclcpp::NodeOptions &); /** * \brief Callback for dynamic parameter updates */ - rcl_interfaces::msg::SetParametersResult + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); //// // Overrides //// void plan_trajectory_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; bool get_availability() override; std::string get_version_id() override; - + /** * \brief This method should be used to load parameters and will be called on the configure state transition. - */ + */ carma_ros2_utils::CallbackReturn on_configure_plugin() override; }; diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 5d07d0bb96..83f84c7ef7 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -19,24 +19,24 @@ namespace light_controlled_intersection_tactical_plugin { - LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) - :wm_(wm), config_(config), plugin_name_(plugin_name), logger_(logger) + LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, const std::string& plugin_name, + std::shared_ptr nh) + :wm_(wm), config_(config), plugin_name_(plugin_name), nh_(nh) { } - void LightControlledIntersectionTacticalPlugin::planTrajectoryCB( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + void LightControlledIntersectionTacticalPlugin::planTrajectoryCB( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); // Start timing the execution time for planning so it can be logged - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Starting light controlled intersection trajectory planning"); - + if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) { throw std::invalid_argument( - "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + + "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } @@ -48,7 +48,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); resp->related_maneuvers.push_back(req->maneuver_index_to_plan); } - else + else { throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); } @@ -62,7 +62,7 @@ namespace light_controlled_intersection_tactical_plugin auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); lanelet::ConstLanelet current_lanelet; - + if (current_lanelets.empty()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Given vehicle position is not on the road! Returning..."); @@ -93,11 +93,11 @@ namespace light_controlled_intersection_tactical_plugin config_.default_downsample_ratio, config_.turn_downsample_ratio); - wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, - config_.curve_resample_step_size, config_.minimum_speed, + wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, + config_.curve_resample_step_size, config_.minimum_speed, config_.vehicle_accel_limit, - config_.lateral_accel_limit, - config_.speed_moving_average_window_size, + config_.lateral_accel_limit, + config_.speed_moving_average_window_size, config_.curvature_moving_average_window_size, config_.back_distance, config_.buffer_ending_downtrack); @@ -137,7 +137,7 @@ namespace light_controlled_intersection_tactical_plugin last_final_speeds_ = reduced_final_speeds; last_trajectory_.trajectory_points = reduced_last_traj.trajectory_points; - + if (is_last_case_successful_ != boost::none && last_case_ != boost::none && last_case_.get() == new_case && is_new_case_successful == true @@ -159,7 +159,7 @@ namespace light_controlled_intersection_tactical_plugin resp->trajectory_plan = last_trajectory_; RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Last Traj's target time: " << std::to_string(rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds()) << ", and stamp:" << std::to_string(rclcpp::Time(req->header.stamp).seconds()) << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); - } + } if (!resp->trajectory_plan.trajectory_points.empty()) // if has valid trajectory saved from before return { @@ -176,10 +176,10 @@ namespace light_controlled_intersection_tactical_plugin return; } - + // IF NOT USING LAST TRAJECTORY PLAN NEW TRAJECTORY - // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver + // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver auto points_and_target_speeds = createGeometryProfile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); @@ -197,30 +197,30 @@ namespace light_controlled_intersection_tactical_plugin trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); // Compose smooth trajectory/speed by resampling - trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, - req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, wpg_detail_config); // Compute the trajectory // Set the planning plugin field name for (auto& p : trajectory.trajectory_points) { p.planner_plugin_name = plugin_name_; } - + if (trajectory.trajectory_points.size () < 2) { if (last_trajectory_.trajectory_points.size() >= 2 && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp)) { resp->trajectory_plan = last_trajectory_; - RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory, so using last valid trajectory!"); } else { resp->trajectory_plan = trajectory; - RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Failed to generate new trajectory or use old valid trajectory, so returning empty/invalid trajectory!"); } } - else + else { last_trajectory_ = trajectory; resp->trajectory_plan = trajectory; @@ -235,12 +235,23 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "USING NEW CASE!!! : " << (int)last_case_.get()); - + } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful); resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front(); + // Yield for potential obstacles in the road + // Aside from the flag, yield_plugin should not be called on invalid trajectories + if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2) + { + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); + } + else + { + RCLCPP_DEBUG(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Ignored Object Avoidance"); + } + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -251,14 +262,14 @@ namespace light_controlled_intersection_tactical_plugin return; } - void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp) { if (points_and_target_speeds.empty()) { throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); } - + // Checking route geometry start against start_dist and adjust profile double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist @@ -283,23 +294,23 @@ namespace light_controlled_intersection_tactical_plugin if (planning_downtrack_start < start_dist) { //Account for the buffer distance that is technically not part of this maneuver - + total_dist_planned = planning_downtrack_start - start_dist; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); } - + double prev_speed = starting_speed; auto prev_point = points_and_target_speeds.front(); - + for(auto& p : points_and_target_speeds) { double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); - total_dist_planned += delta_d; + total_dist_planned += delta_d; //Apply the speed from algorithm at dist covered //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) double speed_i; - if (total_dist_planned <= epsilon_) + if (total_dist_planned <= epsilon_) { //Keep target speed same for buffer distance portion speed_i = starting_speed; @@ -317,7 +328,7 @@ namespace light_controlled_intersection_tactical_plugin //Third segment speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt } - else + else { //buffer points that will be cut speed_i = prev_speed; @@ -330,7 +341,7 @@ namespace light_controlled_intersection_tactical_plugin } p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); - p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); + p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); prev_point = p; @@ -340,7 +351,7 @@ namespace light_controlled_intersection_tactical_plugin void LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) { - if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || + if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); } @@ -366,7 +377,7 @@ namespace light_controlled_intersection_tactical_plugin double entry_dist = ending_downtrack - starting_downtrack; // change speed profile depending on algorithm case starting from maneuver start_dist - applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, + applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, departure_speed, tsp); } @@ -388,7 +399,7 @@ namespace light_controlled_intersection_tactical_plugin const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) { std::vector points_and_target_speeds; - + bool first = true; std::unordered_set visited_lanelets; std::vector processed_maneuvers; @@ -400,7 +411,7 @@ namespace light_controlled_intersection_tactical_plugin auto maneuver = maneuvers.front(); double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); - + starting_downtrack = std::min(starting_downtrack, max_starting_downtrack); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Used downtrack: " << starting_downtrack); @@ -413,26 +424,31 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_DEBUG_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Creating Lane Follow Geometry"); std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); - processed_maneuvers.push_back(maneuver); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + processed_maneuvers.push_back(maneuver); } - else + else { throw std::invalid_argument("Light Control Intersection Plugin can only create a geometry profile for one maneuver"); } - //Add buffer ending to lane follow points at the end of maneuver(s) end dist + //Add buffer ending to lane follow points at the end of maneuver(s) end dist if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config); } - + return points_and_target_speeds; } - void LightControlledIntersectionTacticalPlugin::setConfig(const Config& config) + void LightControlledIntersectionTacticalPlugin::setConfig(const Config& config) { config_ = config; } + void LightControlledIntersectionTacticalPlugin::set_yield_client(carma_ros2_utils::ClientPtr client) + { + yield_client_ = client; + } + } // light_controlled_intersection_tactical_plugin diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 7cea76134f..6594b4f5c1 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -46,6 +46,9 @@ namespace light_controlled_intersection_tactical_plugin config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.vehicle_accel_limit = declare_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); config_.vehicle_decel_limit = declare_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + } rcl_interfaces::msg::SetParametersResult LightControlledIntersectionTransitPluginNode::parameter_update_callback(const std::vector ¶meters) @@ -58,10 +61,10 @@ namespace light_controlled_intersection_tactical_plugin {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier}, {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier}, - {"lat_accel_multiplier", config_.lat_accel_multiplier}, - {"stop_line_buffer", config_.stop_line_buffer}, - {"minimum_speed", config_.minimum_speed}, - {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, + {"lat_accel_multiplier", config_.lat_accel_multiplier}, + {"stop_line_buffer", config_.stop_line_buffer}, + {"minimum_speed", config_.minimum_speed}, + {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, {"algorithm_evaluation_period", config_.algorithm_evaluation_period}}, parameters); // Global acceleration limits not allowed to dynamically update auto error_2 = update_params( {{"default_downsample_ratio", config_.default_downsample_ratio}, @@ -108,6 +111,8 @@ namespace light_controlled_intersection_tactical_plugin get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); get_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); get_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + get_parameter("enable_object_avoidance", config_.enable_object_avoidance); // Use the configured multipliers to update the accel limits config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; @@ -120,7 +125,11 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_INFO_STREAM(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Loaded params: " << config_); // Initialize worker object - worker_ = std::make_shared(get_world_model(), config_, get_plugin_name(), get_node_logging_interface()); + worker_ = std::make_shared(get_world_model(), config_, get_plugin_name(), shared_from_this()); + + yield_client_ = create_client("yield_plugin/plan_trajectory"); + worker_->set_yield_client(yield_client_); + RCLCPP_INFO(rclcpp::get_logger("light_controlled_intersection_tactical_plugin"), "Yield Client Set"); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; @@ -135,8 +144,8 @@ namespace light_controlled_intersection_tactical_plugin } void LightControlledIntersectionTransitPluginNode::plan_trajectory_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { worker_->planTrajectoryCB(req, resp); diff --git a/light_controlled_intersection_tactical_plugin/test/node_test.cpp b/light_controlled_intersection_tactical_plugin/test/node_test.cpp index 6fdb936dcb..22451ac5fa 100644 --- a/light_controlled_intersection_tactical_plugin/test/node_test.cpp +++ b/light_controlled_intersection_tactical_plugin/test/node_test.cpp @@ -27,7 +27,7 @@ namespace light_controlled_intersection_tactical_plugin { // TODO: The package requires additional unit tests to improve unit test coverage. These unit tests will be created // in a follow-on story. - + TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm) { std::shared_ptr wm = std::make_shared(); @@ -35,8 +35,8 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; @@ -44,8 +44,8 @@ namespace light_controlled_intersection_tactical_plugin std::vector points_and_target_speeds; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + TrajectoryParams tp; tp.v1_ = 1.0; tp.v2_ = 2.0; @@ -74,7 +74,7 @@ namespace light_controlled_intersection_tactical_plugin points_and_target_speeds.push_back(pair); lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 1, 4, 1, 1, tp); - + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); @@ -123,7 +123,7 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); } - + TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile) { std::shared_ptr wm = std::make_shared(); @@ -131,8 +131,8 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; @@ -140,8 +140,8 @@ namespace light_controlled_intersection_tactical_plugin std::vector points_and_target_speeds; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -152,7 +152,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 4; maneuver_msg.lane_following_maneuver.end_speed = 1; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -193,7 +193,7 @@ namespace light_controlled_intersection_tactical_plugin points_and_target_speeds.push_back(pair); EXPECT_THROW(lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds), std::invalid_argument); - + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_); lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds); @@ -204,7 +204,7 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); } - + TEST(LCITacticalPluginTest, createGeometryProfile) { std::shared_ptr wm = std::make_shared(); @@ -212,15 +212,15 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; config.minimum_speed = 1; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -231,7 +231,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 4; maneuver_msg.lane_following_maneuver.end_speed = 2; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -277,9 +277,9 @@ namespace light_controlled_intersection_tactical_plugin EXPECT_EQ(lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config).size(), 7); - + auto points_and_target_speeds = lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config); - + EXPECT_NEAR(points_and_target_speeds.front().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); EXPECT_NEAR(points_and_target_speeds.front().point.y(), 0.0, 0.001); @@ -293,16 +293,16 @@ namespace light_controlled_intersection_tactical_plugin wm->setMap(map); carma_wm::test::setSpeedLimit(15_mph, wm); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - - + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); Config config; config.minimum_speed = 1; config.default_downsample_ratio = 1; - auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node->get_node_logging_interface()); - + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, "", lci_node); + carma_planning_msgs::msg::Maneuver maneuver_msg; maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = @@ -313,7 +313,7 @@ namespace light_controlled_intersection_tactical_plugin maneuver_msg.lane_following_maneuver.start_speed = 1; maneuver_msg.lane_following_maneuver.end_dist = 11.0; maneuver_msg.lane_following_maneuver.end_speed = 2; - + TrajectoryParams tsp; tsp.v1_ = 1.0; tsp.v2_ = 2.0; @@ -347,20 +347,20 @@ namespace light_controlled_intersection_tactical_plugin auto req = std::make_shared(); auto resp = std::make_shared(); - + req->maneuver_plan.maneuvers.push_back(maneuver_msg); req->maneuver_index_to_plan = 0; req->vehicle_state = state; lci_tactical.planTrajectoryCB(req, resp); - + EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.front().target_time).seconds(), 0.0, 0.001); EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.back().target_time).seconds(), 9.23, 0.1); EXPECT_NEAR(resp->trajectory_plan.trajectory_points.front().y, 0.1, 0.001); } - + } // namespace light_controlled_intersection_tactical_plugin @@ -377,4 +377,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return success; -} \ No newline at end of file +} \ No newline at end of file diff --git a/motion_computation/config/parameters.yaml b/motion_computation/config/parameters.yaml index 6ac8cadac4..d3a2fe079d 100644 --- a/motion_computation/config/parameters.yaml +++ b/motion_computation/config/parameters.yaml @@ -2,7 +2,7 @@ prediction_time_step: 0.1 # Period of prediction in seconds -prediction_period: 2.0 +prediction_period: 3.0 # CV Model X-Axis Acceleration Noise cv_x_accel_noise: 9.0 @@ -16,18 +16,18 @@ prediction_process_noise_max: 1000.0 # Percentage of initial confidence to propagate to next time step prediction_confidence_drop_rate: 0.95 -# Boolean: If true then BSM messages will be converted to ExternalObjects. +# Boolean: If true then BSM messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) enable_bsm_processing: false -# Boolean: If true then PSM messages will be converted to ExternalObjects. +# Boolean: If true then PSM messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_psm_processing: true +enable_psm_processing: false -# Boolean: If true then MobilityPath messages will be converted to ExternalObjects. +# Boolean: If true then MobilityPath messages will be converted to ExternalObjects. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_mobility_path_processing: true +enable_mobility_path_processing: false -# Boolean: If true then ExternalObjects generated from sensor data will be processed. +# Boolean: If true then ExternalObjects generated from sensor data will be processed. # If other object sources are enabled, they will be synchronized but no fusion will occur (objects may be duplicated) -enable_sensor_processing: false \ No newline at end of file +enable_sensor_processing: true \ No newline at end of file diff --git a/plan_delegator/config/plan_delegator_params.yaml b/plan_delegator/config/plan_delegator_params.yaml index 4c54e3cd7f..6bedaa2e4d 100644 --- a/plan_delegator/config/plan_delegator_params.yaml +++ b/plan_delegator/config/plan_delegator_params.yaml @@ -26,9 +26,10 @@ min_speed: 2.2352 # Units: Seconds duration_to_signal_before_lane_change: 2.5 -# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory +# Int: The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory # generation takes longer than this, then trajectory planning will immediately end for the current trajectory planning iteration. # NOTE: It is highly desirable to maintain a timeout of 100 ms or less, but trajectory generation success cannot be guaranteed with this duration # for tactical plugins (primarily cooperative_lanechange) in all test scenarios at this time. # Units: Milliseconds -tactical_plugin_service_call_timeout: 100 \ No newline at end of file +# Configured in VehicleConfigPrams.yaml in carma-config +# tactical_plugin_service_call_timeout: 100 \ No newline at end of file diff --git a/stop_and_wait_plugin/include/stop_and_wait_config.hpp b/stop_and_wait_plugin/include/stop_and_wait_config.hpp index 0063bd62d9..6126d50c96 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_config.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_config.hpp @@ -32,6 +32,9 @@ struct StopandWaitConfig double centerline_sampling_spacing = 1.0; // The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions double default_stopping_buffer = 3.0; // The default buffer in meters used for where the vehicle can come to a stop during execution. This value is overriden by the first value in maneuver.parameters.float_valued_meta_data[] double moving_average_window_size = 11.0; // Moving Average filter window size + int tactical_plugin_service_call_timeout = 100; // Tactical plugin service call request timeout in milliseconds + bool enable_object_avoidance = false; // True to enable object avoidance using yield_plugin + friend std::ostream& operator<<(std::ostream& output, const StopandWaitConfig& c) { output << "StopandWaitConfig { " << std::endl @@ -43,6 +46,8 @@ struct StopandWaitConfig << "crawl_speed: " << c.crawl_speed << std::endl << "centerline_sampling_spacing: " << c.crawl_speed << std::endl << "default_stopping_buffer: " << c.crawl_speed << std::endl + << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl + << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "}" << std::endl; return output; } diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.hpp b/stop_and_wait_plugin/include/stop_and_wait_node.hpp index 378eb46403..b8a0fbf00a 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_node.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_node.hpp @@ -55,6 +55,9 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin StopandWaitConfig config_; + // Service Clients + carma_ros2_utils::ClientPtr yield_client_; + // Worker std::shared_ptr plugin_; @@ -62,21 +65,21 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin std::string plugin_name_; public: - + /** - * \brief Node constructor + * \brief Node constructor */ explicit StopandWaitNode(const rclcpp::NodeOptions &); /** * \brief This method should be used to load parameters and will be called on the configure state transition. - */ + */ carma_ros2_utils::CallbackReturn on_configure_plugin(); /** * \brief Callback for dynamic parameter updates */ - rcl_interfaces::msg::SetParametersResult + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); //// @@ -84,8 +87,8 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin //// void plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; bool get_availability() override; diff --git a/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp index 575fa1e5e9..b16aea3e64 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp @@ -53,9 +53,9 @@ class StopandWait /** * \brief Constructor */ - StopandWait(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const StopandWaitConfig& config, + StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, const std::string& plugin_name, const std::string& version_id); @@ -77,9 +77,9 @@ class StopandWait * the vehicle position or earlier. If the first maneuver exceeds this then it's downtrack will be shifted to this * value. * - * ASSUMPTION: Since the vehicle is trying to stop the assumption made is that the speed limit is irrelevant. + * ASSUMPTION: Since the vehicle is trying to stop the assumption made is that the speed limit is irrelevant. * ASSUMPTION: The provided maneuver lies on the route shortest path - * + * * \return List of centerline points paired with speed limits. All output points will have speed matching state.logitudinal_velocity */ std::vector maneuvers_to_points(const std::vector& maneuvers, @@ -109,6 +109,13 @@ class StopandWait const std::vector& points, const std::vector& times, const std::vector& yaws, rclcpp::Time startTime); + /** + * \brief set the yield service + * + * \param yield_srv input yield service + */ + void set_yield_client(carma_ros2_utils::ClientPtr client); + private: double epsilon_ = 0.001; //small constant to compare double @@ -119,6 +126,8 @@ class StopandWait carma_wm::WorldModelConstPtr wm_; StopandWaitConfig config_; std::shared_ptr nh_; - + // Service Clients + carma_ros2_utils::ClientPtr yield_client_; + }; } // namespace stop_and_wait_plugin diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 962f5eb3ff..9cc9a55cf7 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -48,9 +48,9 @@ using oss = std::ostringstream; namespace stop_and_wait_plugin { -StopandWait::StopandWait(std::shared_ptr nh, - carma_wm::WorldModelConstPtr wm, - const StopandWaitConfig& config, +StopandWait::StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, const std::string& plugin_name, const std::string& version_id) : version_id_ (version_id),plugin_name_(plugin_name),config_(config),nh_(nh), wm_(wm) @@ -80,7 +80,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R if (req->vehicle_state.longitudinal_vel < epsilon_) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Detected that car is already stopped! Ignoring the request to plan Stop&Wait"); - + return true; } @@ -114,7 +114,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R // Extract the stopping buffer used to consider a stopping behavior complete double stop_location_buffer = config_.default_stopping_buffer; // If no maneuver meta data is provided we will use the default buffer - + double stopping_accel = 0.0; if (maneuver_plan[0].stop_and_wait_maneuver.parameters.presence_vector & carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA) @@ -132,7 +132,7 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R throw std::invalid_argument("stop and wait maneuver message missing required float meta data"); } - double initial_speed = req->vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline + double initial_speed = req->vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline RCLCPP_DEBUG_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Original size: " << points_and_target_speeds.size()); @@ -146,6 +146,16 @@ bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::R resp->trajectory_plan = trajectory; + // Yield for potential obstacles in the road + // Aside from the flag, yield_plugin should not be called on invalid trajectories + if (config_.enable_object_avoidance && resp->trajectory_plan.trajectory_points.size() >= 2) + { + basic_autonomy::waypoint_generation::modify_trajectory_to_yield_to_obstacles(nh_, req, resp, yield_client_, config_.tactical_plugin_service_call_timeout); + } + else + { + RCLCPP_DEBUG(rclcpp::get_logger("stop_and_wait_plugin"), "Ignored Object Avoidance"); + } std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); // Planning complete @@ -184,7 +194,7 @@ std::vector StopandWait::maneuvers_to_points(const std::vector StopandWait::trajecto tpp.yaw = yaws[i]; tpp.planner_plugin_name = plugin_name_; tpp.controller_plugin_name = "default"; - + traj.push_back(tpp); } @@ -253,7 +263,7 @@ std::vector StopandWait::compose_ break; } } - + if (req_dist > remaining_distance) { @@ -290,7 +300,7 @@ std::vector StopandWait::compose_ prev_pair = pair; continue; } - + if (reached_end || v_i >= starting_speed) { // We are walking backward, so if the prev speed is greater than or equal to the starting speed then we are done // backtracking @@ -302,7 +312,7 @@ std::vector StopandWait::compose_ prev_pair = pair; continue; // continue until loop end } - + double v_f = sqrt(v_i * v_i + 2 * target_accel * dx); PointSpeedPair pair = points[i]; @@ -315,7 +325,7 @@ std::vector StopandWait::compose_ // Now we have a trajectory that decelerates from our end point to somewhere in the maneuver std::reverse(final_points.begin(), - final_points.end()); + final_points.end()); std::vector speeds; std::vector raw_points; @@ -325,7 +335,7 @@ std::vector StopandWait::compose_ double max_downtrack = inverse_downtracks.back(); std::vector downtracks = lanelet::utils::transform(inverse_downtracks, [max_downtrack](const auto& d) { return max_downtrack - d; }); std::reverse(downtracks.begin(), - downtracks.end()); + downtracks.end()); bool in_range = false; double stopped_downtrack = 0; @@ -343,7 +353,7 @@ std::vector StopandWait::compose_ if (downtracks.back() - downtrack < stop_location_buffer && filtered_speeds[i] < config_.crawl_speed + one_mph_in_mps) { // if we are within the stopping buffer and going at near crawl speed then command stop - + // To avoid any issues in control plugin behavior we only command 0 if the vehicle is inside the buffer if (vehicle_in_buffer || (i == filtered_speeds.size() - 1)) { // Vehicle is in the buffer filtered_speeds[i] = 0.0; @@ -408,4 +418,9 @@ void StopandWait::splitPointSpeedPairs(const std::vector& points } } +void StopandWait::set_yield_client(carma_ros2_utils::ClientPtr client) +{ + yield_client_ = client; +} + } // namespace stop_and_wait_plugin \ No newline at end of file diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp index b3caf08fa9..3e90dc1050 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp @@ -25,7 +25,7 @@ namespace stop_and_wait_plugin { // Create initial config config_ = StopandWaitConfig(); - + // Declare parameters config_.minimal_trajectory_duration = declare_parameter("minimal_trajectory_duration", config_.minimal_trajectory_duration); config_.stop_timestep = declare_parameter("stop_timestep", config_.stop_timestep); @@ -35,6 +35,9 @@ namespace stop_and_wait_plugin config_.crawl_speed = declare_parameter("crawl_speed", config_.crawl_speed); config_.centerline_sampling_spacing = declare_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); config_.default_stopping_buffer = declare_parameter("default_stopping_buffer", config_.default_stopping_buffer); + config_.tactical_plugin_service_call_timeout = declare_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + } rcl_interfaces::msg::SetParametersResult StopandWaitNode::parameter_update_callback(const std::vector ¶meters) @@ -68,21 +71,27 @@ namespace stop_and_wait_plugin get_parameter("crawl_speed", config_.crawl_speed); get_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); get_parameter("default_stopping_buffer", config_.default_stopping_buffer); + get_parameter("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout); + get_parameter("enable_object_avoidance", config_.enable_object_avoidance); RCLCPP_INFO_STREAM(rclcpp::get_logger("stop_and_wait_plugin"),"Done loading parameters: " << config_); // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&StopandWaitNode::parameter_update_callback, this, std_ph::_1)); - + plugin_ = std::make_shared(shared_from_this(), get_world_model(), config_,plugin_name_,version_id_); - + + yield_client_ = create_client("yield_plugin/plan_trajectory"); + plugin_->set_yield_client(yield_client_); + RCLCPP_INFO(rclcpp::get_logger("stop_and_wait_plugin"), "Yield Client Set"); + // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } void StopandWaitNode::plan_trajectory_callback( - std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { plugin_->plan_trajectory_cb(req, resp); diff --git a/yield_plugin/config/parameters.yaml b/yield_plugin/config/parameters.yaml index ca5b321613..fe7e5b5264 100644 --- a/yield_plugin/config/parameters.yaml +++ b/yield_plugin/config/parameters.yaml @@ -1,8 +1,8 @@ -# Adjustment factor for safe and comfortable acceleration/deceleration +# Adjustment factor for safe and comfortable acceleration/deceleration # Value type: Desired # No unit -acceleration_adjustment_factor: 4.0 +acceleration_adjustment_factor: 4.0 # Time horizon for collision detection for vehicles on the route # NOTE: not applied for pedestrians or other non-vehicle obstacles on the road # Units: s @@ -11,7 +11,7 @@ on_route_vehicle_collision_horizon: 10.0 # Minimum speed for moving obstacle # Units: m/s # Value type: Desired -min_obstacle_speed: 2.0 +min_obstacle_speed: 0.5 # Minimum object avoidance planning time # Units: s # Value type: Desired @@ -39,10 +39,10 @@ vehicle_height: 3.0 # Maximum speed value to consider the ego vehicle stopped # Units: m/s # Value type: Desired -max_stop_speed: 1.0 +max_stop_speed: 1.0 # parameter to enable cooperative behavior # Value type: Desired -enable_cooperative_behavior: true +enable_cooperative_behavior: true # parameter to always accept mobility request # Value type: Desired always_accept_mobility_request: true diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index e767af3b5e..f63e9de099 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -60,7 +60,7 @@ namespace yield_plugin for (size_t i=0; iprojectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , 1); - + + lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { static_cast(ecef_point.ecef_x)/100.0, static_cast(ecef_point.ecef_y)/100.0, static_cast(ecef_point.ecef_z)/100.0 } , 1); + return lanelet::traits::to2D(map_point); - } - - + } + + carma_v2x_msgs::msg::MobilityResponse YieldPlugin::compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const { @@ -123,7 +123,7 @@ namespace yield_plugin out_mobility_response.is_accepted = true; } else out_mobility_response.is_accepted = false; - + return out_mobility_response; } @@ -143,10 +143,12 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(),"Cooperative Lane Change Request Received"); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_RECEIVED; lc_status_msg.description = "Received lane merge request"; + if (incoming_request.m_header.recipient_id == config_.vehicle_id) { RCLCPP_DEBUG(nh_->get_logger(),"CLC Request correctly received"); } + // extract mobility header std::string req_sender_id = incoming_request.m_header.sender_id; std::string req_plan_id = incoming_request.m_header.plan_id; @@ -166,7 +168,7 @@ namespace yield_plugin int req_traj_fractional = pt.get("f"); int start_lanelet_id = pt.get("sl"); int end_lanelet_id = pt.get("el"); - double req_traj_speed = (double)req_traj_speed_full + (double)(req_traj_fractional)/10.0; + double req_traj_speed = static_cast(req_traj_speed_full) + static_cast(req_traj_fractional)/10.0; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_traj_speed" << req_traj_speed); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"start_lanelet_id" << start_lanelet_id); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"end_lanelet_id" << end_lanelet_id); @@ -175,39 +177,39 @@ namespace yield_plugin req_traj_plan = convert_eceftrajectory_to_mappoints(incoming_trajectory); - double req_expiration_sec = (double)incoming_request.expiration; + double req_expiration_sec = static_cast(incoming_request.expiration); double current_time_sec = nh_->now().seconds(); bool response_to_clc_req = false; // ensure there is enough time for the yield double req_plan_time = req_expiration_sec - current_time_sec; - double req_timestamp = (double)incoming_request.m_header.timestamp / 1000.0 - current_time_sec; + double req_timestamp = static_cast(incoming_request.m_header.timestamp) / 1000.0 - current_time_sec; set_incoming_request_info(req_traj_plan, req_traj_speed, req_plan_time, req_timestamp); - + if (req_expiration_sec - current_time_sec >= config_.tpmin && cooperative_request_acceptable_) { timesteps_since_last_req_ = 0; lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_ACCEPTED; lc_status_msg.description = "Accepted lane merge request"; - response_to_clc_req = true; - RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); + response_to_clc_req = true; + RCLCPP_DEBUG(nh_->get_logger(),"CLC accepted"); } else { lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REQUEST_REJECTED; lc_status_msg.description = "Rejected lane merge request"; response_to_clc_req = false; - RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); + RCLCPP_DEBUG(nh_->get_logger(),"CLC rejected"); } carma_v2x_msgs::msg::MobilityResponse outgoing_response = compose_mobility_response(req_sender_id, req_plan_id, response_to_clc_req); mobility_response_publisher_(outgoing_response); lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::RESPONSE_SENT; - RCLCPP_DEBUG(nh_->get_logger(),"response sent"); + RCLCPP_DEBUG(nh_->get_logger(),"response sent"); } } lc_status_publisher_(lc_status_msg); - + } void YieldPlugin::set_incoming_request_info(std::vector req_trajectory, double req_speed, double req_planning_time, double req_timestamp) @@ -215,7 +217,7 @@ namespace yield_plugin req_trajectory_points_ = req_trajectory; req_target_speed_ = req_speed; req_target_plan_time_ = req_planning_time; - RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"req_target_plan_time_" << req_target_plan_time_); req_timestamp_ = req_timestamp; } @@ -226,9 +228,10 @@ namespace yield_plugin } void YieldPlugin::plan_trajectory_callback( - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { + RCLCPP_DEBUG(nh_->get_logger(),"Yield_plugin was called!"); if (req->initial_trajectory_plan.trajectory_points.size() < 2){ throw std::invalid_argument("Empty Trajectory received by Yield"); } @@ -252,7 +255,7 @@ namespace yield_plugin { RCLCPP_DEBUG(nh_->get_logger(),"unreliable CLC communication, switching to object avoidance"); yield_trajectory = update_traj_for_object(original_trajectory, external_objects_, req->vehicle_state.longitudinal_vel); // Compute the trajectory - } + } } else { @@ -262,7 +265,7 @@ namespace yield_plugin yield_trajectory.header.frame_id = "map"; yield_trajectory.header.stamp = nh_->now(); yield_trajectory.trajectory_id = original_trajectory.trajectory_id; - yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. + yield_trajectory.initial_longitudinal_velocity = original_trajectory.initial_longitudinal_velocity;//copy the original trajectory's desired speed for now. resp->trajectory_plan = yield_trajectory; @@ -323,7 +326,7 @@ namespace yield_plugin if (planning_time > min_time) { cooperative_request_acceptable_ = true; - cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); + cooperative_trajectory = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, planning_time); } else { @@ -331,7 +334,7 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(),"The incoming requested trajectory is rejected, due to insufficient gap"); cooperative_trajectory = original_tp; } - + } else { @@ -343,7 +346,7 @@ namespace yield_plugin return cooperative_trajectory; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time) { carma_planning_msgs::msg::TrajectoryPlan jmt_trajectory; std::vector jmt_trajectory_points; @@ -355,15 +358,15 @@ namespace yield_plugin double original_max_speed = max_trajectory_speed(original_tp.trajectory_points); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"original_max_speed" << original_max_speed); - std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, + std::vector values = quintic_coefficient_calculator::quintic_coefficient_calculator(initial_pos, goal_pos, - initial_velocity, - goal_velocity, - initial_accel, - goal_accel, - initial_time, + initial_velocity, + goal_velocity, + initial_accel, + goal_accel, + initial_time, planning_time); - + std::vector original_traj_downtracks = get_relative_downtracks(original_tp); std::vector calculated_speeds = {}; calculated_speeds.push_back(initial_velocity); @@ -392,32 +395,32 @@ namespace yield_plugin { double dt = (2 * original_traj_downtracks.at(i)) / (current_speed + prev_speed); jmt_tpp = original_tp.trajectory_points.at(i); - jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.at(i-1).target_time) + rclcpp::Duration(dt*1e9); + jmt_tpp.target_time = rclcpp::Time(jmt_trajectory_points.back().target_time) + rclcpp::Duration(dt*1e9); RCLCPP_DEBUG_STREAM(nh_->get_logger(), "non-empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } else { - RCLCPP_DEBUG(nh_->get_logger(),"target speed is zero"); + RCLCPP_DEBUG(nh_->get_logger(),"Target speed is zero"); // if speed is zero, the vehicle will stay in previous location. - jmt_tpp = jmt_trajectory_points.at(i-1); - // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase - jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points[i -1 ].target_time).seconds()) * 1e9); + jmt_tpp = jmt_trajectory_points.back(); + // MISH: Potential bug where if purepursuit accidentally picks a point that has previous time, then it may speed up. target_time is assumed to increase + jmt_tpp.target_time = rclcpp::Time(std::max((rclcpp::Time(jmt_trajectory_points[0].target_time) + rclcpp::Duration(traj_target_time*1e9)).seconds(), rclcpp::Time(jmt_trajectory_points.back().target_time).seconds()) * 1e9); RCLCPP_DEBUG_STREAM(nh_->get_logger(), "empty x: " << jmt_tpp.x << ", y:" << jmt_tpp.y << ", t:" << std::to_string(rclcpp::Time(jmt_tpp.target_time).seconds())); jmt_trajectory_points.push_back(jmt_tpp); } prev_speed = current_speed; } - + jmt_trajectory.header = original_tp.header; jmt_trajectory.trajectory_id = original_tp.trajectory_id; jmt_trajectory.trajectory_points = jmt_trajectory_points; return jmt_trajectory; } - - std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) + + std::optional YieldPlugin::detect_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& trajectory1, const std::vector& trajectory2, double collision_radius) { // Iterate through each pair of consecutive points in the trajectories @@ -427,13 +430,13 @@ namespace yield_plugin bool on_route = false; int on_route_idx = 0; - for (size_t j = 0; j < trajectory2.size(); ++j) + for (size_t j = 0; j < trajectory2.size(); j+=4) // Checking every 4th point to save computation time { lanelet::BasicPoint2d curr_point; curr_point.x() = trajectory2.at(j).predicted_position.position.x; curr_point.y() = trajectory2.at(j).predicted_position.position.y; - auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 4); // get 4 lanelets + auto corresponding_lanelets = wm_->getLaneletsFromPoint(curr_point, 8); // some intersection can have 8 overlapping lanelets for (const auto& llt: corresponding_lanelets) { @@ -455,13 +458,13 @@ namespace yield_plugin RCLCPP_DEBUG(nh_->get_logger(), "Predicted states are not on the route! ignoring"); return std::nullopt; } - + double smallest_dist = std::numeric_limits::max(); - for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) + for (size_t i = 0; i < trajectory1.trajectory_points.size() - 1; ++i) { auto p1a = trajectory1.trajectory_points.at(i); auto p1b = trajectory1.trajectory_points.at(i + 1); - for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) + for (size_t j = on_route_idx; j < trajectory2.size() - 1; ++j) { auto p2a = trajectory2.at(j); auto p2b = trajectory2.at(j + 1); @@ -496,7 +499,7 @@ namespace yield_plugin return std::nullopt; } - if (distance > collision_radius) + if (distance > collision_radius) { continue; } @@ -516,7 +519,7 @@ namespace yield_plugin { RCLCPP_WARN_STREAM(nh_->get_logger(), "Collision detected at timestamp " << std::to_string(p2a_t) << ", x: " << x1 << ", y: " << y1 << ", within distance: " << distance); return rclcpp::Time(p2a.header.stamp); - } + } } } @@ -524,8 +527,14 @@ namespace yield_plugin return std::nullopt; } - carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) - { + carma_planning_msgs::msg::TrajectoryPlan YieldPlugin::update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector& external_objects, double initial_velocity) + { + if (original_tp.trajectory_points.size() < 2) + { + RCLCPP_WARN(nh_->get_logger(), "Yield plugin received less than 2 points in update_traj_for_object, returning unchanged..."); + return original_tp; + } + if (original_tp.trajectory_points.empty()) { RCLCPP_ERROR(nh_->get_logger(), "Yield plugin received empty trajectory plan in update_traj_for_object"); @@ -576,7 +585,7 @@ namespace yield_plugin new_list.insert(new_list.end(), curr_obstacle.predictions.cbegin(), curr_obstacle.predictions.cend()); auto collision_time = detect_collision_time(original_tp, new_list, config_.intervehicle_collision_distance); - + if (collision_time != std::nullopt) { checked_external_object_ids.insert(curr_obstacle.id); @@ -593,11 +602,11 @@ namespace yield_plugin lanelet::BasicPoint2d point(original_tp.trajectory_points[0].x,original_tp.trajectory_points[0].y); double vehicle_downtrack = wm_->routeTrackPos(point).downtrack; - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"vehicle_downtrack: " << vehicle_downtrack); - // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, - // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. + // Issue (https://github.com/usdot-fhwa-stol/carma-platform/issues/2155): If the yield_plugin can detect if the roadway object is moving along the route, + // it is able to plan yielding much earlier and smoother using on_route_vehicle_collision_horizon. if(earliest_collision_obj.has_value()) { @@ -607,10 +616,10 @@ namespace yield_plugin double object_downtrack = wm_->routeTrackPos(point_o).downtrack; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object_downtrack: " << object_downtrack); - + double dist_to_object = object_downtrack - vehicle_downtrack; RCLCPP_DEBUG_STREAM(nh_->get_logger(),"dist_to_object: " << dist_to_object); - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"object speed: " << earliest_collision_obj.value().velocity.twist.linear.x); // Distance from the original trajectory point to the lead vehicle/object @@ -629,14 +638,15 @@ namespace yield_plugin // check if a digital_gap is available double digital_gap = check_traj_for_digital_min_gap(original_tp); RCLCPP_DEBUG_STREAM(nh_->get_logger(),"digital_gap: " << digital_gap); - // if a digital gap is available, it is replaced as safety gap + // if a digital gap is available, it is replaced as safety gap safety_gap = std::max(safety_gap, digital_gap); } // safety gap is implemented - double goal_pos = x_lead - safety_gap; + double goal_pos = x_lead - safety_gap; if (goal_velocity <= config_.min_obstacle_speed){ - RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving"); + RCLCPP_WARN(nh_->get_logger(),"The obstacle is not moving, goal velocity is set to 0"); + goal_velocity = 0.0; } double initial_time = 0; @@ -666,14 +676,14 @@ namespace yield_plugin { tp = t_ref; } - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Object avoidance planning time: " << tp); update_tpp_vector = generate_JMT_trajectory(original_tp, initial_pos, goal_pos, initial_velocity, goal_velocity, tp); return update_tpp_vector; } - + RCLCPP_DEBUG(nh_->get_logger(),"No collision detection, so trajectory not modified."); return original_tp; } @@ -684,11 +694,12 @@ namespace yield_plugin std::vector downtracks; downtracks.reserve(trajectory_plan.trajectory_points.size()); // relative downtrack distance of the fist Point is 0.0 - downtracks[0] = 0.0; + downtracks.push_back(0.0); for (size_t i=1; i < trajectory_plan.trajectory_points.size(); i++){ + double dx = trajectory_plan.trajectory_points.at(i).x - trajectory_plan.trajectory_points.at(i-1).x; double dy = trajectory_plan.trajectory_points.at(i).y - trajectory_plan.trajectory_points.at(i-1).y; - downtracks.at(i) = sqrt(dx*dx + dy*dy); + downtracks.push_back(sqrt(dx*dx + dy*dy)); } return downtracks; } @@ -698,7 +709,7 @@ namespace yield_plugin double result = 0; for (size_t i = 0; i < coeff.size(); i++) { - double value = coeff.at(i) * pow(x, (int)(coeff.size() - 1 - i)); + double value = coeff.at(i) * pow(x, static_cast(coeff.size() - 1 - i)); result = result + value; } return result; @@ -707,9 +718,9 @@ namespace yield_plugin double YieldPlugin::polynomial_calc_d(std::vector coeff, double x) const { double result = 0; - for (size_t i = 0; i < coeff.size()-1; i++) + for (size_t i = 0; i < coeff.size()-1; i++) { - double value = (int)(coeff.size() - 1 - i) * coeff.at(i) * pow(x, (int)(coeff.size() - 2 - i)); + double value = static_cast(coeff.size() - 1 - i) * coeff.at(i) * pow(x, static_cast(coeff.size() - 2 - i)); result = result + value; } return result; @@ -722,7 +733,7 @@ namespace yield_plugin { double dx = trajectory_points.at(i + 1).x - trajectory_points.at(i).x; double dy = trajectory_points.at(i + 1).y - trajectory_points.at(i).y; - double d = sqrt(dx*dx + dy*dy); + double d = sqrt(dx*dx + dy*dy); double t = (rclcpp::Time(trajectory_points.at(i + 1).target_time).seconds() - rclcpp::Time(trajectory_points.at(i).target_time).seconds()); double v = d/t; if(v > max_speed) @@ -744,11 +755,11 @@ namespace yield_plugin if (llts.empty()) { RCLCPP_WARN_STREAM(nh_->get_logger(),"Trajectory point: x= " << original_tp.trajectory_points.at(i).x << "y="<< original_tp.trajectory_points.at(i).y); - + throw std::invalid_argument("Trajectory Point is not on a valid lanelet."); } auto digital_min_gap = llts[0].regulatoryElementsAs(); //Returns a list of these elements) - if (!digital_min_gap.empty()) + if (!digital_min_gap.empty()) { double digital_gap = digital_min_gap[0]->getMinimumGap(); // Provided gap is in meters RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Digital Gap found with value: " << digital_gap); @@ -758,14 +769,14 @@ namespace yield_plugin return desired_gap; } - void YieldPlugin::set_georeference_string(const std::string& georeference) + void YieldPlugin::set_georeference_string(const std::string& georeference) { map_projector_ = std::make_shared(georeference.c_str()); // Build projector from proj string } void YieldPlugin::set_external_objects(const std::vector& object_list) { - external_objects_ = object_list; + external_objects_ = object_list; } } // namespace yield_plugin