diff --git a/.circleci/config.yml b/.circleci/config.yml index 5cb8c09aa7..7db5e42012 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,12 +28,12 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastol/autoware.ai:carma-system-3.10.0 + - image: usdotfhwastol/autoware.ai:carma-system-3.11.0 user: carma environment: TERM: xterm INIT_ENV: /home/carma/.base-image/init-env.sh - resource_class: large + resource_class: large # Set working directory working_directory: "/opt/carma/" # Pull code and execute tests diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index f7c394d50d..f34f83d8ba 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -66,7 +66,8 @@ sonar.modules= bsm_generator, \ mobilitypath_visualizer, \ wz_strategic_plugin, \ sci_strategic_plugin, \ - stop_controlled_intersection_tactical_plugin + stop_controlled_intersection_tactical_plugin, \ + light_controlled_intersection_tactical_plugin guidance.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/guidance bsm_generator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/bsm_generator @@ -106,6 +107,7 @@ mobilitypath_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mobi wz_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/wz_strategic_plugin sci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/sci_strategic_plugin stop_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/stop_controlled_intersection_tactical_plugin +light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/light_controlled_intersection_tactical_plugin # C++ Package differences # Sources @@ -147,6 +149,7 @@ mobilitypath_visualizer.sonar.sources = src wz_strategic_plugin.sonar.sources = src sci_strategic_plugin.sonar.sources = src stop_controlled_intersection_tactical_plugin.sonar.sources = src +light_controlled_intersection_tactical_plugin.sonar.sources = src # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -184,3 +187,4 @@ mobilitypath_visualizer.sonar.tests = test wz_strategic_plugin.sonar.tests = test sci_strategic_plugin.sonar.tests = test stop_controlled_intersection_tactical_plugin.sonar.tests = test +light_controlled_intersection_tactical_plugin.sonar.tests = test diff --git a/Dockerfile b/Dockerfile index 9a3c97f2ad..8c26ba8204 100644 --- a/Dockerfile +++ b/Dockerfile @@ -33,7 +33,7 @@ # Stage 1 - Acquire the CARMA source as well as any extra packages # ///////////////////////////////////////////////////////////////////////////// -FROM usdotfhwastol/autoware.ai:carma-system-3.10.0 AS base-image +FROM usdotfhwastol/autoware.ai:carma-system-3.11.0 AS base-image FROM base-image AS source-code diff --git a/README.md b/README.md index 20aea2bdae..09300707cb 100644 --- a/README.md +++ b/README.md @@ -71,10 +71,12 @@ For administrative information on CARMA2, including vehicle and developer PC con ## Other CARMA Packages CARMA PlatformSM is a downloadable, open source software (OSS) platform architected to be extensible and reusable for a wide variety of research purposes to advance innovation for cooperative driving automation. It enables communication between vehicles, road users such as pedestrians, bicyclists, and scooters, and infrastructure devices capable of communication. It promotes collaboration between a community of engineers and researchers to accelerate the development, testing, and evaluation of cooperative driving automation while advancing the safety, security, data, and use of artificial intelligence in automated driving technology. -The CARMA Platform is distributed as a set of multiple independent packages hosted in separate Github repositories. These packages facilitate operation of the CARMA Platform with different hardware configurations or allow it to support different modes of operation. To include one of these packages in your build of the CARMA Platform system please clone the Github repository into the same Catkin workspace `src/` folder as this repository. The Catkin build system will verify that dependencies are resolved appropriately and build the newly included package when you next run `catkin_make`. An incomplete listing of available packages for CARMA2 includes: +The CARMA Platform is distributed as a set of multiple independent packages hosted in separate Github repositories. These packages facilitate operation of the CARMA Platform with different hardware configurations or allow it to support different modes of operation. To include one of these packages in your build of the CARMA Platform system please clone the Github repository into the same Catkin workspace `src/` folder as this repository. The Catkin build system will verify that dependencies are resolved appropriately and build the newly included package when you next run `catkin_make`. An incomplete listing of available packages for CARMA3 includes: ### Vehicle Controller Interface Drivers * [carma-ssc-interface-wrapper](https://github.com/usdot-fhwa-stol/carma-ssc-interface-wrapper) +* [dataspeed_controller_driver](https://github.com/VT-ASIM-LAB/dataspeed_controller_driver) (Externally managed) +* [dataspeed_can_driver](https://github.com/VT-ASIM-LAB/dataspeed_can_driver) (Externally managed) ### Sensor Drivers * [carma-cohda-dsrc-driver](https://github.com/usdot-fhwa-stol/carma-cohda-dsrc-driver) @@ -82,6 +84,9 @@ The CARMA Platform is distributed as a set of multiple independent packages host * [avt_vimba_camera](https://github.com/usdot-fhwa-stol/avt_vimba_camera) * [carma-delphi-srr2-driver](https://github.com/usdot-fhwa-stol/carma-delphi-srr2-driver) * [novatel_gps_driver](https://github.com/usdot-fhwa-stol/novatel_gps_driver) +* [gstreamer_camera_driver](https://github.com/VT-ASIM-LAB/gstreamer_camera_driver) (Externally managed) +* [ouster_lidar_driver](https://github.com/VT-ASIM-LAB/ouster_lidar_driver) (Externally managed) +* [inertiallabs_gnss_driver](https://github.com/VT-ASIM-LAB/inertiallabs_gnss_driver) (Externally managed) ### General System Utilites * [carma-web-ui](https://github.com/usdot-fhwa-stol/carma-web-ui) diff --git a/arbitrator/config/arbitrator_params.yaml b/arbitrator/config/arbitrator_params.yaml index 03b7b5ba8c..fe2910e791 100644 --- a/arbitrator/config/arbitrator_params.yaml +++ b/arbitrator/config/arbitrator_params.yaml @@ -26,4 +26,4 @@ use_fixed_costs: true # Map: The priorities associated with each plugin during the planning # process, values will be normalized at runtime and inverted into costs # Unit: N/a -plugin_priorities: {RouteFollowingPlugin: 4.0, WorkZonePlugin: 8.0, PlatooningStrategicPlugin: 1.0} +plugin_priorities: {SCIStrategicPlugin: 4.0, RouteFollowingPlugin: 2.0, WorkZonePlugin: 4.0, PlatooningStrategicPlugin: 1.0} diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index 3f8424931b..7046e24ae0 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -33,6 +33,7 @@ namespace arbitrator for (auto i = topics.begin(); i != topics.end(); i++) { ros::ServiceClient sc = nh_->serviceClient(*i); + ROS_DEBUG_STREAM("found client: " << *i); if (sc.call(msg)) { responses.emplace(*i, msg); } diff --git a/arbitrator/src/capabilities_interface.cpp b/arbitrator/src/capabilities_interface.cpp index 0128451c62..70d0bedb61 100644 --- a/arbitrator/src/capabilities_interface.cpp +++ b/arbitrator/src/capabilities_interface.cpp @@ -43,6 +43,11 @@ namespace arbitrator stream << std::endl; ROS_INFO(stream.str().c_str()); } + else + { + ROS_DEBUG_STREAM("servicd call failed..."); + ROS_DEBUG_STREAM("client: " << sc_s); + } return topics; diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.h b/basic_autonomy/include/basic_autonomy/basic_autonomy.h index 423e786f71..3c9e668408 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.h +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.h @@ -244,8 +244,8 @@ namespace basic_autonomy const cav_msgs::VehicleState &ending_state_before_buffer, carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg, const DetailedTrajConfig &detailed_config); - //Functions specific to lane change - /** + //Functions specific to lane change + /** * \brief Converts a set of requested LANE_CHANGE maneuvers to point speed limit pairs. * * \param maneuvers The list of maneuvers to convert @@ -260,9 +260,36 @@ namespace basic_autonomy * * \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 create_lanechange_geometry(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack, + std::vector get_lanechange_points_from_maneuver(const cav_msgs::Maneuver &maneuver, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer, - const cav_msgs::VehicleState &state, const DetailedTrajConfig &detailed_config); + const cav_msgs::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config); + + /** + * \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 + * \param ending_downtrack The downtrack distance at which the lane change maneuver end + * \param wm Pointer to intialized world model for semantic map access + * \param state The vehicle state at the time the function is called + * + * \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,const cav_msgs::VehicleState &state, int downsample_ratio); + + + /** + * \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) + */ + std::vector> resample_linestring_pair_to_same_size(std::vector& line_1, std::vector& line_2); /** * \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. diff --git a/basic_autonomy/include/basic_autonomy/helper_functions.h b/basic_autonomy/include/basic_autonomy/helper_functions.h index d6678eca39..9dc38f5eaf 100644 --- a/basic_autonomy/include/basic_autonomy/helper_functions.h +++ b/basic_autonomy/include/basic_autonomy/helper_functions.h @@ -95,8 +95,7 @@ namespace waypoint_generation * \return index of nearest point in points */ int get_nearest_index_by_downtrack(const std::vector& points, const carma_wm::WorldModelConstPtr& wm, - const cav_msgs::VehicleState& state); - + const cav_msgs::VehicleState& state); } } \ No newline at end of file diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 3373ebf87f..2fdde54d29 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -47,7 +47,7 @@ namespace basic_autonomy } else if(maneuver.type == cav_msgs::Maneuver::LANE_CHANGE){ ROS_DEBUG_STREAM("Creating Lane Change Geometry"); - std::vector lane_change_points = create_lanechange_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, state, detailed_config); + std::vector lane_change_points = get_lanechange_points_from_maneuver(maneuver, starting_downtrack, wm, ending_state_before_buffer, state, general_config, detailed_config); points_and_target_speeds.insert(points_and_target_speeds.end(), lane_change_points.begin(), lane_change_points.end()); } else{ @@ -257,9 +257,220 @@ namespace basic_autonomy return constrained_points; } - std::vector create_lanechange_geometry(const cav_msgs::Maneuver &maneuver, double starting_downtrack, + 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,const cav_msgs::VehicleState &state, int downsample_ratio) + { + std::vector centerline_points; + + //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. + // 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()); + + while(!shared_boundary_found){ + //Assumption- Adjacent lanelets share lane boundary + if(current_lanelet.leftBound() == ending_lanelet.rightBound()){ + is_lanechange_left = true; + shared_boundary_found = true; + } + + else if(current_lanelet.rightBound() == ending_lanelet.leftBound()){ + shared_boundary_found = true; + } + + else{ + //If there are no following lanelets on route, lanechange should be completing before reaching it + if(wm->getMapRoutingGraph()->following(current_lanelet, false).empty()) + { + // Maneuver requires we travel further before completing lane change, but no routable lanelet directly ahead + //In this case we have reached a lanelet which does not have a routable lanelet ahead + isn't adjacent to the lanelet where lane change ends + //A lane change should have already happened at this point + 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(); + if(current_lanelet.id() == starting_lanelet.id()){ + //Looped back to starting lanelet + throw(std::invalid_argument("No lane change in path")); + } + 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); + } + } + + //Find crosstrack distance between starting lane and ending lane + std::vector target_lane_centerline; + for(size_t i = 0;igetMapRoutingGraph()->left(starting_lane[0])){ + curr_end_lanelet = wm->getMapRoutingGraph()->left(starting_lane[0]).get(); + } + else{ + curr_end_lanelet = wm->getMapRoutingGraph()->adjacentLeft(starting_lane[0]).get(); + } + } + else{ + + //get right lanelet + if(wm->getMapRoutingGraph()->right(starting_lane[0])){ + curr_end_lanelet = wm->getMapRoutingGraph()->right(starting_lane[0]).get(); + } + else{ + curr_end_lanelet = wm->getMapRoutingGraph()->adjacentRight(starting_lane[0]).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. + // Value picked based on annecdotal evidence from STOL system testing + + std::vector downsampled_starting_centerline; + downsampled_starting_centerline.reserve(400); + downsampled_starting_centerline = carma_utils::containers::downsample_vector(reference_centerline, downsample_ratio); + + std::vector downsampled_target_centerline; + downsampled_target_centerline.reserve(400); + downsampled_target_centerline = carma_utils::containers::downsample_vector(target_lane_centerline, downsample_ratio); + + //If points are not the same size - resample to ensure same size along both centerlines + if(downsampled_starting_centerline.size() != downsampled_target_centerline.size()) + { + auto centerlines = resample_linestring_pair_to_same_size(downsampled_starting_centerline, downsampled_target_centerline); + downsampled_starting_centerline = centerlines.front(); + downsampled_target_centerline = centerlines.back(); + + } + + //Constrain to starting and ending downtrack + int start_index_starting_centerline = waypoint_generation::get_nearest_index_by_downtrack(downsampled_starting_centerline, wm, starting_downtrack); + cav_msgs::VehicleState start_state; + start_state.X_pos_global = downsampled_starting_centerline[start_index_starting_centerline].x(); + start_state.Y_pos_global = downsampled_starting_centerline[start_index_starting_centerline].y(); + int start_index_target_centerline = waypoint_generation::get_nearest_point_index(downsampled_target_centerline, start_state); + + int end_index_target_centerline = waypoint_generation::get_nearest_index_by_downtrack(downsampled_target_centerline, wm, ending_downtrack); + cav_msgs::VehicleState end_state; + end_state.X_pos_global = downsampled_target_centerline[end_index_target_centerline].x(); + end_state.Y_pos_global = downsampled_target_centerline[end_index_target_centerline].y(); + int end_index_starting_centerline = waypoint_generation::get_nearest_point_index(downsampled_starting_centerline, end_state); + + std::vector constrained_start_centerline(downsampled_starting_centerline.begin() + start_index_starting_centerline, downsampled_starting_centerline.begin() + end_index_starting_centerline); + std::vector constrained_target_centerline(downsampled_target_centerline.begin() + start_index_target_centerline, downsampled_target_centerline.begin() + end_index_target_centerline); + + //Create Trajectory geometry + double delta_step = 1.0 / constrained_start_centerline.size(); + + for (size_t i = 0; i < constrained_start_centerline.size(); ++i) + { + lanelet::BasicPoint2d current_position; + lanelet::BasicPoint2d start_lane_pt = constrained_start_centerline[i]; + lanelet::BasicPoint2d target_lane_pt = constrained_target_centerline[i]; + double delta = delta_step * i; + current_position.x() = target_lane_pt.x() * delta + (1 - delta) * start_lane_pt.x(); + current_position.y() = target_lane_pt.y() * delta + (1 - delta) * start_lane_pt.y(); + + centerline_points.push_back(current_position); + } + + //Add points from following lanelet to provide sufficient distance for adding buffer + auto following_lanelets = wm->getMapRoutingGraph()->following(ending_lanelet, false); + 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(), + following_lanelet_centerline.end()); + } + + return centerline_points; + } + + std::vector> resample_linestring_pair_to_same_size(std::vector& line_1, std::vector& line_2){ + + std::vector> output; + ros::WallTime start_time = ros::WallTime::now(); // Start timing the execution time for planning so it can be logged + + //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) + { + throw std::invalid_argument("Could not fit a spline curve along the starting_lane centerline points!"); + } + + std::unique_ptr fit_curve_2 = compute_fit(line_2); // Compute splines based on curve points + if (!fit_curve_2) + { + throw std::invalid_argument("Could not fit a spline curve along the ending_lane centerline points!"); + } + + //Sample spline to get centerlines of equal size + std::vector all_sampling_points_line1; + std::vector all_sampling_points_line2; + + size_t total_point_size = std::min(line_1.size(), line_2.size()); + + all_sampling_points_line1.reserve(1 + total_point_size * 2); + std::vector downtracks_raw_line1 = carma_wm::geometry::compute_arc_lengths(line_1); + int total_step_along_curve1 = static_cast(downtracks_raw_line1.back() / 2.0); + double step_threshold_line1 = (double)total_step_along_curve1 / (double)total_point_size; + + all_sampling_points_line2.reserve(1 + total_point_size * 2); + std::vector downtracks_raw_line2 = carma_wm::geometry::compute_arc_lengths(line_2); + int total_step_along_curve2 = static_cast(downtracks_raw_line2.back() / 2.0); + 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 get_lanechange_points_from_maneuver(const cav_msgs::Maneuver &maneuver, double starting_downtrack, const carma_wm::WorldModelConstPtr &wm, cav_msgs::VehicleState &ending_state_before_buffer, - const cav_msgs::VehicleState &state, const DetailedTrajConfig &detailed_config) + const cav_msgs::VehicleState &state, const GeneralTrajConfig &general_config,const DetailedTrajConfig &detailed_config) { if(maneuver.type != cav_msgs::Maneuver::LANE_CHANGE){ throw std::invalid_argument("Create_lanechange called on a maneuver type which is not LANE_CHANGE"); @@ -275,8 +486,10 @@ namespace basic_autonomy throw(std::invalid_argument("Start distance is greater than or equal to ending distance")); } - //get route between starting and ending downtracks - std::vector route_geometry = create_route_geom(starting_downtrack, stoi(lane_change_maneuver.starting_lane_id), ending_downtrack + detailed_config.buffer_ending_downtrack, wm); + //get route between starting and ending downtracks - downtracks should be constant for complete length of maneuver + double lanechange_starting_downtrack; + std::vector route_geometry = create_lanechange_geometry(std::stoi(lane_change_maneuver.starting_lane_id),std::stoi(lane_change_maneuver.ending_lane_id), + starting_downtrack, ending_downtrack, wm, state, general_config.default_downsample_ratio); ROS_DEBUG_STREAM("Route geometry size:"< future_geom_points; std::vector final_actual_speeds; split_point_speed_pairs(future_points, &future_geom_points, &final_actual_speeds); - std::vector final_yaw_values = carma_wm::geometry::compute_tangent_orientations(future_geom_points); + std::unique_ptr fit_curve = compute_fit(future_geom_points); + if(!fit_curve){ + throw std::invalid_argument("Could not fit a spline curve along the given trajectory!"); + } + ROS_DEBUG("Got fit"); + std::vector all_sampling_points; + all_sampling_points.reserve(1 + future_geom_points.size() * 2); + + //Compute points to local downtracks std::vector downtracks = carma_wm::geometry::compute_arc_lengths(future_geom_points); + + auto total_step_along_curve = static_cast(downtracks.back() /detailed_config.curve_resample_step_size); + size_t total_point_size = future_geom_points.size(); + + double scaled_steps_along_curve = 0.0; //from 0 (start) to 1 (end) for the whole trajectory + + for(size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++){ + lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve); + + all_sampling_points.push_back(p); + + scaled_steps_along_curve += 1.0 / total_step_along_curve; + } + ROS_DEBUG_STREAM("Got sampled points with size:" << all_sampling_points.size()); + + std::vector final_yaw_values = carma_wm::geometry::compute_tangent_orientations(future_geom_points); final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, detailed_config.speed_moving_average_window_size); //Convert speeds to time @@ -821,94 +1058,6 @@ namespace basic_autonomy return traj_points; } - lanelet::BasicLineString2d create_lanechange_path(const lanelet::ConstLanelet &start_lanelet, const lanelet::ConstLanelet &end_lanelet) - { - std::vector centerline_points = {}; - lanelet::BasicLineString2d centerline_start_lane = start_lanelet.centerline2d().basicLineString(); - lanelet::BasicLineString2d centerline_end_lane = end_lanelet.centerline2d().basicLineString(); - - auto total_points = std::min(centerline_start_lane.size(), centerline_end_lane.size()); - double delta_step = 1.0 / total_points; - - for (int i = 0; i < total_points; i++) - { - lanelet::BasicPoint2d current_position; - lanelet::BasicPoint2d start_lane_pt = centerline_start_lane[i]; - lanelet::BasicPoint2d end_lane_pt = centerline_end_lane[i]; - double delta = delta_step * i; - current_position.x() = end_lane_pt.x() * delta + (1 - delta) * start_lane_pt.x(); - current_position.y() = end_lane_pt.y() * delta + (1 - delta) * start_lane_pt.y(); - - centerline_points.push_back(current_position); - } - - std::unique_ptr fit_curve = compute_fit(centerline_points); - if (!fit_curve) - { - throw std::invalid_argument("Could not fit a spline curve along the given trajectory!"); - } - - lanelet::BasicLineString2d lc_route; - - double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory - for (int i = 0; i < centerline_points.size(); i++) - { - lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve); - lc_route.push_back(p); - scaled_steps_along_curve += 1.0 / total_points; - } - lc_route.push_back(centerline_end_lane.back()); // at 1.0 scaled steps - - return lc_route; - } - - std::vector create_route_geom(double starting_downtrack, int starting_lane_id, double ending_downtrack, const carma_wm::WorldModelConstPtr &wm) - { - //Create route geometry for lane change maneuver - //Starting downtrack maybe in the previous lanelet, if it is, have a lane follow till new lanelet starts - std::vector lanelets_in_path = wm->getLaneletsBetween(starting_downtrack, ending_downtrack, true); - lanelet::BasicLineString2d centerline_points = {}; - int lane_change_iteration = 0; - if (lanelets_in_path[0].id() != starting_lane_id) - { - bool lane_change_req = false; - //Check if future lanelet is lane change lanelet - for (int i = 0; i < lanelets_in_path.size(); i++) - { - if (lanelets_in_path[i].id() == starting_lane_id) - { - lane_change_req = true; - lane_change_iteration = i; - break; - } - } - if (!lane_change_req) - { - throw std::invalid_argument("Current path does not require a lane change. Request Incorrectly sent to Cooperative lane change plugin"); - } - //lane_follow till lane_change req - lanelet::BasicLineString2d new_points = lanelets_in_path[lane_change_iteration - 1].centerline2d().basicLineString(); - lanelet::BasicLineString2d new_points_spliced(new_points.begin(), new_points.end() - 1); - centerline_points.insert(centerline_points.end(), new_points_spliced.begin(), new_points_spliced.end()); - } - - lanelet::BasicLineString2d new_points = create_lanechange_path(lanelets_in_path[lane_change_iteration],lanelets_in_path[lane_change_iteration + 1]); - centerline_points.insert(centerline_points.end(), new_points.begin(), new_points.end()); - - //Add points from following lanelet to provide sufficient distance for adding buffer - auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets_in_path[lane_change_iteration + 1], false); - - 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() + 1, - following_lanelet_centerline.end()); - } - - std::vector centerline_as_vector(centerline_points.begin(), centerline_points.end()); - - return centerline_as_vector; - } } } \ No newline at end of file diff --git a/basic_autonomy/src/helper_functions.cpp b/basic_autonomy/src/helper_functions.cpp index a256d37800..3a6de36b31 100644 --- a/basic_autonomy/src/helper_functions.cpp +++ b/basic_autonomy/src/helper_functions.cpp @@ -112,5 +112,6 @@ namespace waypoint_generation double ending_downtrack = wm->routeTrackPos(state_pos).downtrack; return get_nearest_index_by_downtrack(points, wm, ending_downtrack); } + } } \ 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 f93123ac2a..6188d8a44a 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -590,6 +590,96 @@ namespace basic_autonomy ASSERT_NEAR(6.0, result[4].point.y(), 0.0000001); } + TEST(BasicAutonomyTest, test_lanechange_trajectory) + { + //Case 1: Lane change from start to end of adjacent lanelets + std::string path = ros::package::getPath("basic_autonomy"); + std::string file = "/resource/map/town01_vector_map_lane_change.osm"; + file = path.append(file); + int projector_type = 0; + std::string target_frame; + lanelet::ErrorMessages load_errors; + lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); + lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); + lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); + if (map->laneletLayer.size() == 0) + { + FAIL() << "Input map does not contain any lanelets"; + } + std::shared_ptr cwm = std::make_shared(); + cwm->carma_wm::CARMAWorldModel::setMap(map); + + //Set Route + lanelet::Id start_id = 106; + lanelet::Id end_id = 111; + + carma_wm::test::setRouteByIds({start_id,end_id},cwm); + //get starting position + auto shortest_path = cwm->getRoute()->shortestPath(); + lanelet::BasicPoint2d veh_pos = shortest_path[0].centerline2d().front(); + double starting_downtrack =cwm->routeTrackPos(veh_pos).downtrack; + double ending_downtrack = cwm->routeTrackPos(shortest_path.back().centerline2d().back()).downtrack; + + //Arguments for create geometry profile function- + cav_msgs::Maneuver maneuver; + maneuver.type = cav_msgs::Maneuver::LANE_CHANGE; + maneuver.lane_change_maneuver.start_dist = starting_downtrack; + maneuver.lane_change_maneuver.end_dist = ending_downtrack; + maneuver.lane_change_maneuver.start_speed = 5.0; + maneuver.lane_change_maneuver.start_time = ros::Time::now(); + //calculate end_time assuming constant acceleration + double acc = pow(maneuver.lane_change_maneuver.start_speed, 2) / (2 * (ending_downtrack - starting_downtrack)); + double end_time = maneuver.lane_change_maneuver.start_speed / acc; + maneuver.lane_change_maneuver.end_speed = 25.0; + maneuver.lane_change_maneuver.end_time = ros::Time(end_time + 10.0); + maneuver.lane_change_maneuver.starting_lane_id = std::to_string(start_id); + maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); + + std::vector maneuvers; + maneuvers.push_back(maneuver); + cav_msgs::VehicleState state; + state.X_pos_global = veh_pos.x(); + state.Y_pos_global = veh_pos.y(); + state.longitudinal_vel = 8.0; + + std::string trajectory_type = "cooperative_lanechange"; + 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); + double maneuver_fraction_completed; + cav_msgs::VehicleState ending_state; + + std::vector points = basic_autonomy::waypoint_generation::create_geometry_profile(maneuvers, + starting_downtrack, cwm, ending_state, state, general_config, config); + ros::Time state_time = ros::Time::now(); + double target_speed = 11.176; + + EXPECT_EQ(points.back().speed, state.longitudinal_vel); + + + //Case 2: Complete lane change before end of lanelet + 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, + starting_downtrack, cwm, ending_state, state, general_config, config); + + EXPECT_TRUE(points.size() > 4); + + //Test resample linestring + lanelet::BasicLineString2d line_one = shortest_path.front().centerline2d().basicLineString(); + std::vector line_1; + for(int i = 0;i line_2; + for(int i = 0;i> linestrings = basic_autonomy::waypoint_generation::resample_linestring_pair_to_same_size(line_1, line_2); + + } + TEST(BasicAutonomyTest, maneuvers_to_lanechange_points) { @@ -643,8 +733,8 @@ namespace basic_autonomy state.longitudinal_vel = 8.0; std::string trajectory_type = "cooperative_lanechange"; - waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 0, 0); - const waypoint_generation::DetailedTrajConfig config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20); + 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); double maneuver_fraction_completed; cav_msgs::VehicleState ending_state; @@ -656,19 +746,7 @@ namespace basic_autonomy std::vector trajectory_points = basic_autonomy::waypoint_generation::compose_lanechange_trajectory_from_path(points, state, state_time, cmw, ending_state, config); EXPECT_TRUE(trajectory_points.size() > 2); - - basic_autonomy::waypoint_generation::create_route_geom(starting_downtrack, int(start_id), ending_downtrack, cmw); - - lanelet::ConstLanelet start_lanelet = shortest_path.front(); - lanelet::BasicPoint2d lc_start_point = start_lanelet.centerline2d().front(); - lanelet::ConstLanelet end_lanelet = shortest_path.back(); - lanelet::BasicPoint2d lc_end_point = end_lanelet.centerline2d().back(); - //create lanechange path creates the actual lanechange path. From starting of first lanelet's centerline, to the end of the adjacent - //lanelet's centerline - lanelet::BasicLineString2d lc_geom = basic_autonomy::waypoint_generation::create_lanechange_path(start_lanelet, end_lanelet); - - ASSERT_NEAR(lc_start_point.y(), lc_geom.front().y(), 0.000001); - ASSERT_NEAR(lc_start_point.x(), lc_geom.front().x(), 0.000001); + basic_autonomy::waypoint_generation::create_lanechange_geometry(start_id, end_id,starting_downtrack, ending_downtrack, cmw, state, 1); } TEST(BasicAutonomyTest, lanefollow_geometry_visited_lanelets) diff --git a/bsm_generator/CMakeLists.txt b/bsm_generator/CMakeLists.txt index 4b80563cd9..21f06e3898 100644 --- a/bsm_generator/CMakeLists.txt +++ b/bsm_generator/CMakeLists.txt @@ -32,7 +32,6 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs pacmod_msgs j2735_msgs - novatel_gps_msgs wgs84_utils lanelet2_extension ) @@ -46,7 +45,7 @@ find_package(Boost REQUIRED COMPONENTS system) catkin_package( CATKIN_DEPENDS roscpp std_msgs cav_msgs carma_utils automotive_platform_msgs - geometry_msgs pacmod_msgs j2735_msgs novatel_gps_msgs wgs84_utils lanelet2_extension + geometry_msgs pacmod_msgs j2735_msgs wgs84_utils lanelet2_extension ) ########### diff --git a/bsm_generator/config/parameters.yaml b/bsm_generator/config/parameters.yaml index 8cd7fb06c6..4713ce5727 100644 --- a/bsm_generator/config/parameters.yaml +++ b/bsm_generator/config/parameters.yaml @@ -1,2 +1,3 @@ # The desired frequency for BSM generation bsm_generation_frequency: 10.0 + diff --git a/bsm_generator/include/bsm_generator.h b/bsm_generator/include/bsm_generator.h index 3631c3f804..5c5d4d7553 100644 --- a/bsm_generator/include/bsm_generator.h +++ b/bsm_generator/include/bsm_generator.h @@ -22,12 +22,20 @@ #include #include #include +#include +#include #include #include #include -#include #include #include +#include +#include +#include +#include +#include +#include +#include #include "bsm_generator_worker.h" namespace bsm_generator @@ -70,6 +78,12 @@ namespace bsm_generator // frequency for bsm generation double bsm_generation_frequency_; + //Enable/disable rotation of BSM ID during vehicle operations + bool bsm_id_rotation_enabled_; + + std::vector bsm_message_id_; + + // size of the vehicle double vehicle_length_, vehicle_width_; @@ -88,12 +102,12 @@ namespace bsm_generator // callbacks for the subscribers void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg); void accelCallback(const automotive_platform_msgs::VelocityAccelCovConstPtr& msg); - void yawCallback(const pacmod_msgs::YawRateRptConstPtr& msg); + void yawCallback(const sensor_msgs::ImuConstPtr& msg); void gearCallback(const j2735_msgs::TransmissionStateConstPtr& msg); - void speedCallback(const std_msgs::Float64ConstPtr& msg); + void speedCallback(const geometry_msgs::TwistStampedPtr& msg); void steerWheelAngleCallback(const std_msgs::Float64ConstPtr& msg); void brakeCallback(const std_msgs::Float64ConstPtr& msg); - void headingCallback(const novatel_gps_msgs::NovatelDualAntennaHeadingConstPtr& msg); + void headingCallback(const gps_common::GPSFixConstPtr& msg); /** * \brief Callback for map projection string to define lat/lon -> map conversion diff --git a/bsm_generator/launch/bsm_generator.launch b/bsm_generator/launch/bsm_generator.launch index b662105305..ccf3dd0109 100644 --- a/bsm_generator/launch/bsm_generator.launch +++ b/bsm_generator/launch/bsm_generator.launch @@ -15,5 +15,10 @@ --> - + + + + + + diff --git a/bsm_generator/package.xml b/bsm_generator/package.xml index d92e4c4fc8..e9f05551a5 100644 --- a/bsm_generator/package.xml +++ b/bsm_generator/package.xml @@ -32,7 +32,6 @@ geometry_msgs pacmod_msgs j2735_msgs - novatel_gps_msgs wgs84_utils lanelet2_extension diff --git a/bsm_generator/src/bsm_generator.cpp b/bsm_generator/src/bsm_generator.cpp index 5ac627f2cc..3ddb2d020a 100644 --- a/bsm_generator/src/bsm_generator.cpp +++ b/bsm_generator/src/bsm_generator.cpp @@ -23,21 +23,29 @@ namespace bsm_generator void BSMGenerator::initialize() { + int bsmid; nh_.reset(new ros::CARMANodeHandle()); pnh_.reset(new ros::CARMANodeHandle("~")); pnh_->param("bsm_generation_frequency", bsm_generation_frequency_, 10.0); + pnh_->param("/bsm_id_rotation_enabled", bsm_id_rotation_enabled_, true); + pnh_->param("/bsm_message_id", bsmid, 0); + for(size_t i = 0; i < 4; ++i ) //As the BSM Messsage ID is a four-element vector, the loop should iterate four times. + { + bsm_message_id_.emplace_back( bsmid >> (8 * i) ); + } + nh_->param("vehicle_length", vehicle_length_, 5.0); nh_->param("vehicle_width", vehicle_width_, 2.0); bsm_pub_ = nh_->advertise("bsm_outbound", 5); timer_ = nh_->createTimer(ros::Duration(1.0 / bsm_generation_frequency_), &BSMGenerator::generateBSM, this); gear_sub_ = nh_->subscribe("transmission_state", 1, &BSMGenerator::gearCallback, this); - speed_sub_ = nh_->subscribe("vehicle_speed_cov", 1, &BSMGenerator::speedCallback, this); + speed_sub_ = nh_->subscribe("ekf_twist", 1, &BSMGenerator::speedCallback, this); steer_wheel_angle_sub_ = nh_->subscribe("steering_wheel_angle", 1, &BSMGenerator::steerWheelAngleCallback, this); accel_sub_ = nh_->subscribe("velocity_accel_cov", 1, &BSMGenerator::accelCallback, this); - yaw_sub_ = nh_->subscribe("yaw_rate_rpt", 1, &BSMGenerator::yawCallback, this); + yaw_sub_ = nh_->subscribe("imu_raw", 1, &BSMGenerator::yawCallback, this); brake_sub_ = nh_->subscribe("brake_position", 1, &BSMGenerator::brakeCallback, this); pose_sub_ = nh_->subscribe("pose", 1, &BSMGenerator::poseCallback, this); - heading_sub_ = nh_->subscribe("dual_antenna_heading", 1, &BSMGenerator::headingCallback, this); + heading_sub_ = nh_->subscribe("gnss_fix_fused", 1, &BSMGenerator::headingCallback, this); georeference_sub_ = nh_->subscribe("georeference", 1, &BSMGenerator::georeferenceCallback, this); } @@ -62,9 +70,9 @@ namespace bsm_generator map_projector_ = std::make_shared(msg->data.c_str()); // Build projector from proj string } - void BSMGenerator::speedCallback(const std_msgs::Float64ConstPtr& msg) + void BSMGenerator::speedCallback(const geometry_msgs::TwistStampedPtr& msg) { - bsm_.core_data.speed = worker.getSpeedInRange(msg->data); + bsm_.core_data.speed = worker.getSpeedInRange(msg->twist.linear.x); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.SPEED_AVAILABLE; } @@ -85,9 +93,9 @@ namespace bsm_generator bsm_.core_data.accelSet.presence_vector = bsm_.core_data.accelSet.presence_vector | bsm_.core_data.accelSet.ACCELERATION_AVAILABLE; } - void BSMGenerator::yawCallback(const pacmod_msgs::YawRateRptConstPtr& msg) + void BSMGenerator::yawCallback(const sensor_msgs::ImuConstPtr& msg) { - bsm_.core_data.accelSet.yaw_rate = worker.getYawRateInRange(msg->yaw_rate); + bsm_.core_data.accelSet.yaw_rate = worker.getYawRateInRange(static_cast(msg->angular_velocity.z)); bsm_.core_data.accelSet.presence_vector = bsm_.core_data.accelSet.presence_vector | bsm_.core_data.accelSet.YAWRATE_AVAILABLE; } @@ -115,16 +123,25 @@ namespace bsm_generator } - void BSMGenerator::headingCallback(const novatel_gps_msgs::NovatelDualAntennaHeadingConstPtr& msg) + void BSMGenerator::headingCallback(const gps_common::GPSFixConstPtr& msg) { - bsm_.core_data.heading = worker.getHeadingInRange(msg->heading); + bsm_.core_data.heading = worker.getHeadingInRange(static_cast(msg->track)); + bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.HEADING_AVAILABLE; } void BSMGenerator::generateBSM(const ros::TimerEvent& event) { bsm_.header.stamp = ros::Time::now(); bsm_.core_data.msg_count = worker.getNextMsgCount(); - bsm_.core_data.id = worker.getMsgId(ros::Time::now()); + + if(bsm_id_rotation_enabled_) + { + bsm_.core_data.id = worker.getMsgId(ros::Time::now()); + } + else + { + bsm_.core_data.id = bsm_message_id_; + } bsm_.core_data.sec_mark = worker.getSecMark(ros::Time::now()); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.SEC_MARK_AVAILABLE; // currently the accuracy is not available because ndt_matching does not provide accuracy measurement diff --git a/carma-platform.repos b/carma-platform.repos index 4eb43efcad..f09ef0f9a3 100644 --- a/carma-platform.repos +++ b/carma-platform.repos @@ -6,7 +6,7 @@ repositories: src/autoware.ai: type: git url: git@github.com:usdot-fhwa-stol/autoware.ai.git - version: develop + version: carma-develop src/avt_vimba_camera: type: git url: git@github.com:usdot-fhwa-stol/avt_vimba_camera.git diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 88adfdd77e..4cb51db0c2 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -231,6 +231,16 @@ + + + + + + + + + + diff --git a/carma_wm/CMakeLists.txt b/carma_wm/CMakeLists.txt index 1acc5ed778..25c45365fe 100644 --- a/carma_wm/CMakeLists.txt +++ b/carma_wm/CMakeLists.txt @@ -79,6 +79,7 @@ add_library( src/TrafficControl.cpp src/IndexedDistanceMap.cpp src/collision_detection.cpp + src/SignalizedIntersectionManager.cpp ) ## Add cmake target dependencies of the library @@ -136,6 +137,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gmock(${PROJECT_NAME}-test test/TestMain.cpp test/CARMAWorldModelTest.cpp + test/SignalizedIntersectionManagerTest.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests ) diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.h b/carma_wm/include/carma_wm/CARMAWorldModel.h index fd62742ad2..9d813bddce 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.h +++ b/carma_wm/include/carma_wm/CARMAWorldModel.h @@ -30,7 +30,7 @@ #include "TrackPos.h" #include #include "boost/date_time/posix_time/posix_time.hpp" - +#include namespace carma_wm { @@ -216,12 +216,14 @@ class CARMAWorldModel : public WorldModel std::vector nonConnectedAdjacentLeft(const lanelet::BasicPoint2d& input_point, const unsigned int n = 10) const override; - std::vector getLightsAlongRoute(const lanelet::BasicPoint2d& loc) const override; + std::vector getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const override; std::vector> getIntersectionsAlongRoute(const lanelet::BasicPoint2d& loc) const override; std::unordered_map traffic_light_ids_; + carma_wm::SignalizedIntersectionManager sim_; // records SPAT/MAP lane ids to lanelet ids + private: double config_speed_limit_; @@ -250,7 +252,7 @@ class CARMAWorldModel : public WorldModel LaneletRoutePtr route_; LaneletRoutingGraphPtr map_routing_graph_; double route_length_ = 0; - std::unordered_map>>> traffic_light_states_; //[intersection_id][signal_group_id] + std::unordered_map>>> traffic_light_states_; //[intersection_id][signal_group_id] lanelet::LaneletSubmapConstUPtr shortest_path_view_; // Map containing only lanelets along the shortest path of the // route std::vector shortest_path_centerlines_; // List of disjoint centerlines seperated by lane diff --git a/carma_wm/include/carma_wm/SignalizedIntersectionManager.h b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h new file mode 100644 index 0000000000..df60a290ef --- /dev/null +++ b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h @@ -0,0 +1,143 @@ +#pragma once + +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace carma_wm +{ + +struct LANE_DIRECTION +{ + static const uint8_t INGRESS = 1; + static const uint8_t EGRESS = 2; +}; + +using namespace lanelet::units::literals; + +/*! \brief This class manages and keeps track of all signalized intersections in the map. + All of the SPAT and MAP standard's lane ids to lanelet id mapping is recorded here. + NOTE: This class functions do not update the map given. + */ +class SignalizedIntersectionManager +{ +public: + SignalizedIntersectionManager(){} + + /*! + * \brief Create relevant signalized intersection and carma traffic signals based on the MAP.msg and the lanelet_map + NOTE: The function does not update the map with new elements + * \param[out] intersections to return + * \param[out] traffic_signals to return + * \param map_msg MAP.msg that consists all static data portion of the intersection + * \param map lanelet_map to check + * \param routing_graph of the lanelet map to accurately detect lanes + */ + void createIntersectionFromMapMsg(std::vector& intersections, std::vector& traffic_signals, const cav_msgs::MapData& map_msg, + const std::shared_ptr& map, std::shared_ptr routing_graph); + + /*! + * \brief Returns mapping of MAP lane id to lanelet id for the given map and intersection.msg in the MAP.msg. + This function also records signal_group_id_to its own lanelet id, and also signal group to entry and exit lanelets id mappings + * \param[out] entry lane id to lanelet id mappings to return + * \param[out] exit lane id to lanelet id mappings to return + * \param intersection MAP.msg that consists all static data portion of the intersection + * \param map lanelet_map to check + * \param routing_graph of the lanelet map to accurately detect lanes + * \throw invalid_argument if given coordinates in the msg doesn't exist in the map + * TODO: Need to think about error buffer in the future. Map msgs are made from google maps or open streets maps normally so this function might run into some errors from that. + */ + void convertLaneToLaneletId(std::unordered_map& entry, std::unordered_map& exit, const cav_msgs::IntersectionGeometry& intersection, + const std::shared_ptr& map, std::shared_ptr current_routing_graph); + + /*! + * \brief Sets the max lane width in meters. Map msg points are associated to a lanelet if they are + * within this distance to a lanelet as map msg points are guaranteed to apply to a single lane + */ + void setMaxLaneWidth(double max_lane_width); + + /*! + * \brief Returns existing signalized intersection with same entry and exit llts if exists. + * \param entry_llts of the intersection + * \param exit_llts of the intersection + * \param map lanelet_map to check + * \return id of the matching intersection in the map, or lanelet::InvalId if none exists + */ + lanelet::Id matchSignalizedIntersection(const lanelet::Lanelets& entry_llts, const lanelet::Lanelets& exit_llts, const std::shared_ptr& map); + + /*! + * \brief Saves the georeference string to be used for converting MAP.msg coordinates + * \param target_frame PROJ string of the map + */ + void setTargetFrame(const std::string& target_frame); + + /*! + * \brief Returns carma traffic signal and saves all id relevant id mappings (signal group to lanelet id) internally + * \param signal_group_id of the traffic signal + * \param exit_llts of the signal_group + * \param entry_llts of the signal_group + * \return traffic signal corresponding to the signal group + */ + std::shared_ptr createTrafficSignalUsingSGID(uint8_t signal_group_id, const lanelet::Lanelets& entry_lanelets, const lanelet::Lanelets& exit_lanelets); + + /*! + * \brief Returns carma traffic signal and saves all id relevant id mappings (signal group to lanelet id) internally + * \param signal_group_id of the traffic signal + * \param exit_llts of the signal_group + * \param entry_llts of the signal_group + * \return traffic signal corresponding to the signal group + */ + lanelet::Lanelets identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr& map); + + // SignalizedIntersection quick id lookup + std::unordered_map intersection_id_to_regem_id_; + + // CarmaTrafficSignal quick id lookup + std::unordered_map signal_group_to_traffic_light_id_; + + // CarmaTrafficSignal exit lanelets ids quick lookup + std::unordered_map> signal_group_to_exit_lanelet_ids_; + + // CarmaTrafficSignal entry lanelets ids quick lookup + std::unordered_map> signal_group_to_entry_lanelet_ids_; + + // Traffic signal states and their end_time mappings. + std::unordered_map>>> traffic_signal_states_; //[intersection_id][signal_group_id] + +private: + + // PROJ string of current map + std::string target_frame_ = ""; + + // Max width of lane in meters + double max_lane_width_ = 4; +}; + +} // namespace carma_wm \ No newline at end of file diff --git a/carma_wm/include/carma_wm/TrafficControl.h b/carma_wm/include/carma_wm/TrafficControl.h index 996c7ae59c..5783b73d7e 100644 --- a/carma_wm/include/carma_wm/TrafficControl.h +++ b/carma_wm/include/carma_wm/TrafficControl.h @@ -26,9 +26,10 @@ #include #include #include -#include +#include #include #include +#include #include @@ -59,6 +60,9 @@ class TrafficControl // traffic light id lookup std::vector> traffic_light_id_lookup_; + + // signalized intersection manager + carma_wm::SignalizedIntersectionManager sim_; }; /** @@ -115,6 +119,9 @@ inline void save(Archive& ar, const carma_wm::TrafficControl& gf, unsigned int / size_t traffic_light_id_lookup_size = gf.traffic_light_id_lookup_.size(); ar << traffic_light_id_lookup_size; for (auto pair : gf.traffic_light_id_lookup_) ar << pair; + + // convert signalized intersection manager + ar << gf.sim_; } template @@ -165,6 +172,115 @@ inline void load(Archive& ar, carma_wm::TrafficControl& gf, unsigned int /*versi ar >> traffic_light_id_pair; gf.traffic_light_id_lookup_.push_back(traffic_light_id_pair); } + + // save signalized intersection manager + ar >> gf.sim_; +} + + +template +// NOLINTNEXTLINE +inline void save(Archive& ar, const carma_wm::SignalizedIntersectionManager& sim, unsigned int /*version*/) +{ + // convert traffic light id lookup + size_t intersection_id_to_regem_id_size = sim.intersection_id_to_regem_id_.size(); + ar << intersection_id_to_regem_id_size; + for (auto pair : sim.intersection_id_to_regem_id_) + { + ar << pair; + } + + // convert traffic light id lookup + size_t signal_group_to_traffic_light_id_size = sim.signal_group_to_traffic_light_id_.size(); + ar << signal_group_to_traffic_light_id_size; + for (auto pair : sim.signal_group_to_traffic_light_id_) + { + ar << pair; + } + + size_t signal_group_to_exit_lanelet_ids_size = sim.signal_group_to_exit_lanelet_ids_.size(); + ar << signal_group_to_exit_lanelet_ids_size; + for (auto pair : sim.signal_group_to_exit_lanelet_ids_) + { + size_t set_size = pair.second.size(); + ar << set_size; + ar << pair.first; + for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + { + ar << *iter; + } + } + + size_t signal_group_to_entry_lanelet_ids_size = sim.signal_group_to_entry_lanelet_ids_.size(); + ar << signal_group_to_entry_lanelet_ids_size; + for (auto pair : sim.signal_group_to_entry_lanelet_ids_) + { + size_t set_size = pair.second.size(); + ar << set_size; + ar << pair.first; + for (auto iter = pair.second.begin(); iter != pair.second.end(); iter++) + { + ar << *iter; + } + } +} + +template +// NOLINTNEXTLINE +inline void load(Archive& ar, carma_wm::SignalizedIntersectionManager& sim, unsigned int /*version*/) +{ + // save traffic light id lookup + size_t intersection_id_to_regem_id_size; + ar >> intersection_id_to_regem_id_size; + for (size_t i = 0; i < intersection_id_to_regem_id_size; i ++) + { + std::pair pair; + ar >> pair; + sim.intersection_id_to_regem_id_.emplace(pair); + } + + // save traffic light id lookup + size_t signal_group_to_traffic_light_id_size; + ar >> signal_group_to_traffic_light_id_size; + for (size_t i = 0; i < signal_group_to_traffic_light_id_size; i ++) + { + std::pair pair; + ar >> pair; + sim.signal_group_to_traffic_light_id_.emplace(pair); + } + + size_t signal_group_to_exit_lanelet_ids_size; + ar >> signal_group_to_exit_lanelet_ids_size; + for (size_t i = 0; i < signal_group_to_exit_lanelet_ids_size; i ++) + { + size_t set_size; + ar >> set_size; + uint8_t signal_grp; + ar >> signal_grp; + for (size_t j = 0; j >id; + sim.signal_group_to_exit_lanelet_ids_[signal_grp].insert(id); + } + } + + size_t signal_group_to_entry_lanelet_ids_size; + ar >> signal_group_to_entry_lanelet_ids_size; + for (size_t i = 0; i < signal_group_to_entry_lanelet_ids_size; i ++) + { + size_t set_size; + ar >> set_size; + uint8_t signal_grp; + ar >> signal_grp; + for (size_t j = 0; j >id; + sim.signal_group_to_entry_lanelet_ids_[signal_grp].insert(id); + } + } + } template @@ -174,6 +290,20 @@ void serialize(Archive& ar, std::pair +void serialize(Archive& ar, std::pair& p, unsigned int /*version*/) +{ + ar& p.first; + ar& p.second; +} + +template +void serialize(Archive& ar, std::pair& p, unsigned int /*version*/) +{ + ar& p.first; + ar& p.second; +} + template void serialize(Archive& ar, std::pair& p, unsigned int /*version*/) { @@ -184,4 +314,5 @@ void serialize(Archive& ar, std::pair& p, unsigned int /* } // namespace serialization } // namespace boost -BOOST_SERIALIZATION_SPLIT_FREE(carma_wm::TrafficControl); \ No newline at end of file +BOOST_SERIALIZATION_SPLIT_FREE(carma_wm::TrafficControl); +BOOST_SERIALIZATION_SPLIT_FREE(carma_wm::SignalizedIntersectionManager); diff --git a/carma_wm/include/carma_wm/WMTestLibForGuidance.h b/carma_wm/include/carma_wm/WMTestLibForGuidance.h index 81a6bbc412..02c6cc4cd4 100644 --- a/carma_wm/include/carma_wm/WMTestLibForGuidance.h +++ b/carma_wm/include/carma_wm/WMTestLibForGuidance.h @@ -30,7 +30,7 @@ #include #include #include -#include +#include /** * This is a test library made for guidance unit tests. In general, it includes the following : * - Helper functions to create the world from scratch or extend the world in getGuidanceTestMap() @@ -488,16 +488,16 @@ inline void setRouteByIds (std::vector lanelet_ids, std::shared_ptr * \param light_id The lanelet id to use for the generated traffic light regulatory element. This id should NOT be present in the map prior to this method call * \param owning_lanelet_id The id of the lanelet which will own thr traffic light element. This id MUST be present in the map prior to this method being called * \param controlled_lanelet_ids A list of lanelet ids which the traffic light will control access to. The ids MUST be present in the map. Additionally, they should be listed in order along the direction of travel. - * \param timeing_plan Optional parameter that is the timing plan to use for the light. The specifications match those of CarmaTrafficLightState.setStates() + * \param timeing_plan Optional parameter that is the timing plan to use for the light. The specifications match those of CarmaTrafficSignalState.setStates() * The default timing plan is 4sec yewllow, 20sec red, 20sec green */ inline void addTrafficLight(std::shared_ptr cmw, lanelet::Id light_id, lanelet::Id owning_lanelet_id, std::vector controlled_lanelet_ids, -std::vector> timing_plan = +std::vector> timing_plan = { - std::make_pair(lanelet::time::timeFromSec(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green - std::make_pair(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE), // 4 sec yellow - std::make_pair(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN), // 20 sec red - std::make_pair(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green + std::make_pair(lanelet::time::timeFromSec(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED), // Just ended green + std::make_pair(lanelet::time::timeFromSec(4.0), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE), // 4 sec yellow + std::make_pair(lanelet::time::timeFromSec(24.0), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN), // 20 sec red + std::make_pair(lanelet::time::timeFromSec(44.0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) // 20 sec green }) { std::vector controlled_lanelets; @@ -524,7 +524,7 @@ std::vector lanelet::LineString3d virtual_stop_line(lanelet::utils::getId(), { owning_lanelet.leftBound().back(), owning_lanelet.rightBound().back() }); // Build traffic light - std::shared_ptr traffic_light(new lanelet::CarmaTrafficLight(lanelet::CarmaTrafficLight::buildData(light_id, { virtual_stop_line }, controlled_lanelets ))); + std::shared_ptr traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(light_id, { virtual_stop_line }, {controlled_lanelets.front()}, {controlled_lanelets.back()} ))); // Set the timing plan traffic_light->setStates(timing_plan,0); diff --git a/carma_wm/include/carma_wm/WorldModel.h b/carma_wm/include/carma_wm/WorldModel.h index 591f43cae0..0c8f089bca 100644 --- a/carma_wm/include/carma_wm/WorldModel.h +++ b/carma_wm/include/carma_wm/WorldModel.h @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include "TrackPos.h" @@ -366,7 +366,7 @@ class WorldModel * * \return list of traffic lights along the current route */ - virtual std::vector getLightsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0; + virtual std::vector getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const = 0; /** * \brief Return a list of all way stop intersections along the current route. diff --git a/carma_wm/include/carma_wm/WorldModelUtils.h b/carma_wm/include/carma_wm/WorldModelUtils.h index 0b61a802ce..2a757e8c49 100644 --- a/carma_wm/include/carma_wm/WorldModelUtils.h +++ b/carma_wm/include/carma_wm/WorldModelUtils.h @@ -30,6 +30,10 @@ #include #include #include +#include +#include +#include + namespace carma_wm { @@ -103,6 +107,30 @@ std::vector nonConnectedAdjacentLeft(const lanelet::Lanel std::vector nonConnectedAdjacentLeft(const lanelet::LaneletMapPtr& semantic_map, const lanelet::BasicPoint2d& input_point, const unsigned int n = 10); + +/*! + * \brief Gets the affected lanelet or areas based on the points in the given map's frame + * \param geofence_msg lanelet::Points3d in local frame + * \param lanelet_map Lanelet Map Ptr + * \param routing_graph Routing graph of the lanelet map + * \param max_lane_width max lane width of the lanes in the map + * + * NOTE:Currently this function only checks lanelets and will be expanded to areas in the future. + */ +lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& gf_pts, const lanelet::LaneletMapPtr& lanelet_map, std::shared_ptr routing_graph, double max_lane_width); + +/*! + * \brief A function that filters successor lanelets of root_lanelets from possible_lanelets + * \param possible_lanelets all possible lanelets to check + * \param root_lanelets lanelets to filter from + * \param lanelet_map Lanelet Map Ptr + * \param routing_graph Routing graph of the lanelet map + * + * NOTE:Mainly used as a helper function for getAffectedLaneletOrAreas + */ +std::unordered_set filterSuccessorLanelets(const std::unordered_set& possible_lanelets, const std::unordered_set& root_lanelets, + const lanelet::LaneletMapPtr& lanelet_map, std::shared_ptr routing_graph); + } // namespace query namespace utils diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 37fb3f80e9..255d6c7b3c 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -1136,7 +1136,7 @@ std::vector CARMAWorldModel::nonConnectedAdjacentLeft(con return carma_wm::query::getLaneletsFromPoint(getMap(), input_point, n); } -std::vector CARMAWorldModel::getLightsAlongRoute(const lanelet::BasicPoint2d& loc) const +std::vector CARMAWorldModel::getSignalsAlongRoute(const lanelet::BasicPoint2d& loc) const { // Check if the map is loaded yet if (!semantic_map_ || semantic_map_->laneletLayer.empty()) @@ -1150,12 +1150,12 @@ std::vector CARMAWorldModel::getLightsAlongRoute( ROS_ERROR_STREAM("Route has not yet been loaded"); return {}; } - std::vector light_list; + 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(); + auto lights = semantic_map_->laneletLayer.get(ll.id()).regulatoryElementsAs(); if (lights.empty()) { continue; @@ -1244,7 +1244,7 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) ", and signal_group_id: " << (int)current_movement_state.signal_group); continue; } - auto curr_light_list = lanelets_general[0].regulatoryElementsAs(); + auto curr_light_list = lanelets_general[0].regulatoryElementsAs(); if (curr_light_list.empty()) { @@ -1252,7 +1252,7 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) ", and signal_group_id: " << (int)current_movement_state.signal_group); continue; } - lanelet::CarmaTrafficLightPtr curr_light = curr_light_list[0]; + lanelet::CarmaTrafficSignalPtr curr_light = curr_light_list[0]; // reset states if the intersection's geometry changed if (curr_light->revision_ != curr_intersection.revision) @@ -1307,7 +1307,7 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) //if same data as last time: //where state is same and timestamp is equal or less, skip if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() > 0 && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second == - static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first >= min_end_time) { continue; @@ -1316,17 +1316,17 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) // if received same state as last time, but with updated time, just update the time of last state // skip setting state until received a new state that is different from last recorded one if (traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].size() > 0 && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second == - static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) && traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first < min_end_time) { - ROS_DEBUG_STREAM("Updated time for state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << ", with time: " + ROS_DEBUG_STREAM("Updated time for state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << ", with time: " << std::to_string(lanelet::time::toSec(min_end_time))); traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first = min_end_time; continue; } // detected that new state received; therefore, set the last recorded state (not new one received) - ROS_DEBUG_STREAM("Received new state for light: " << curr_light_id << ", with state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << + ROS_DEBUG_STREAM("Received new state for light: " << curr_light_id << ", with state: " << static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state) << ", time: " << ros::Time::fromBoost(min_end_time)); @@ -1341,12 +1341,12 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) } else if (curr_light->recorded_time_stamps.empty()) // if it was never initialized, do its best to plan with the current state until the future state is also received. { - std::vector> default_state; + std::vector> default_state; // green 20sec, yellow 3sec, red 20sec, back to green 20sec etc... - default_state.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(RED_LIGHT_DURATION), lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); - default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); + default_state.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); + default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(YELLOW_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); + default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(RED_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); + default_state.push_back(std::make_pair(default_state.back().first + lanelet::time::durationFromSec(GREEN_LIGHT_DURATION), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); curr_light->setStates(default_state, curr_intersection.revision); ROS_DEBUG_STREAM("Set default cycle of total seconds: " << lanelet::time::toSec(curr_light->fixed_cycle_duration)); @@ -1357,7 +1357,7 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) auto yellow_light_duration = lanelet::time::durationFromSec(YELLOW_LIGHT_DURATION); auto red_light_duration = lanelet::time::durationFromSec(RED_LIGHT_DURATION); - std::vector> partial_states; + std::vector> partial_states; // set the partial cycle. ROS_DEBUG_STREAM("Setting last recorded state for light: " << curr_light_id << ", with state: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().second << ", time: " << traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].back().first); @@ -1365,20 +1365,20 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) { auto light_state = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].second; - if (light_state == lanelet::CarmaTrafficLightState::STOP_AND_REMAIN || light_state == lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED) + if (light_state == lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN || light_state == lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED) red_light_duration = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; - else if (light_state == lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED || light_state == lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) + else if (light_state == lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED || light_state == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) green_light_duration = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; - else if (light_state == lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE || light_state == lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE) + else if (light_state == lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE || light_state == lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE) yellow_light_duration = traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].first - traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group][i].first; } - partial_states.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); - partial_states.push_back(std::make_pair(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); - partial_states.push_back(std::make_pair(partial_states.back().first + red_light_duration, lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); - partial_states.push_back(std::make_pair(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); + partial_states.push_back(std::make_pair(boost::posix_time::from_time_t(0), lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); + partial_states.push_back(std::make_pair(partial_states.back().first + yellow_light_duration, lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); + partial_states.push_back(std::make_pair(partial_states.back().first + red_light_duration, lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); + partial_states.push_back(std::make_pair(partial_states.back().first + green_light_duration, lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); curr_light->setStates(partial_states, curr_intersection.revision); ROS_DEBUG_STREAM("Set new partial cycle of total seconds: " << lanelet::time::toSec(curr_light->fixed_cycle_duration)); @@ -1387,7 +1387,7 @@ void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT& spat_msg) // record the new state received traffic_light_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time, - static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state))); + static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state))); } } } diff --git a/carma_wm/src/SignalizedIntersectionManager.cpp b/carma_wm/src/SignalizedIntersectionManager.cpp new file mode 100644 index 0000000000..f9a68d9540 --- /dev/null +++ b/carma_wm/src/SignalizedIntersectionManager.cpp @@ -0,0 +1,340 @@ +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include + +namespace carma_wm +{ + void SignalizedIntersectionManager::setTargetFrame(const std::string& target_frame) + { + target_frame_ = target_frame; + } + + void SignalizedIntersectionManager::setMaxLaneWidth(double max_lane_width) + { + max_lane_width_ = max_lane_width; + } + + void SignalizedIntersectionManager::convertLaneToLaneletId(std::unordered_map& entry, std::unordered_map& exit, const cav_msgs::IntersectionGeometry& intersection, + const std::shared_ptr& map, std::shared_ptr current_routing_graph) + { + std::unordered_map> signal_group_to_exit_lanes; + std::unordered_map> signal_group_to_entry_lanes; + + if (target_frame_ == "") + { + throw std::invalid_argument("Map is not initialized yet as the georeference was not found..."); + } + + lanelet::projection::LocalFrameProjector local_projector(target_frame_.c_str()); + + lanelet::GPSPoint gps_point; + gps_point.lat = intersection.ref_point.latitude; + gps_point.lon = intersection.ref_point.longitude; + gps_point.ele = intersection.ref_point.elevation; + + auto ref_node = local_projector.forward(gps_point); + + ROS_DEBUG_STREAM("Reference node in map frame x: " << ref_node.x() << ", y: " << ref_node.y()); + + std::vector node_list; + + for (auto lane : intersection.lane_list) + { + double curr_x = ref_node.x(); + double curr_y = ref_node.y(); + + ROS_DEBUG_STREAM("Processing Lane id: " << (int)lane.lane_id); + + size_t min_number_of_points = 2; // only two points are sufficient to get corresponding lanelets + + if (lane.node_list.nodes.node_set_xy.size() < min_number_of_points) + { + ROS_WARN_STREAM("Not enough points are provided to match a lane. Skipping... "); + continue; + } + + for (size_t i = 0; i < min_number_of_points; i ++ ) + { + curr_x = lane.node_list.nodes.node_set_xy[i].delta.x + curr_x; + curr_y = lane.node_list.nodes.node_set_xy[i].delta.y + curr_y; + lanelet::Point3d curr_node{map->pointLayer.uniqueId(), curr_x, curr_y, 0}; + + ROS_DEBUG_STREAM("Current node x: " << curr_x << ", y: " << curr_y); + + node_list.push_back(curr_node); + } + + for (auto node : node_list) + { + ROS_ERROR_STREAM("x: " << node.x() << ", y: " << node.y()); + } + + // save which signal group connect to which exit lanes + for (auto connection : lane.connect_to_list) + { + signal_group_to_exit_lanes[connection.signal_group].emplace(connection.connecting_lane.lane); + + if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS) + signal_group_to_entry_lanes[connection.signal_group].emplace(lane.lane_id); + } + + // query corresponding lanelet lane from local map + auto affected_llts = carma_wm::query::getAffectedLaneletOrAreas(node_list, map, current_routing_graph, max_lane_width_); + + if (affected_llts.empty()) + { + throw std::invalid_argument("Given offset points are not inside the map..."); + } + + lanelet::Id corresponding_lanelet_id = affected_llts.front().id(); + + ROS_DEBUG_STREAM("Found existing lanelet id: " << corresponding_lanelet_id); + + if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS) + { + ROS_DEBUG_STREAM("Detected INGRESS, " << (int)lane.lane_id); + entry[lane.lane_id] = corresponding_lanelet_id; + } + else if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS) + { + ROS_DEBUG_STREAM("Detected EGRESS, " << (int)lane.lane_id); + exit[lane.lane_id] = corresponding_lanelet_id; + } + // ignoring types that are neither ingress nor egress + + node_list = {}; + } + + // convert and save exit lane ids into lanelet ids with their corresponding signal group ids + for (auto iter = signal_group_to_exit_lanes.begin(); iter != signal_group_to_exit_lanes.end(); iter++) + { + for (auto exit_lane : iter->second) + { + if (exit.find(exit_lane) != exit.end()) + { + ROS_DEBUG_STREAM("Adding exit_lane id: " << exit_lane); + signal_group_to_exit_lanelet_ids_[iter->first].insert(exit[exit_lane]); + } + else + { + throw std::invalid_argument(std::string("Unable to convert exit lane Id: " + std::to_string((int)exit_lane) + ", to lanelet id using the given MAP.msg!").c_str()); + } + } + } + + // convert and save entry lane ids into lanelet ids with their corresponding signal group ids + for (auto iter = signal_group_to_entry_lanes.begin(); iter != signal_group_to_entry_lanes.end(); iter++) + { + for (auto entry_lane : iter->second) + { + ROS_DEBUG_STREAM("Adding entry_lane id: " << entry_lane); + if (entry.find(entry_lane) != entry.end()) + { + signal_group_to_entry_lanelet_ids_[iter->first].insert(entry[entry_lane]); + } + else + { + throw std::invalid_argument(std::string("Unable to convert entry lane Id: " + std::to_string((int)entry_lane) + ", to lanelet id using the given MAP.msg!").c_str()); + } + } + } + } + + lanelet::Id SignalizedIntersectionManager::matchSignalizedIntersection(const lanelet::Lanelets& entry_llts, const lanelet::Lanelets& exit_llts, const std::shared_ptr& map) + { + lanelet::Id matching_id = lanelet::InvalId; + + std::vector existing_si; + + for (auto llt : entry_llts) + { + auto intersections = llt.regulatoryElementsAs(); + + if (intersections.empty()) + { + // no match if any of the entry lanelet is not part of any intersection. + break; + } + existing_si.insert(existing_si.end(), intersections.begin(),intersections.end()); + } + + for (auto intersection : existing_si) + { + auto existing_entry_llts = intersection->getEntryLanelets(); + auto existing_exit_llts = intersection->getExitLanelets(); + + if (existing_exit_llts.size() != exit_llts.size() || existing_entry_llts.size() != entry_llts.size()) + continue; + + bool is_different = false; + + for (auto llt: existing_entry_llts) + { + if (std::find(entry_llts.begin(), entry_llts.end(), llt) == entry_llts.end()) + { + is_different = true; + + break; + } + } + + for (auto llt: existing_exit_llts) + { + if (std::find(exit_llts.begin(), exit_llts.end(), llt) == exit_llts.end()) + { + is_different = true; + + break; + } + } + + if (!is_different) + { + // found a match + matching_id = intersection->id(); + + break; + } + + } + + return matching_id; + } + + std::shared_ptr SignalizedIntersectionManager::createTrafficSignalUsingSGID(uint8_t signal_group_id, const lanelet::Lanelets& entry_lanelets, const lanelet::Lanelets& exit_lanelets) + { + std::vector stop_lines; + for (auto llt : entry_lanelets) + { + std::vector points; + points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.leftBound2d().back().x(), llt.leftBound2d().back().y(), 0)); + points.push_back(lanelet::Point3d(lanelet::utils::getId(), llt.rightBound().back().x(), llt.rightBound().back().y(), 0)); + lanelet::LineString3d stop_line(lanelet::utils::getId(), points); + stop_lines.push_back(stop_line); + } + + lanelet::Id traffic_light_id = lanelet::utils::getId(); + std::shared_ptr traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, stop_lines, entry_lanelets, exit_lanelets))); + signal_group_to_traffic_light_id_[signal_group_id] = traffic_light_id; + + for (auto llt : exit_lanelets) + { + signal_group_to_exit_lanelet_ids_[signal_group_id].insert(llt.id()); + } + for (auto llt : entry_lanelets) + { + signal_group_to_entry_lanelet_ids_[signal_group_id].insert(llt.id()); + } + return traffic_light; + } + + void SignalizedIntersectionManager::createIntersectionFromMapMsg(std::vector& sig_intersections, std::vector& traffic_signals, const cav_msgs::MapData& map_msg, + const std::shared_ptr& map, std::shared_ptr routing_graph) + { + + for (auto const& intersection : map_msg.intersections) + { + std::unordered_map entry; + std::unordered_map exit; + + convertLaneToLaneletId(entry, exit, intersection, map, routing_graph); + + std::vector entry_llts; + std::vector exit_llts; + + for (auto iter = entry.begin(); iter != entry.end(); iter++) + { + entry_llts.push_back(map->laneletLayer.get(iter->second)); + } + for (auto iter = exit.begin(); iter != exit.end(); iter++) + { + exit_llts.push_back(map->laneletLayer.get(iter->second)); + } + + lanelet::Id intersection_id = matchSignalizedIntersection(entry_llts, exit_llts, map); + + if (intersection_id == lanelet::InvalId) + { + ROS_DEBUG_STREAM("No existing intersection found. Creating a new one..."); + intersection_id = lanelet::utils::getId(); + + std::vector interior_llts = identifyInteriorLanelets(entry_llts, map); + + std::shared_ptr sig_inter(new lanelet::SignalizedIntersection + (lanelet::SignalizedIntersection::buildData(intersection_id, entry_llts, exit_llts, interior_llts))); + intersection_id_to_regem_id_[intersection.id.id] = intersection_id; + sig_intersections.push_back(sig_inter); + } + } + + // create signal group for each from the message + // check if it already exists + for (auto sig_grp_pair : signal_group_to_exit_lanelet_ids_) + { + ROS_DEBUG_STREAM("Creating signal for: " << (int)sig_grp_pair.first); + // ignore the traffic signals already inside + if (signal_group_to_traffic_light_id_.find(sig_grp_pair.first) != signal_group_to_traffic_light_id_.end() && + map->regulatoryElementLayer.exists(signal_group_to_traffic_light_id_[sig_grp_pair.first])) + { + continue; + } + + std::vector exit_lanelets; + for (auto iter = sig_grp_pair.second.begin(); iter != sig_grp_pair.second.end(); iter++) + { + exit_lanelets.push_back(map->laneletLayer.get(*iter)); + } + std::vector entry_lanelets; + for (auto iter = signal_group_to_entry_lanelet_ids_[sig_grp_pair.first].begin(); iter != signal_group_to_entry_lanelet_ids_[sig_grp_pair.first].end(); iter++) + { + entry_lanelets.push_back(map->laneletLayer.get(*iter)); + } + + traffic_signals.push_back(createTrafficSignalUsingSGID(sig_grp_pair.first, entry_lanelets, exit_lanelets)); + } + } + + lanelet::Lanelets SignalizedIntersectionManager::identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr& map) + { + lanelet::BasicLineString2d polygon_corners; + + if (entry_llts.size() < 2) //at least two lanes (1 ingress and 1 egress) needed to form intersection + { + return {}; + } + + for (auto llt : entry_llts) + { + lanelet::BasicPoint2d pt(llt.centerline2d().back().x(), llt.centerline2d().back().y()); + polygon_corners.push_back(pt); + } + lanelet::BasicPolygon2d polygon(polygon_corners); + auto interior_llt_pairs = lanelet::geometry::findWithin2d(map->laneletLayer, polygon); + lanelet::Lanelets interior_llts; + + for (auto pair : interior_llt_pairs) + { + if (std::find(entry_llts.begin(),entry_llts.end(), pair.second) == entry_llts.end()) + interior_llts.push_back(pair.second); + } + return interior_llts; + } + + +} // namespace carma_wm diff --git a/carma_wm/src/WMListenerWorker.cpp b/carma_wm/src/WMListenerWorker.cpp index da55c974f9..0984fe6627 100644 --- a/carma_wm/src/WMListenerWorker.cpp +++ b/carma_wm/src/WMListenerWorker.cpp @@ -17,12 +17,13 @@ #include #include #include -#include +#include #include "WMListenerWorker.h" namespace carma_wm { -enum class GeofenceType{ INVALID, DIGITAL_SPEED_LIMIT, PASSING_CONTROL_LINE, REGION_ACCESS_RULE, DIGITAL_MINIMUM_GAP, DIRECTION_OF_TRAVEL, STOP_RULE, CARMA_TRAFFIC_LIGHT/* ... others */ }; +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) { @@ -32,7 +33,8 @@ GeofenceType resolveGeofenceType(const std::string& rule_name) if (rule_name.compare(lanelet::DigitalMinimumGap::RuleName) == 0) return GeofenceType::DIGITAL_MINIMUM_GAP; if (rule_name.compare(lanelet::DirectionOfTravel::RuleName) == 0) return GeofenceType::DIRECTION_OF_TRAVEL; if (rule_name.compare(lanelet::StopRule::RuleName) == 0) return GeofenceType::STOP_RULE; - if (rule_name.compare(lanelet::CarmaTrafficLight::RuleName) == 0) return GeofenceType::CARMA_TRAFFIC_LIGHT; + if (rule_name.compare(lanelet::CarmaTrafficSignal::RuleName) == 0) return GeofenceType::CARMA_TRAFFIC_LIGHT; + if (rule_name.compare(lanelet::SignalizedIntersection::RuleName) == 0) return GeofenceType::SIGNALIZED_INTERSECTION; return GeofenceType::INVALID; } @@ -194,6 +196,10 @@ void WMListenerWorker::mapUpdateCallback(const autoware_lanelet2_msgs::MapBinPtr world_model_->setTrafficLightIds(pair.first, pair.second); } + ROS_DEBUG_STREAM("Geofence id" << gf_ptr->id_ << " sends record of intersections size: " << gf_ptr->sim_.intersection_id_to_regem_id_.size()); + if (gf_ptr->sim_.intersection_id_to_regem_id_.size() > 0) + world_model_->sim_ = gf_ptr->sim_; + ROS_DEBUG_STREAM("Geofence id" << gf_ptr->id_ << " requests removal of size: " << gf_ptr->remove_list_.size()); for (auto pair : gf_ptr->remove_list_) { @@ -326,7 +332,7 @@ void WMListenerWorker::newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet case GeofenceType::CARMA_TRAFFIC_LIGHT: { - lanelet::CarmaTrafficLightPtr ctl = std::dynamic_pointer_cast(factory_pcl); + lanelet::CarmaTrafficSignalPtr ctl = std::dynamic_pointer_cast(factory_pcl); world_model_->getMutableMap()->update(parent_llt, ctl); break; diff --git a/carma_wm/src/WMListenerWorker.h b/carma_wm/src/WMListenerWorker.h index f04aaf5b04..842455faa8 100644 --- a/carma_wm/src/WMListenerWorker.h +++ b/carma_wm/src/WMListenerWorker.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace carma_wm { @@ -141,5 +142,6 @@ class WMListenerWorker bool rerouting_flag_=false; //indicates whether if route node is in middle of rerouting bool route_node_flag_=false; //indicates whether if this node is route node long most_recent_update_msg_seq_ = -1; // Tracks the current sequence number for map update messages. Dropping even a single message would invalidate the map + }; } // namespace carma_wm diff --git a/carma_wm/src/WorldModelUtils.cpp b/carma_wm/src/WorldModelUtils.cpp index 8437ede24d..09eecd983d 100644 --- a/carma_wm/src/WorldModelUtils.cpp +++ b/carma_wm/src/WorldModelUtils.cpp @@ -112,6 +112,167 @@ std::vector nonConnectedAdjacentLeft(const lanelet::Lanel return opposite_lanelets; } + + +lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& gf_pts, const lanelet::LaneletMapPtr& lanelet_map, std::shared_ptr routing_graph, double max_lane_width) +{ + // Logic to detect which part is affected + ROS_DEBUG_STREAM("Get affected lanelets loop"); + std::unordered_set affected_lanelets; + for (size_t idx = 0; idx < gf_pts.size(); idx ++) + { + ROS_DEBUG_STREAM("Index: " << idx << " Point: " << gf_pts[idx].x() << ", " << gf_pts[idx].y()); + std::unordered_set possible_lanelets; + + // This loop identifes the lanelets which this point lies within that could be impacted by the geofence + // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. + // The reason findNearest is used instead of nearestUntil is because that method orders results by bounding box which + // can give invalid sequences when dealing with large curved lanelets. + bool continue_search = true; + size_t nearest_count = 0; + while (continue_search) { + + nearest_count += 10; // Increase the index search radius by 10 each loop until all nearby lanelets are found + + for (const auto& ll_pair : lanelet::geometry::findNearest(lanelet_map->laneletLayer, gf_pts[idx].basicPoint2d(), nearest_count)) { // Get the nearest lanelets and iterate over them + auto ll = std::get<1>(ll_pair); + + if (possible_lanelets.find(ll) != possible_lanelets.end()) { // Skip if already found + continue; + } + + double dist = std::get<0>(ll_pair); + ROS_DEBUG_STREAM("Distance to lanelet " << ll.id() << ": " << dist << " max_lane_width: " << max_lane_width); + + if (dist > max_lane_width) { // Only save values closer than max_lane_width. Since we are iterating in distance order when we reach this distance the search can stop + continue_search = false; + break; + } + + // Check if the point is inside this lanelet + if(dist == 0.0) { // boost geometry uses a distance of 0 to indicate a point is within a polygon + possible_lanelets.insert(ll); + } + + } + + if (nearest_count >= lanelet_map->laneletLayer.size()) { // if we are out of lanelets to evaluate then end the search + continue_search = false; + } + } + + // among these llts, filter the ones that are on same direction as the geofence using routing + if (idx + 1 == gf_pts.size()) // we only check this for the last gf_pt after saving everything + { + ROS_DEBUG_STREAM("Last point"); + std::unordered_set filtered = filterSuccessorLanelets(possible_lanelets, affected_lanelets, lanelet_map, routing_graph); + ROS_DEBUG_STREAM("Got successor lanelets of size: " << filtered.size()); + affected_lanelets.insert(filtered.begin(), filtered.end()); + break; + } + + ROS_DEBUG_STREAM("Checking possible lanelets"); + // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts + for (auto llt: possible_lanelets) + { + ROS_DEBUG_STREAM("Evaluating lanelet: " << llt.id()); + lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()}); + lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()}); + + // record the llts that are on the same dir + if (boost::geometry::intersects(llt_boundary, gf_dir_line)) + { + ROS_DEBUG_STREAM("Overlaps end line"); + affected_lanelets.insert(llt); + } + // check condition if two geofence points are in one lanelet then check matching direction and record it also + else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && + affected_lanelets.find(llt) == affected_lanelets.end()) + { + ROS_DEBUG_STREAM("Within new lanelet"); + lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , + ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2}); + // turn into vectors + Eigen::Vector2d vec_to_median(median); + Eigen::Vector2d vec_to_gf_start(gf_pts[idx].basicPoint2d()); + Eigen::Vector2d vec_to_gf_end(gf_pts[idx + 1].basicPoint2d()); + + // Get vector from start to external point + Eigen::Vector2d start_to_median = vec_to_median - vec_to_gf_start; + + // Get vector from start to end point + Eigen::Vector2d start_to_end = vec_to_gf_end - vec_to_gf_start; + + // Get angle between both vectors + double interior_angle = carma_wm::geometry::getAngleBetweenVectors(start_to_median, start_to_end); + + ROS_DEBUG_STREAM("vec_to_median: " << vec_to_median.x() << ", " << vec_to_median.y()); + ROS_DEBUG_STREAM("vec_to_gf_start: " << vec_to_gf_start.x() << ", " << vec_to_gf_start.y()); + ROS_DEBUG_STREAM("vec_to_gf_end: " << vec_to_gf_end.x() << ", " << vec_to_gf_end.y()); + ROS_DEBUG_STREAM("start_to_median: " << start_to_median.x() << ", " << start_to_median.y()); + ROS_DEBUG_STREAM("start_to_end: " << start_to_end.x() << ", " << start_to_end.y()); + ROS_DEBUG_STREAM("interior_angle: " << interior_angle); + // Save the lanelet if the direction of two points inside aligns with that of the lanelet + + if (interior_angle < M_PI_2 && interior_angle >= 0) affected_lanelets.insert(llt); + } + else + { + ROS_DEBUG_STREAM("------ Did not record anything..."); + } + + } + + } + + ROS_DEBUG_STREAM("affected_lanelets size: " << affected_lanelets.size()); + // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object + // by implementing non-const version of that LaneletOrArea + lanelet::ConstLaneletOrAreas affected_parts; + // return results in ascending downtrack order from the first point of geofence + std::vector> sorted_parts; + for (auto llt : affected_lanelets) + { + sorted_parts.push_back(std::make_pair(carma_wm::geometry::trackPos(llt, gf_pts.front().basicPoint2d()).downtrack, llt)); + } + std::sort(sorted_parts.begin(), sorted_parts.end(), [](const auto& x, const auto& y){return x.first > y.first;}); + + for (auto pair : sorted_parts) + { + affected_parts.push_back(pair.second); + } + + return affected_parts; +} + +// helper function that filters successor lanelets of root_lanelets from possible_lanelets +std::unordered_set filterSuccessorLanelets(const std::unordered_set& possible_lanelets, const std::unordered_set& root_lanelets, + const lanelet::LaneletMapPtr& lanelet_map, std::shared_ptr routing_graph) +{ + if (!routing_graph) { + throw std::invalid_argument("No routing graph available"); + } + + std::unordered_set filtered_lanelets; + // we utilize routes to filter llts that are overlapping but not connected + // as this is the last lanelet + // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts + for (auto recorded_llt: root_lanelets) + { + for (auto following_llt: routing_graph->following(recorded_llt, false)) + { + auto mutable_llt = lanelet_map->laneletLayer.get(following_llt.id()); + auto it = possible_lanelets.find(mutable_llt); + if (it != possible_lanelets.end()) + { + filtered_lanelets.insert(mutable_llt); + } + } + } + return filtered_lanelets; +} + + } // namespace query namespace utils diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index 800949b6ef..17f3454e9e 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -1312,7 +1312,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) lanelet::Id traffic_light_id = lanelet::utils::getId(); lanelet::LineString3d virtual_stop_line(lanelet::utils::getId(), {pl2, pr2}); // Creat passing control line for solid dashed line - std::shared_ptr traffic_light(new lanelet::CarmaTrafficLight(lanelet::CarmaTrafficLight::buildData(traffic_light_id, { virtual_stop_line }, { ll_1 }))); + std::shared_ptr traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, { virtual_stop_line }, { ll_1 }, { ll_1 }))); traffic_light->revision_ = 0; ll_1.addRegulatoryElement(traffic_light); auto map = lanelet::utils::createMap({ ll_1 }, {}); @@ -1336,7 +1336,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) state.movement_list.push_back(movement); spat.intersection_state_list.push_back(state); cmw.processSpatFromMsg(spat); - auto lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + auto lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // default EXPECT_EQ(lanelet::time::durationFromSec(43), lights1[0]->fixed_cycle_duration); @@ -1347,7 +1347,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) state.movement_list[0] = movement; spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // nothing changed EXPECT_EQ(lanelet::time::durationFromSec(43), lights1[0]->fixed_cycle_duration); @@ -1358,7 +1358,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) state.movement_list[0] = movement; spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // partial state 7 EXPECT_EQ(lanelet::time::durationFromSec(44.0), lights1[0]->fixed_cycle_duration); @@ -1369,7 +1369,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) state.movement_list[0] = movement; spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // partial state 7 EXPECT_EQ(lanelet::time::durationFromSec(44.0), lights1[0]->fixed_cycle_duration); @@ -1380,7 +1380,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) state.movement_list[0] = movement; spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // partial state 3 EXPECT_EQ(lanelet::time::durationFromSec(45), lights1[0]->fixed_cycle_duration); @@ -1392,7 +1392,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc EXPECT_EQ(lanelet::time::durationFromSec(46), lights1[0]->fixed_cycle_duration); @@ -1405,12 +1405,12 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) spat.intersection_state_list[0] = state; cmw.processSpatFromMsg(spat); - lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // and query the regem again to check if its entries are updated, by checking revision or getState or predictState etc EXPECT_EQ(lanelet::time::durationFromSec(46), lights1[0]->fixed_cycle_duration); } -TEST(CARMAWorldModelTest, getLightsAlongRoute) +TEST(CARMAWorldModelTest, getSignalsAlongRoute) { carma_wm::CARMAWorldModel cmw; lanelet::LaneletMapPtr map; @@ -1426,15 +1426,15 @@ TEST(CARMAWorldModelTest, getLightsAlongRoute) lanelet::Id traffic_light_id1 = lanelet::utils::getId(); lanelet::Id traffic_light_id2 = lanelet::utils::getId(); lanelet::LineString3d virtual_stop_line1(lanelet::utils::getId(), {pl2, pr2}); - std::shared_ptr traffic_light1(new lanelet::CarmaTrafficLight(lanelet::CarmaTrafficLight::buildData(traffic_light_id1, { virtual_stop_line1 }, { cmw_ptr->getMutableMap()->laneletLayer.get(1200) }))); + std::shared_ptr traffic_light1(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id1, { virtual_stop_line1 }, { cmw_ptr->getMutableMap()->laneletLayer.get(1200) }, { cmw_ptr->getMutableMap()->laneletLayer.get(1200) }))); lanelet::LineString3d virtual_stop_line2(lanelet::utils::getId(), {pl3, pr3}); - std::shared_ptr traffic_light2(new lanelet::CarmaTrafficLight(lanelet::CarmaTrafficLight::buildData(traffic_light_id2, { virtual_stop_line2 }, { cmw_ptr->getMutableMap()->laneletLayer.get(1201) }))); + std::shared_ptr traffic_light2(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id2, { virtual_stop_line2 }, { cmw_ptr->getMutableMap()->laneletLayer.get(1201) }, { cmw_ptr->getMutableMap()->laneletLayer.get(1201) }))); cmw_ptr->getMutableMap()->update(cmw_ptr->getMutableMap()->laneletLayer.get(1200), traffic_light1); cmw_ptr->getMutableMap()->update(cmw_ptr->getMutableMap()->laneletLayer.get(1201), traffic_light2); carma_wm::test::setRouteByIds({ 1200, 1201, 1202}, cmw_ptr); - auto lights = cmw_ptr->getLightsAlongRoute({0.5, 0}); + auto lights = cmw_ptr->getSignalsAlongRoute({0.5, 0}); EXPECT_EQ(lights.size(), 2); EXPECT_EQ(lights[0]->id(), traffic_light_id1); diff --git a/carma_wm/test/SignalizedIntersectionManagerTest.cpp b/carma_wm/test/SignalizedIntersectionManagerTest.cpp new file mode 100644 index 0000000000..7698842322 --- /dev/null +++ b/carma_wm/test/SignalizedIntersectionManagerTest.cpp @@ -0,0 +1,291 @@ +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "TestHelpers.h" + +#include +#include + +using ::testing::_; +using ::testing::A; +using ::testing::DoAll; +using ::testing::InSequence; +using ::testing::Return; +using ::testing::ReturnArg; + +namespace carma_wm + +{ + +TEST(SignalizedIntersectionManger, convertLaneToLaneletId) +{ + + /* |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + carma_wm::test::MapOptions options; + options.lane_length_ = 25; + options.lane_width_ = 5; + auto cwm = carma_wm::test::getGuidanceTestMap(options); + auto lanelet_map = cwm->getMutableMap(); + + carma_wm::SignalizedIntersectionManager sim; + std::string georeference = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + sim.setTargetFrame(georeference); + + std::unordered_map entry; + std::unordered_map exit; + + cav_msgs::IntersectionGeometry intersection; //ref_point lat, lon, el = 0; + intersection.id.id = 9001; + + cav_msgs::GenericLane lane; + lane.lane_id = 1210; + lane.lane_attributes.directional_use.lane_direction = 1u; //ingress + j2735_msgs::Connection connection; + connection.signal_group = 1; + connection.connecting_lane.lane = 1211; + + lane.connect_to_list.push_back(connection); + + cav_msgs::NodeXY node; + node.delta.x = 7.5; + node.delta.y = 12.5; + + lane.node_list.nodes.node_set_xy.push_back(node); + + node.delta.x = 0.0; //offset from previous + node.delta.y = 0.5; + + lane.node_list.nodes.node_set_xy.push_back(node); + + intersection.lane_list.push_back(lane); + + lane.lane_id = 1211; + lane.lane_attributes.directional_use.lane_direction = 2u; // egress imagining intersection + // entering 1210 from left and out through 1220 + lane.node_list = {}; + lane.connect_to_list = {}; + + node.delta.x = 7.5; + node.delta.y = 37.5; + + lane.node_list.nodes.node_set_xy.push_back(node); + + node.delta.x = 0.0; //offset from previous + node.delta.y = 50.0; + + lane.node_list.nodes.node_set_xy.push_back(node); + intersection.lane_list.push_back(lane); + + sim.convertLaneToLaneletId(entry, exit, intersection, lanelet_map, cwm->getMapRoutingGraph()); + + EXPECT_EQ(sim.signal_group_to_entry_lanelet_ids_.size(), 1); + EXPECT_EQ(sim.signal_group_to_exit_lanelet_ids_.size(), 1); + + EXPECT_EQ(sim.signal_group_to_entry_lanelet_ids_[1].size(), 1); + EXPECT_EQ(sim.signal_group_to_exit_lanelet_ids_[1].size(), 1); + EXPECT_EQ(entry.size(), 1); + EXPECT_EQ(exit.size(), 1); +} + +TEST(SignalizedIntersectionManger, createIntersectionFromMapMsg) +{ + /* |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + carma_wm::test::MapOptions options; + options.lane_length_ = 25; + options.lane_width_ = 5; + auto cwm = carma_wm::test::getGuidanceTestMap(options); + auto lanelet_map = cwm->getMutableMap(); + + carma_wm::SignalizedIntersectionManager sim; + std::string georeference = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + sim.setTargetFrame(georeference); + + cav_msgs::MapData map_msg; + cav_msgs::IntersectionGeometry intersection; //ref_point lat, lon, el = 0; + intersection.id.id = 9001; + + cav_msgs::GenericLane lane; + lane.lane_id = 1210; + lane.lane_attributes.directional_use.lane_direction = 1u; //ingress + j2735_msgs::Connection connection; + connection.signal_group = 1; + connection.connecting_lane.lane = 1220; + + lane.connect_to_list.push_back(connection); + + cav_msgs::NodeXY node; + node.delta.x = 5; + node.delta.y = 12.5; + + lane.node_list.nodes.node_set_xy.push_back(node); + + node.delta.x = 2.5; //offset from previous + node.delta.y = 0; + + lane.node_list.nodes.node_set_xy.push_back(node); + + intersection.lane_list.push_back(lane); + + lane.lane_id = 1220; + lane.lane_attributes.directional_use.lane_direction = 2u; // egress imagining intersection + // entering 1210 from left and out through 1220 + lane.node_list = {}; + lane.connect_to_list = {}; + + node.delta.x = 10; + node.delta.y = 12.5; + + lane.node_list.nodes.node_set_xy.push_back(node); + + node.delta.x = 2.5; //offset from previous + node.delta.y = 0; + + lane.node_list.nodes.node_set_xy.push_back(node); + intersection.lane_list.push_back(lane); + map_msg.intersections.push_back(intersection); + + std::vector> intersections; + std::vector> traffic_signals; + + sim.createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, lanelet_map, cwm->getMapRoutingGraph()); + + EXPECT_EQ(sim.signal_group_to_entry_lanelet_ids_.size(), 1); + EXPECT_EQ(sim.signal_group_to_exit_lanelet_ids_.size(), 1); + + EXPECT_EQ(*sim.signal_group_to_entry_lanelet_ids_[1].begin(), 1210); + EXPECT_EQ(*sim.signal_group_to_exit_lanelet_ids_[1].begin(), 1220); + EXPECT_EQ(sim.signal_group_to_traffic_light_id_[1], traffic_signals.front()->id()); + EXPECT_EQ(sim.intersection_id_to_regem_id_.size(), 1); + EXPECT_EQ(intersections.size(), 1); + EXPECT_EQ(traffic_signals.size(), 1); + +} + + +TEST(SignalizedIntersectionManger, matchSignalizedIntersection) +{ + /* |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + + auto lanelet_map = carma_wm::test::buildGuidanceTestMap(5.0, 25.0); + + carma_wm::SignalizedIntersectionManager sim; + std::string georeference = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + sim.setTargetFrame(georeference); + + lanelet::Id intersection_id = lanelet::utils::getId(); + auto intersection = std::make_shared(lanelet::SignalizedIntersection::buildData(intersection_id, + {lanelet_map->laneletLayer.get(1210)}, {lanelet_map->laneletLayer.get(1220)}, {})); + + lanelet_map->update({lanelet_map->laneletLayer.get(1210)}, intersection); + + lanelet::Id queried_id = sim.matchSignalizedIntersection({lanelet_map->laneletLayer.get(1210)}, {lanelet_map->laneletLayer.get(1220)}, lanelet_map); + + EXPECT_EQ(queried_id, intersection_id); + +} +TEST(SignalizedIntersectionManger, createTrafficSignalUsingSGID) +{ + /* |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + + auto lanelet_map = carma_wm::test::buildGuidanceTestMap(5.0, 25.0); + + carma_wm::SignalizedIntersectionManager sim; + std::string georeference = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + sim.setTargetFrame(georeference); + + auto signal = sim.createTrafficSignalUsingSGID(1, {lanelet_map->laneletLayer.get(1210)}, {lanelet_map->laneletLayer.get(1220)}); + + EXPECT_EQ(sim.signal_group_to_entry_lanelet_ids_.size(), 1); + EXPECT_EQ(sim.signal_group_to_exit_lanelet_ids_.size(), 1); + EXPECT_EQ(sim.signal_group_to_traffic_light_id_[1], signal->id()); +} +TEST(SignalizedIntersectionManger, identifyInteriorLanelets) +{ + /* |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + + auto lanelet_map = carma_wm::test::buildGuidanceTestMap(5.0, 25.0); + + carma_wm::SignalizedIntersectionManager sim; + std::string georeference = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + sim.setTargetFrame(georeference); + + auto interior = sim.identifyInteriorLanelets({lanelet_map->laneletLayer.get(1203),lanelet_map->laneletLayer.get(1211), lanelet_map->laneletLayer.get(1223)}, lanelet_map); + + EXPECT_EQ(interior.size(), 4); + + +} + + +} // namespace carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h index 0c522476fb..f0ff13041c 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h @@ -29,11 +29,18 @@ #include #include #include +#include namespace carma_wm_ctrl { using namespace lanelet::units::literals; + +// Map Update Geofence Common Labels +const std::string MAP_MSG_INTERSECTION = "MAP_MSG_INTERSECTION"; +const std::string MAP_MSG_TF_SIGNAL = "MAP_MSG_TF_SIGNAL"; + + /** * @brief An object representing a geofence use for communications with CARMA Cloud * @@ -76,8 +83,11 @@ specific type of regulatory element (such as digital speed limit, passing contro std::vector> prev_regems_; lanelet::ConstLaneletOrAreas affected_parts_; - // original message for this geofence + // original traffic control message for this geofence cav_msgs::TrafficControlMessageV01 msg_; + + // original MAP message for this geofence + cav_msgs::MapData map_msg_; // Helper member for PassingControlLine type regulatory geofence bool pcl_affects_left_ = false; diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h index 2556086215..33fe8abfd2 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h @@ -55,6 +55,8 @@ #include #include #include +#include +#include namespace carma_wm_ctrl @@ -108,6 +110,13 @@ class WMBroadcaster */ void geofenceCallback(const cav_msgs::TrafficControlMessage& geofence_msg); + /*! + * \brief Callback to MAP.msg which contains intersections' static info such geometry and lane ids + * + * \param map_msg The ROS msg of the MAP.msg to process + */ + void externalMapMsgCallback(const cav_msgs::MapData& map_msg); + /*! * \brief Adds a geofence to the current map and publishes the ROS msg */ @@ -193,6 +202,14 @@ class WMBroadcaster */ void geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& geofence_msg); + /*! + * \brief Fills geofence object from MAP Data ROS Msg which contains intersections' static data such as geometry and signal_group + * \param Geofence object to fill with information extracted from this msg + * \param geofence_msg The MAP ROS msg that contains intersection information + * \throw InvalidObjectStateError if base_map is not set or the base_map's georeference is empty + */ + std::vector> geofenceFromMapMsg(std::shared_ptr gf_ptr, const cav_msgs::MapData& map_msg); + /*! * \brief Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet * \param curr_pos Current position in local coordinates @@ -390,7 +407,7 @@ class WMBroadcaster lanelet::LaneletMapPtr base_map_; lanelet::LaneletMapPtr current_map_; - lanelet::routing::RoutingGraphUPtr current_routing_graph_; // Current map routing graph + lanelet::routing::RoutingGraphPtr current_routing_graph_; // Current map routing graph lanelet::Velocity config_limit; std::string participant_ = lanelet::Participants::VehicleCar;//Default participant type std::unordered_set checked_geofence_ids_; @@ -422,6 +439,7 @@ class WMBroadcaster size_t update_count_ = 0; // Records the total number of sent map updates. Used as the set value for update.header.seq + carma_wm::SignalizedIntersectionManager sim_; }; diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index bd476e21d1..1f7cc6348c 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -199,6 +199,42 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c } } + +std::vector> WMBroadcaster::geofenceFromMapMsg(std::shared_ptr gf_ptr, const cav_msgs::MapData& map_msg) +{ + std::vector> updates_to_send; + std::vector> intersections; + std::vector> traffic_signals; + + sim_.createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_); + + for (auto intersection : intersections) + { + auto update = std::make_shared(); + update->id_ = boost::uuids::random_generator()(); + update->label_ = carma_wm_ctrl::MAP_MSG_INTERSECTION; + update->regulatory_element_ = intersection; + for (auto llt : intersection->getEntryLanelets()) + { + update->affected_parts_.push_back(llt); + } + } + + for (auto signal : traffic_signals) + { + auto update = std::make_shared(); + update->id_ = boost::uuids::random_generator()(); + update->label_ = carma_wm_ctrl::MAP_MSG_TF_SIGNAL; + update->regulatory_element_ = signal; + for (auto llt : signal->getControlStartLanelets()) + { + update->affected_parts_.push_back(llt); + } + } + + return updates_to_send; +} + void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01) { bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos; @@ -413,7 +449,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m controlled_taper_right.push_back(back_llt_diag); - lanelet::CarmaTrafficLightPtr tfl_parallel = std::make_shared(lanelet::CarmaTrafficLight::buildData(lanelet::utils::getId(), {parallel_stop_ls}, controlled_taper_right)); + lanelet::CarmaTrafficSignalPtr tfl_parallel = std::make_shared(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {parallel_stop_ls}, {controlled_taper_right.front()}, {controlled_taper_right.back()})); gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()}); @@ -439,7 +475,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m controlled_open_right.push_back(current_map_->laneletLayer.get(llt.lanelet().get().id())); } - lanelet::CarmaTrafficLightPtr tfl_opposite = std::make_shared(lanelet::CarmaTrafficLight::buildData(lanelet::utils::getId(), {opposite_stop_ls}, controlled_open_right)); + lanelet::CarmaTrafficSignalPtr tfl_opposite = std::make_shared(lanelet::CarmaTrafficSignal::buildData(lanelet::utils::getId(), {opposite_stop_ls}, {controlled_open_right.front()}, {controlled_open_right.back()})); gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()}); @@ -963,6 +999,42 @@ ros::V_string WMBroadcaster::combineParticipantsToVehicle(const ros::V_string& i return participants; } +void WMBroadcaster::externalMapMsgCallback(const cav_msgs::MapData& map_msg) +{ + auto gf_ptr = std::make_shared(); + + // check if we have seen this message already + bool up_to_date = false; + if (sim_.intersection_id_to_regem_id_.size() == map_msg.intersections.size()) + { + up_to_date = true; + // check id of the intersection only + for (auto intersection : map_msg.intersections) + { + if (sim_.intersection_id_to_regem_id_.find(intersection.id.id) == sim_.intersection_id_to_regem_id_.end()) + { + up_to_date = false; + } + } + } + + if(up_to_date) + return; + + gf_ptr->map_msg_ = map_msg; + gf_ptr->msg_.package.label_exists = true; + gf_ptr->msg_.package.label = "MAP_MSG"; + + // create dummy traffic Control message to add instant activation schedule + cav_msgs::TrafficControlMessageV01 traffic_control_msg; + + // process schedule from message + addScheduleFromMsg(gf_ptr, traffic_control_msg); + + scheduleGeofence(gf_ptr); + +} + // currently only supports geofence message version 1: TrafficControlMessageV01 void WMBroadcaster::geofenceCallback(const cav_msgs::TrafficControlMessage& geofence_msg) { @@ -1076,6 +1148,7 @@ void WMBroadcaster::geoReferenceCallback(const std_msgs::String& geo_ref) void WMBroadcaster::setMaxLaneWidth(double max_lane_width) { max_lane_width_ = max_lane_width; + sim_.setMaxLaneWidth(max_lane_width_); } void WMBroadcaster::setConfigSpeedLimit(double cL) @@ -1188,155 +1261,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficCo lanelet::ConstLaneletOrAreas WMBroadcaster::getAffectedLaneletOrAreas(const lanelet::Points3d& gf_pts) { - // Logic to detect which part is affected - ROS_DEBUG_STREAM("Get affected lanelets loop"); - std::unordered_set affected_lanelets; - for (size_t idx = 0; idx < gf_pts.size(); idx ++) - { - ROS_DEBUG_STREAM("Index: " << idx << " Point: " << gf_pts[idx].x() << ", " << gf_pts[idx].y()); - std::unordered_set possible_lanelets; - - // This loop identifes the lanelets which this point lies within that could be impacted by the geofence - // This loop somewhat inefficiently calls the findNearest method iteratively until all the possible lanelets are identified. - // The reason findNearest is used instead of nearestUntil is because that method orders results by bounding box which - // can give invalid sequences when dealing with large curved lanelets. - bool continue_search = true; - size_t nearest_count = 0; - while (continue_search) { - - nearest_count += 10; // Increase the index search radius by 10 each loop until all nearby lanelets are found - - for (const auto& ll_pair : lanelet::geometry::findNearest(current_map_->laneletLayer, gf_pts[idx].basicPoint2d(), nearest_count)) { // Get the nearest lanelets and iterate over them - auto ll = std::get<1>(ll_pair); - - if (possible_lanelets.find(ll) != possible_lanelets.end()) { // Skip if already found - continue; - } - - double dist = std::get<0>(ll_pair); - ROS_DEBUG_STREAM("Distance to lanelet " << ll.id() << ": " << dist << " max_lane_width: " << max_lane_width_); - - if (dist > max_lane_width_) { // Only save values closer than max_lane_width. Since we are iterating in distance order when we reach this distance the search can stop - continue_search = false; - break; - } - - // Check if the point is inside this lanelet - if(dist == 0.0) { // boost geometry uses a distance of 0 to indicate a point is within a polygon - possible_lanelets.insert(ll); - } - - } - - if (nearest_count >= current_map_->laneletLayer.size()) { // if we are out of lanelets to evaluate then end the search - continue_search = false; - } - } - - // among these llts, filter the ones that are on same direction as the geofence using routing - if (idx + 1 == gf_pts.size()) // we only check this for the last gf_pt after saving everything - { - ROS_DEBUG_STREAM("Last point"); - std::unordered_set filtered = filterSuccessorLanelets(possible_lanelets, affected_lanelets); - ROS_DEBUG_STREAM("Got successor lanelets of size: " << filtered.size()); - affected_lanelets.insert(filtered.begin(), filtered.end()); - break; - } - - ROS_DEBUG_STREAM("Checking possible lanelets"); - // check if each lines connecting end points of the llt is crossing with the line connecting current and next gf_pts - for (auto llt: possible_lanelets) - { - ROS_DEBUG_STREAM("Evaluating lanelet: " << llt.id()); - lanelet::BasicLineString2d gf_dir_line({gf_pts[idx].basicPoint2d(), gf_pts[idx+1].basicPoint2d()}); - lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().end() -1)->basicPoint2d(), (llt.rightBound2d().end() - 1)->basicPoint2d()}); - - // record the llts that are on the same dir - if (boost::geometry::intersects(llt_boundary, gf_dir_line)) - { - ROS_DEBUG_STREAM("Overlaps end line"); - affected_lanelets.insert(llt); - } - // check condition if two geofence points are in one lanelet then check matching direction and record it also - else if (boost::geometry::within(gf_pts[idx+1].basicPoint2d(), llt.polygon2d()) && - affected_lanelets.find(llt) == affected_lanelets.end()) - { - ROS_DEBUG_STREAM("Within new lanelet"); - lanelet::BasicPoint2d median({((llt.leftBound2d().end() - 1)->basicPoint2d().x() + (llt.rightBound2d().end() - 1)->basicPoint2d().x())/2 , - ((llt.leftBound2d().end() - 1)->basicPoint2d().y() + (llt.rightBound2d().end() - 1)->basicPoint2d().y())/2}); - // turn into vectors - Eigen::Vector2d vec_to_median(median); - Eigen::Vector2d vec_to_gf_start(gf_pts[idx].basicPoint2d()); - Eigen::Vector2d vec_to_gf_end(gf_pts[idx + 1].basicPoint2d()); - - // Get vector from start to external point - Eigen::Vector2d start_to_median = vec_to_median - vec_to_gf_start; - - // Get vector from start to end point - Eigen::Vector2d start_to_end = vec_to_gf_end - vec_to_gf_start; - - // Get angle between both vectors - double interior_angle = carma_wm::geometry::getAngleBetweenVectors(start_to_median, start_to_end); - - ROS_DEBUG_STREAM("vec_to_median: " << vec_to_median.x() << ", " << vec_to_median.y()); - ROS_DEBUG_STREAM("vec_to_gf_start: " << vec_to_gf_start.x() << ", " << vec_to_gf_start.y()); - ROS_DEBUG_STREAM("vec_to_gf_end: " << vec_to_gf_end.x() << ", " << vec_to_gf_end.y()); - ROS_DEBUG_STREAM("start_to_median: " << start_to_median.x() << ", " << start_to_median.y()); - ROS_DEBUG_STREAM("start_to_end: " << start_to_end.x() << ", " << start_to_end.y()); - ROS_DEBUG_STREAM("interior_angle: " << interior_angle); - // Save the lanelet if the direction of two points inside aligns with that of the lanelet - - if (interior_angle < M_PI_2 && interior_angle >= 0) affected_lanelets.insert(llt); - } - - } - - } - - ROS_DEBUG_STREAM("affected_lanelets size: " << affected_lanelets.size()); - // Currently only returning lanelet, but this could be expanded to LanelerOrArea compound object - // by implementing non-const version of that LaneletOrArea - lanelet::ConstLaneletOrAreas affected_parts; - // return results in ascending downtrack order from the first point of geofence - std::vector> sorted_parts; - for (auto llt : affected_lanelets) - { - sorted_parts.push_back(std::make_pair(carma_wm::geometry::trackPos(llt, gf_pts.front().basicPoint2d()).downtrack, llt)); - } - std::sort(sorted_parts.begin(), sorted_parts.end(), [](const auto& x, const auto& y){return x.first > y.first;}); - - for (auto pair : sorted_parts) - { - affected_parts.push_back(pair.second); - } - - return affected_parts; -} - -// helper function that filters successor lanelets of root_lanelets from possible_lanelets -std::unordered_set WMBroadcaster::filterSuccessorLanelets(const std::unordered_set& possible_lanelets, const std::unordered_set& root_lanelets) -{ - if (!current_routing_graph_) { - throw std::invalid_argument("No routing graph available"); - } - - std::unordered_set filtered_lanelets; - // we utilize routes to filter llts that are overlapping but not connected - // as this is the last lanelet - // we have to filter the llts that are only geometrically overlapping yet not connected to prev llts - for (auto recorded_llt: root_lanelets) - { - for (auto following_llt: current_routing_graph_->following(recorded_llt, false)) - { - auto mutable_llt = current_map_->laneletLayer.get(following_llt.id()); - auto it = possible_lanelets.find(mutable_llt); - if (it != possible_lanelets.end()) - { - filtered_lanelets.insert(mutable_llt); - } - } - } - return filtered_lanelets; + return carma_wm::query::getAffectedLaneletOrAreas(gf_pts, current_map_, current_routing_graph_, max_lane_width_); } /*! @@ -1449,53 +1374,68 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) ROS_INFO_STREAM("Adding active geofence to the map with geofence id: " << gf_ptr->id_); // if applying workzone geometry geofence, utilize workzone chache to create one + // also multiple map updates can be sent from one geofence object + std::vector> updates_to_send; + bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos; + bool detected_map_msg_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("MAP_MSG") != std::string::npos; if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) { for (auto gf_cache_ptr : work_zone_geofence_cache_) { geofenceFromMsg(gf_cache_ptr.second, gf_cache_ptr.second->msg_); } - gf_ptr = createWorkzoneGeofence(work_zone_geofence_cache_); + updates_to_send.push_back(createWorkzoneGeofence(work_zone_geofence_cache_)); + } + else if (detected_map_msg_signal) + { + updates_to_send = geofenceFromMapMsg(gf_ptr, gf_ptr->map_msg_); } else { geofenceFromMsg(gf_ptr, gf_ptr->msg_); + updates_to_send.push_back(gf_ptr); } - - // add marker to rviz - tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(gf_ptr->gf_pts)); // create visualizer in rviz + for (auto update : updates_to_send) + { + // add marker to rviz + tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(update->gf_pts)); // create visualizer in rviz - // Process the geofence object to populate update remove lists - addGeofenceHelper(gf_ptr); - - for (auto pair : gf_ptr->update_list_) active_geofence_llt_ids_.insert(pair.first); + // Process the geofence object to populate update remove lists + addGeofenceHelper(update); + + if (!detected_map_msg_signal) + { + for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first); + } - // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated - if (gf_ptr->invalidate_route_) { + // If the geofence invalidates the route graph then recompute the routing graph now that the map has been updated + if (update->invalidate_route_) { - ROS_INFO_STREAM("Rebuilding routing graph after is was invalidated by geofence"); + ROS_INFO_STREAM("Rebuilding routing graph after is was invalidated by geofence"); - lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_); - current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car); + lanelet::traffic_rules::TrafficRulesUPtr traffic_rules_car = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::traffic_rules::CarmaUSTrafficRules::Location, participant_); + current_routing_graph_ = lanelet::routing::RoutingGraph::build(*current_map_, *traffic_rules_car); - ROS_INFO_STREAM("Done rebuilding routing graph after is was invalidated by geofence"); + ROS_INFO_STREAM("Done rebuilding routing graph after is was invalidated by geofence"); + } + + + // Publish + autoware_lanelet2_msgs::MapBin gf_msg; + auto send_data = std::make_shared(carma_wm::TrafficControl(update->id_, update->update_list_, update->remove_list_, update->lanelet_additions_)); + send_data->traffic_light_id_lookup_ = update->traffic_light_id_lookup_; + carma_wm::toBinMsg(send_data, &gf_msg); + update_count_++; // Update the sequence count for the geofence messages + gf_msg.header.seq = update_count_; + gf_msg.invalidates_route=update->invalidate_route_; + gf_msg.map_version = current_map_version_; + map_update_message_queue_.push_back(gf_msg); // Add diff to current map update queue + map_update_pub_(gf_msg); } - - // Publish - autoware_lanelet2_msgs::MapBin gf_msg; - auto send_data = std::make_shared(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, gf_ptr->lanelet_additions_)); - send_data->traffic_light_id_lookup_ = gf_ptr->traffic_light_id_lookup_; - carma_wm::toBinMsg(send_data, &gf_msg); - update_count_++; // Update the sequence count for the geofence messages - gf_msg.header.seq = update_count_; - gf_msg.invalidates_route=gf_ptr->invalidate_route_; - gf_msg.map_version = current_map_version_; - map_update_message_queue_.push_back(gf_msg); // Add diff to current map update queue - map_update_pub_(gf_msg); } void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) @@ -1880,7 +1820,7 @@ void WMBroadcaster::publishLightId() unsigned intersection_id = 0; unsigned group_id = 0; auto route_lanelet= current_map_->laneletLayer.get(id); - auto traffic_lights = route_lanelet.regulatoryElementsAs(); + auto traffic_lights = route_lanelet.regulatoryElementsAs(); if (!traffic_lights.empty()) { diff --git a/cooperative_lanechange/include/cooperative_lanechange.h b/cooperative_lanechange/include/cooperative_lanechange.h index 58b70f14a9..ddfe64dedc 100644 --- a/cooperative_lanechange/include/cooperative_lanechange.h +++ b/cooperative_lanechange/include/cooperative_lanechange.h @@ -182,6 +182,9 @@ namespace cooperative_lanechange double maneuver_fraction_completed_ = 0; // flag to check if CLC plugin is called bool clc_called_ = false; + //flag to check if lane change is in progress + bool is_lanechange_in_progress_ = false; + double lc_starting_downtrack_ = 0.0; // Mobility request id std::string clc_request_id_ = "default_request_id"; // ROS params diff --git a/cooperative_lanechange/src/cooperative_lanechange.cpp b/cooperative_lanechange/src/cooperative_lanechange.cpp index 71f3efe521..a0dd41b9f6 100644 --- a/cooperative_lanechange/src/cooperative_lanechange.cpp +++ b/cooperative_lanechange/src/cooperative_lanechange.cpp @@ -295,6 +295,19 @@ namespace cooperative_lanechange //plan lanechange without filling in response ROS_DEBUG_STREAM("Planning lane change trajectory"); + //get constant starting downtrack for lane change + if(!is_lanechange_in_progress_){ + if(current_lanelet_id == stoi(maneuver_plan[0].lane_change_maneuver.starting_lane_id)){ + is_lanechange_in_progress_ = true; + lc_starting_downtrack_ = maneuver_plan[0].lane_change_maneuver.start_dist; + ROS_DEBUG_STREAM("Planning lane change from starting downtrack: "<= maneuver_plan[0].lane_change_maneuver.end_dist){ + is_lanechange_in_progress_ = false; + } + + } + std::vector planned_trajectory_points = plan_lanechange(req); if(negotiate){ @@ -505,8 +518,8 @@ namespace cooperative_lanechange buffer_ending_downtrack_); ROS_DEBUG_STREAM("Current downtrack:"<> /mnt/rw/rsu1609/conf/stack.conf + echo "SecurityEnable = 0" >> /mnt/rw/rsu1609/conf/stack.conf + echo "SendUnsecuredDot2Header = 1" >> /mnt/rw/rsu1609/conf/stack.conf + echo "WBSS_Service_Mode = 0" >> /mnt/rw/rsu1609/conf/stack.conf + +: <<'SKIP0' + echo "BSMEnabled = 0" >> /mnt/rw/rsu1609/conf/stack.conf + echo "HeadingUseDefault = 1" >> /mnt/rw/rsu1609/conf/stack.conf + echo "BSMUnsecurePSID = 0x20" >> /mnt/rw/rsu1609/conf/stack.conf + echo "RandMAC = 0" >> /mnt/rw/rsu1609/conf/stack.conf + echo "WSMP_TxPower = 24" >> /mnt/rw/rsu1609/conf/stack.conf + +SKIP0 +} + +_unfiltered_packet_forwarding() +{ + if [ -e $PWD/rc.mcap ]; then + chmod 777 $PWD/rc.mcap + else + echo "not configured to use 'llc mcap' forwarding" + fi +} + +_edit_rsu_cfg() +{ + sed -i "57s/.*/$ACT_SVCS_PORT/" /mnt/rw/rsu1609/conf/rsu.cfg +} + +_manually_manipulate_rsu_files() +{ +if [ "$HOSTNAME" == "MK5" ]; then + echo + echo "Performing manual setup that cannot be accomplished via SNMP" + _set_static_ipv4_eth0 + + echo "stopping application(s)" + /opt/cohda/application/rc.local stop &>/dev/null + + dmesg -c &>/dev/null + net-snmp-config --create-snmpv3-user -A $PW -X $PW -a SHA -x AES $ID + + _edit_stack_conf + #_edit_rsu_cfg + rm -rf /mnt/rw/rsu1609/conf/user.conf + #rm -rf /mnt/rw/rsu1609/conf/stack.conf + #cp /mnt/src/stack.conf /mnt/rw/rsu1609/conf/ + #_unfiltered_packet_forwarding + #_setup_coredump + sync + + echo "starting application(s)" + /opt/cohda/application/rc.local start &>/dev/null + + echo + echo "$DELAY after restart" + $DELAY +fi +} + + +############################################################################## +# Helper Functions +############################################################################## + +_set_standby() +{ + snmpset $RW_AUTH_ARGS rsuMode.0 i 2 + until snmpget $RW_AUTH_ARGS rsuMode.0 | grep -q 'standby(2)'; do + echo "Waiting for confirmation..." + sleep 1 + done +} + +_set_operate() +{ + snmpset $RW_AUTH_ARGS rsuMode.0 i 4 + until snmpget $RW_AUTH_ARGS rsuMode.0 | grep -q 'operate(4)'; do + echo "Waiting for confirmation..." + sleep 1 + done +} + +_enable_pcap_logging() +{ + #RSU-MIB::rsuIfaceName.1 = STRING: cw-mon-tx + #RSU-MIB::rsuIfaceName.2 = STRING: cw-mon-txa + #RSU-MIB::rsuIfaceName.3 = STRING: cw-mon-rxa + #RSU-MIB::rsuIfaceName.4 = STRING: cw-mon-txb + #RSU-MIB::rsuIfaceName.5 = STRING: cw-mon-rxb + + snmpset $RW_AUTH_ARGS \ + rsuIfaceGenerate.1 i 0 \ + rsuIfaceGenerate.2 i 1 \ + rsuIfaceGenerate.3 i 1 \ + rsuIfaceGenerate.4 i 1 \ + rsuIfaceGenerate.5 i 1 \ + rsuIfaceMaxFileSize.1 i 40 \ + rsuIfaceMaxFileSize.2 i 40 \ + rsuIfaceMaxFileSize.3 i 40 \ + rsuIfaceMaxFileSize.4 i 40 \ + rsuIfaceMaxFileSize.5 i 40 +} + + +############################################################################## +# WSMFwdRx_* table +############################################################################## +_destroy_WSMFwdRx_Table() +{ +echo +echo "SNMP: Destroy rsuDsrcFwd table" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdStatus.9 i 6 \ +rsuDsrcFwdStatus.8 i 6 \ +rsuDsrcFwdStatus.7 i 6 \ +rsuDsrcFwdStatus.6 i 6 \ +rsuDsrcFwdStatus.5 i 6 \ +rsuDsrcFwdStatus.4 i 6 \ +rsuDsrcFwdStatus.3 i 6 \ +rsuDsrcFwdStatus.2 i 6 \ +rsuDsrcFwdStatus.1 i 6 +} + +_set_WSMFwdRx1() +{ +echo +echo "SNMP: Set WSMFwd_Rx1" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.1 x "$WSMFwdRx_PSID1" \ +rsuDsrcFwdDestIpAddr.1 x "$WSMFwdRx_ADDR" \ +rsuDsrcFwdDestPort.1 i "$WSMFwdRx_PORT" \ +rsuDsrcFwdProtocol.1 i "$WSMFwdRx_PCOL" \ +rsuDsrcFwdRssi.1 i "$WSMFwdRx_RSSI" \ +rsuDsrcFwdMsgInterval.1 i 1 \ +rsuDsrcFwdEnable.1 i "$WSMFwdRx_ENABLE1" \ +rsuDsrcFwdStatus.1 i 4 +} + +_get_WSMFwdRx1() +{ +echo +echo "SNMP: Get WSMFwd_Rx1" +snmpget $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.1 \ +rsuDsrcFwdDestIpAddr.1 \ +rsuDsrcFwdDestPort.1 \ +rsuDsrcFwdProtocol.1 \ +rsuDsrcFwdRssi.1 \ +rsuDsrcFwdMsgInterval.1 \ +rsuDsrcFwdDeliveryStart.1 \ +rsuDsrcFwdDeliveryStop.1 \ +rsuDsrcFwdEnable.1 +} + +_set_WSMFwdRx2() +{ +echo +echo "SNMP: Set WSMFwd_Rx2" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.2 x "$WSMFwdRx_PSID2" \ +rsuDsrcFwdDestIpAddr.2 x "$WSMFwdRx_ADDR" \ +rsuDsrcFwdDestPort.2 i "$WSMFwdRx_PORT" \ +rsuDsrcFwdProtocol.2 i "$WSMFwdRx_PCOL" \ +rsuDsrcFwdRssi.2 i "$WSMFwdRx_RSSI" \ +rsuDsrcFwdMsgInterval.2 i 1 \ +rsuDsrcFwdEnable.2 i "$WSMFwdRx_ENABLE2" \ +rsuDsrcFwdStatus.2 i 4 +} + +_get_WSMFwdRx2() +{ +echo +echo "SNMP: Get WSMFwd_Rx2" +snmpget $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.2 \ +rsuDsrcFwdDestIpAddr.2 \ +rsuDsrcFwdDestPort.2 \ +rsuDsrcFwdProtocol.2 \ +rsuDsrcFwdRssi.2 \ +rsuDsrcFwdMsgInterval.2 \ +rsuDsrcFwdDeliveryStart.2 \ +rsuDsrcFwdDeliveryStop.2 \ +rsuDsrcFwdEnable.2 +} + +_set_WSMFwdRx3() +{ +echo +echo "SNMP: Set WSMFwd_Rx3" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.3 x "$WSMFwdRx_PSID3" \ +rsuDsrcFwdDestIpAddr.3 x "$WSMFwdRx_ADDR" \ +rsuDsrcFwdDestPort.3 i "$WSMFwdRx_PORT" \ +rsuDsrcFwdProtocol.3 i "$WSMFwdRx_PCOL" \ +rsuDsrcFwdRssi.3 i "$WSMFwdRx_RSSI" \ +rsuDsrcFwdMsgInterval.3 i 1 \ +rsuDsrcFwdEnable.3 i "$WSMFwdRx_ENABLE3" \ +rsuDsrcFwdStatus.3 i 4 +} + +_get_WSMFwdRx3() +{ +echo +echo "SNMP: Get WSMFwd_Rx3" +snmpget $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.3 \ +rsuDsrcFwdDestIpAddr.3 \ +rsuDsrcFwdDestPort.3 \ +rsuDsrcFwdProtocol.3 \ +rsuDsrcFwdRssi.3 \ +rsuDsrcFwdMsgInterval.3 \ +rsuDsrcFwdDeliveryStart.3 \ +rsuDsrcFwdDeliveryStop.3 \ +rsuDsrcFwdEnable.3 +} + +_set_WSMFwdRx4() +{ +echo +echo "SNMP: Set WSMFwd_Rx4" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.4 x "$WSMFwdRx_PSID4" \ +rsuDsrcFwdDestIpAddr.4 x "$WSMFwdRx_ADDR" \ +rsuDsrcFwdDestPort.4 i "$WSMFwdRx_PORT" \ +rsuDsrcFwdProtocol.4 i "$WSMFwdRx_PCOL" \ +rsuDsrcFwdRssi.4 i "$WSMFwdRx_RSSI" \ +rsuDsrcFwdMsgInterval.4 i 1 \ +rsuDsrcFwdEnable.4 i "$WSMFwdRx_ENABLE4" \ +rsuDsrcFwdStatus.4 i 4 +} + +_get_WSMFwdRx4() +{ +echo +echo "SNMP: Get WSMFwd_Rx4" +snmpget $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.4 \ +rsuDsrcFwdDestIpAddr.4 \ +rsuDsrcFwdDestPort.4 \ +rsuDsrcFwdProtocol.4 \ +rsuDsrcFwdRssi.4 \ +rsuDsrcFwdMsgInterval.4 \ +rsuDsrcFwdDeliveryStart.4 \ +rsuDsrcFwdDeliveryStop.4 \ +rsuDsrcFwdEnable.4 +} + +_set_WSMFwdRx5() +{ +echo +echo "SNMP: Set WSMFwd_Rx5" +snmpset $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.5 x "$WSMFwdRx_PSID5" \ +rsuDsrcFwdDestIpAddr.5 x "$WSMFwdRx_ADDR" \ +rsuDsrcFwdDestPort.5 i "$WSMFwdRx_PORT" \ +rsuDsrcFwdProtocol.5 i "$WSMFwdRx_PCOL" \ +rsuDsrcFwdRssi.5 i "$WSMFwdRx_RSSI" \ +rsuDsrcFwdMsgInterval.5 i 1 \ +rsuDsrcFwdEnable.5 i "$WSMFwdRx_ENABLE5" \ +rsuDsrcFwdStatus.5 i 4 +} + +_get_WSMFwdRx5() +{ +echo +echo "SNMP: Get WSMFwd_Rx_5" +snmpget $RW_AUTH_ARGS \ +rsuDsrcFwdPsid.5 \ +rsuDsrcFwdDestIpAddr.5 \ +rsuDsrcFwdDestPort.5 \ +rsuDsrcFwdProtocol.5 \ +rsuDsrcFwdRssi.5 \ +rsuDsrcFwdMsgInterval.5 \ +rsuDsrcFwdDeliveryStart.5 \ +rsuDsrcFwdDeliveryStop.5 \ +rsuDsrcFwdEnable.5 +} + + + +############################################################################## +# Store-and-Forward table +############################################################################## +#define DOT3_WSMP_PSID_4BYTE_MAX 0xEFFFFFFF +#define DOT3_WSMP_PSID_4BYTE_MIN 0xE0000000 +#define DOT3_WSMP_PSID_3BYTE_MAX 0xDFFFFF +#define DOT3_WSMP_PSID_3BYTE_MIN 0xC00000 +#define DOT3_WSMP_PSID_2BYTE_MAX 0xBFFF +#define DOT3_WSMP_PSID_2BYTE_MIN 0x8000 +#define DOT3_WSMP_PSID_1BYTE_MAX 0x7F +#define DOT3_WSMP_PSID_1BYTE_MIN 0x00 + + +############################################################################## +############################################################################## +# Main +############################################################################## + +echo +echo "Initial test" +#_enforce_run_from_microSD +_detect_host +_manually_manipulate_rsu_files + +_set_standby +_set_operate + +_set_standby +_destroy_WSMFwdRx_Table +_set_operate + +_set_standby +_set_WSMFwdRx1 +_set_operate +_get_WSMFwdRx1 + +_set_standby +_set_WSMFwdRx2 +_set_operate +_get_WSMFwdRx2 + +_set_standby +_set_WSMFwdRx3 +_set_operate +_get_WSMFwdRx3 + +_set_standby +_set_WSMFwdRx4 +_set_operate +_get_WSMFwdRx4 + +#_set_standby +#_set_WSMFwdRx5 +#_set_operate +#_get_WSMFwdRx5 + +echo "SNMP: Enable pcap logging and latch" +_set_standby +_enable_pcap_logging +_set_operate + +echo +echo "SNMP: 'Walk' the entries" +_set_standby +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.1 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.2 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.3 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.4 +#snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.5 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.6 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.7 +#snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.8 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.9 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.10 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.11 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.12 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.13 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.14 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.15 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.16 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.17 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.18 +snmpwalk $RW_AUTH_ARGS iso.0.15628.4.1.19 +_set_operate + +: <<'SKIP1' +SKIP1 + +exit 0 diff --git a/gnss_to_map_convertor/CMakeLists.txt b/gnss_to_map_convertor/CMakeLists.txt index 524de40492..20136d26d4 100644 --- a/gnss_to_map_convertor/CMakeLists.txt +++ b/gnss_to_map_convertor/CMakeLists.txt @@ -37,7 +37,6 @@ find_package(catkin REQUIRED COMPONENTS cav_msgs cav_srvs message_filters - novatel_gps_msgs carma_utils lanelet2_extension lanelet2_core @@ -55,7 +54,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include LIBRARIES gnss_to_map_convertor - CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs tf2_msgs wgs84_utils message_filters novatel_gps_msgs carma_utils tf2_geometry_msgs lanelet2_extension + CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs tf2_msgs wgs84_utils message_filters carma_utils tf2_geometry_msgs lanelet2_extension ) ########### diff --git a/gnss_to_map_convertor/include/gnss_to_map_convertor/GNSSToMapConvertor.h b/gnss_to_map_convertor/include/gnss_to_map_convertor/GNSSToMapConvertor.h index 3c2a450e3b..b985e1bd48 100644 --- a/gnss_to_map_convertor/include/gnss_to_map_convertor/GNSSToMapConvertor.h +++ b/gnss_to_map_convertor/include/gnss_to_map_convertor/GNSSToMapConvertor.h @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include diff --git a/gnss_to_map_convertor/package.xml b/gnss_to_map_convertor/package.xml index 546ee04c34..ddda8f48a4 100644 --- a/gnss_to_map_convertor/package.xml +++ b/gnss_to_map_convertor/package.xml @@ -33,7 +33,6 @@ cav_srvs wgs84_utils message_filters - novatel_gps_msgs carma_utils tf2_geometry_msgs lanelet2_extension diff --git a/health_monitor/include/plugin_manager.h b/health_monitor/include/plugin_manager.h index de264d5361..bbd852d216 100644 --- a/health_monitor/include/plugin_manager.h +++ b/health_monitor/include/plugin_manager.h @@ -21,6 +21,9 @@ #include #include #include "entry_manager.h" +#include +#include + namespace health_monitor { diff --git a/health_monitor/src/health_monitor.cpp b/health_monitor/src/health_monitor.cpp index 0062969576..a7918114d7 100644 --- a/health_monitor/src/health_monitor.cpp +++ b/health_monitor/src/health_monitor.cpp @@ -39,7 +39,7 @@ namespace health_monitor activate_plugin_service_server_ = nh_->advertiseService("plugins/activate_plugin", &HealthMonitor::activate_plugin_cb, this); get_strategic_plugin_by_capability_server_ = nh_->advertiseService("plugins/get_strategic_plugin_by_capability", &PluginManager::get_strategic_plugins_by_capability, &plugin_manager_); get_tactical_plugin_by_capability_server_ = nh_->advertiseService("plugins/get_tactical_plugin_by_capability", &PluginManager::get_tactical_plugins_by_capability, &plugin_manager_); - plugin_discovery_subscriber_ = nh_->subscribe("plugin_discovery", 5, &HealthMonitor::plugin_discovery_cb, this); + plugin_discovery_subscriber_ = nh_->subscribe("plugin_discovery", 10, &HealthMonitor::plugin_discovery_cb, this); driver_discovery_subscriber_ = nh_->subscribe("driver_discovery", 5, &HealthMonitor::driver_discovery_cb, this); // load params diff --git a/health_monitor/src/plugin_manager.cpp b/health_monitor/src/plugin_manager.cpp index 28b281a7dc..5b50b9f228 100644 --- a/health_monitor/src/plugin_manager.cpp +++ b/health_monitor/src/plugin_manager.cpp @@ -77,6 +77,7 @@ namespace health_monitor void PluginManager::update_plugin_status(const cav_msgs::PluginConstPtr& msg) { + ROS_DEBUG_STREAM("received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); // params: bool available, bool active, std::string name, long timestamp, uint8_t type Entry plugin(msg->available, msg->activated, msg->name, 0, msg->type, msg->capability); @@ -111,8 +112,13 @@ namespace health_monitor if(plugin.type_ == cav_msgs::Plugin::STRATEGIC && (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) { + ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); res.plan_service.push_back(service_prefix_ + plugin.name_ + strategic_service_suffix_); } + else + { + ROS_DEBUG_STREAM("not valid strategic plugin: " << plugin.name_); + } } return true; } diff --git a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp index 17f5bdd7b8..92e0ee3298 100644 --- a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp +++ b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp @@ -12,6 +12,7 @@ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations under * the License. + * */ #include diff --git a/light_controlled_intersection_tactical_plugin/CMakeLists.txt b/light_controlled_intersection_tactical_plugin/CMakeLists.txt new file mode 100644 index 0000000000..56c22b8702 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/CMakeLists.txt @@ -0,0 +1,100 @@ +# +# Copyright (C) 2021 LEIDOS. +# +# Licensed under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. +# + +cmake_minimum_required(VERSION 3.0.2) +project(light_controlled_intersection_tactical_plugin) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") +set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") + +set(CATKIN_DEPS + roscpp + cav_msgs + cav_srvs + carma_utils + trajectory_utils + carma_wm + basic_autonomy +) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPS} +) + +## System dependencies are found with CMake's conventions +find_package(Boost) +find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS ${CATKIN_DEPS} + DEPENDS Boost EIGEN3 +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +add_executable( ${PROJECT_NAME} + src/main.cpp) + +add_library(light_controlled_intersection_tactical_library + src/light_controlled_intersection_tactical_plugin.cpp) + +add_dependencies(light_controlled_intersection_tactical_library ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} light_controlled_intersection_tactical_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +############# +## Install ## +############# + +install(DIRECTORY include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +## Install C++ +install(TARGETS ${PROJECT_NAME} light_controlled_intersection_tactical_library + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install Other Resources +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# +catkin_add_gmock(${PROJECT_NAME}-test + test/test_lci_tactical_plugin.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +target_link_libraries(${PROJECT_NAME}-test light_controlled_intersection_tactical_library ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/config/parameters.yaml b/light_controlled_intersection_tactical_plugin/config/parameters.yaml new file mode 100644 index 0000000000..3402a59ade --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/config/parameters.yaml @@ -0,0 +1,42 @@ +# Double: The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions +# Units: m +centerline_sampling_spacing: 1.0 + +# Trajectory length in seconds +trajectory_time_length: 6.0 + +# Amount to downsample input lanelet centerline data. Value corresponds to saving each nth point. +default_downsample_ratio: 36 + +# Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. +turn_downsample_ratio: 20 + +# Curve re-sampling step size in m +curve_resample_step_size: 1.0 + +# Size of the window used in the moving average filter to smooth the computed curvature +curvature_moving_average_window_size: 9 + +# Size of the window used in the moving average filter to smooth the output speeds +speed_moving_average_window_size: 5 + +# Number of meters behind the first maneuver that need to be included in points for curvature calculation +back_distance: 20.0 + +# Additional distance beyond ending downtrack to ensure sufficient points +buffer_ending_downtrack: 20.0 + +# Double: multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities +vehicle_decel_limit_multiplier : 0.50 + +# Double: multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabilities +vehicle_accel_limit_multiplier : 0.50 + +# Multiplier of lat_accel to bring the value under lat_accel +lat_accel_multiplier: 0.50 + +# Double: A buffer before of the stopping location which will still be considered a valid stop. Units in meters +stop_line_buffer : 2.0 + +# Double: Minimum allowable speed in m/s +minimum_speed : 2.2352 diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h new file mode 100644 index 0000000000..699dfb6a70 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h @@ -0,0 +1,75 @@ +#pragma once + +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include + +/** + * \brief Stuct containing the algorithm configuration values for the LightControlledIntersectionTacticalPlugin + */ +struct LightControlledIntersectionTacticalPluginConfig +{ + int default_downsample_ratio = 36; // Amount to downsample input lanelet centerline data. Value corresponds to saving each nth point. + int turn_downsample_ratio = 20; // Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. + double trajectory_time_length = 6.0; // Trajectory length in seconds + double curve_resample_step_size = 1.0; // Curve re-sampling step size in m + int curvature_moving_average_window_size = 9; // Size of the window used in the moving average filter to smooth the curvature profile + // computed curvature and output speeds + double lateral_accel_limit = 2.5; // Maximum allowable lateral acceleration m/s^2 + double lat_accel_multiplier = 0.50; // Multiplier of lat_accel to bring the value under lat_accel + int speed_moving_average_window_size = 5; // Size of the window used in the moving average filter to smooth both the speed profile + double back_distance = 20; // Number of meters behind the first maneuver that need to be included in points for curvature calculation + double buffer_ending_downtrack = 20.0; // Additional distance beyond ending downtrack to ensure sufficient points + //! The maximum allowable vehicle deceleration limit in m/s + double vehicle_decel_limit = 2.0; + double minimum_speed = 2.2352; // minimum allowable speed in m/s + + //! A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities + double vehicle_decel_limit_multiplier = 0.75; + + //! The maximum allowable vehicle acceleration limit in m/s + double vehicle_accel_limit = 2.0; + + //! A multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabilities + double vehicle_accel_limit_multiplier = 0.75; + + //! A buffer infront of the stopping location which will still be considered a valid stop + double stop_line_buffer = 2.0; + + friend std::ostream& operator<<(std::ostream& output, const LightControlledIntersectionTacticalPluginConfig& c) + { + output <<"LightControlledIntersectionTacticalPluginConfig { " < +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * \brief Macro definition to enable easier access to fields shared across the maneuver types + * \param mvr The maneuver object to invoke the accessors on + * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types + * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field + */ +#define GET_MANEUVER_PROPERTY(mvr, property)\ + (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) + +namespace light_controlled_intersection_transit_plugin +{ +using PublishPluginDiscoveryCB = std::function; +using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; +using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig; +using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig; + +enum SpeedProfileCase { + ACCEL_CRUISE_DECEL = 1, + ACCEL_DECEL = 2, + DECEL_ACCEL = 3, + DECEL_CRUISE_ACCEL = 4, +}; + +/** + * \brief Class containing primary business logic for the Light Controlled Intersection Tactical Plugin + * + */ + +class LightControlledIntersectionTacticalPlugin +{ +public: + /** + * \brief Constructor + * + * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data + * \param config The configuration to be used for this object + * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state + * \param debug_publisher Callback which will publish a debug message. The callback defaults to no-op. + */ + LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,const LightControlledIntersectionTacticalPluginConfig& config, + const PublishPluginDiscoveryCB &plugin_discovery_publisher); + + /** + * \brief Service callback for trajectory planning + * + * \param req The service request + * \param resp The service response + * + * \return True if success. False otherwise + */ + bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); + + /** + * \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 List 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) + * \param end_dist ending downtrack of the maneuver to be planned (excluding buffer points) + * \param remaining_time time interval left for scheduled entry into the intersection + * \param starting_speed starting speed at the start of the maneuver + * \param speed_before_decel highest speed desired between acceleration and decelaration + * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection + * 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 apply_accel_cruise_decel_speed_profile(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double end_dist, + double remaining_time, double starting_speed, double speed_before_decel, double departure_speed); + + /** + * \brief Creates a speed profile according to case three or four of the light controlled intersection, where the vehicle decelerates (then cruises if needed) and accelerates into the intersection. + * \param wm world_model pointer + * \param List 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) + * \param end_dist ending downtrack of the maneuver to be planned (excluding buffer points) + * \param remaining_time time interval left for scheduled entry into the intersection + * \param starting_speed starting speed at the start of the maneuver + * \param speed_before_decel highest speed desired between deceleration and acceleration + * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection + * NOTE: Cruising speed profile is applied (case 4) if speed before acceleration is lower than minimum speed allowed. Otherwise Case 3. + * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and min_speed_allowed_ + */ + void apply_decel_cruise_accel_speed_profile(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double end_dist, + double remaining_time, double starting_speed, double speed_before_decel, double departure_speed); //TODO + + /** + * \brief Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. + * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing + * \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. + * + */ + void apply_optimized_target_speed_profile(const cav_msgs::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); + + /** + * \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. + * \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 + * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver + */ + std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + cav_msgs::VehicleState &ending_state_before_buffer,const cav_msgs::VehicleState& state, + const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); + + /** + * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery updates + */ + + void onSpin(); + + /** + * \brief calculate the speed, right before the car starts to decelerate for timed entry into the intersection + * + * \param entry_time time the vehicle must stop + * + * \param entry_dist distance to stop line + * + * \param current_speed current speed of vehicle + * + * \param departure_speed speed to get into the intersection + * + * \return speed value + */ + double calcSpeedBeforeDecel(double entry_time, double entry_dist, double current_speed, double departure_speed) const; + + /** + * \brief calculate the speed, right before the car starts to accelerate for timed entry into the intersection + * + * \param entry_time time the vehicle must stop + * + * \param entry_dist distance to stop line + * + * \param current_speed current speed of vehicle + * + * \param departure_speed speed to get into the intersection + * + * \return speed value + */ + double calcSpeedBeforeAccel(double entry_time, double entry_dist, double current_speed, double departure_speed) const; + + /** + * \brief Determine the speed profile case for approaching an intersection. + * It internally utilizes config_.min_speed and speed_limit_ to determine upper and lower speed bounds + * + * \param estimated_entry_time estimated time to enter the intersection without speed modification + * + * \param scheduled_entry_time scheduled time to enter the intersection + * + * \param speed_before_decel speed before starting to decelerate (applicable in case 1, 2) + * + * \param speed_before_accel speed before starting to accelerate (applicable in case 1, 2) + * + * \return integer case number + */ + SpeedProfileCase determineSpeedProfileCase(double estimated_entry_time, double scheduled_entry_time, double speed_before_decel, double speed_before_accel); + + + /** + * \brief calculate the time vehicle will enter to intersection with optimal deceleration + * + * \param entry_dist distance to stop line + * + * \param current_speed current speed of vehicle + * + * \param departure_speed speed to get into the intersection + * + * \return the time vehicle will stop with optimal decelarion + */ + double calcEstimatedEntryTimeLeft(double entry_dist, double current_speed, double departure_speed) const; + + ////////// VARIABLES /////////// + // CARMA Streets Variables + // timestamp for msg received from carma streets + uint32_t street_msg_timestamp_ = 0.0; + // scheduled stop time + uint32_t scheduled_stop_time_ = 0.0; + // scheduled enter time + uint32_t scheduled_enter_time_ = 0.0; + // scheduled depart time + uint32_t scheduled_depart_time_ = 0.0; + // scheduled latest depart time + uint32_t scheduled_latest_depart_time_ = 0.0; + // flag to show if the vehicle is allowed in intersection + bool is_allowed_int_ = false; + + // approximate speed limit + double speed_limit_ = 100.0; + + cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later + + // downtrack of host vehicle + double current_downtrack_ = 0.0; + + private: + + carma_wm::WorldModelConstPtr wm_; + LightControlledIntersectionTacticalPluginConfig config_; + PublishPluginDiscoveryCB plugin_discovery_publisher_; + + cav_msgs::Plugin plugin_discovery_msg_; + carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; + + std::string light_controlled_intersection_strategy_ = "Carma/light_controlled_intersection"; + + double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with +}; +}; \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h new file mode 100644 index 0000000000..b650078777 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h @@ -0,0 +1,116 @@ +#pragma once + +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace light_controlled_intersection_transit_plugin +{ + /** + * \brief ROS node for the LightControlledIntersectionTransitPlugin + */ +class LightControlledIntersectionTransitPluginNode +{ +public: + /** + * \brief Entrypoint for this node + */ + void run() + { + ros::CARMANodeHandle nh; + ros::CARMANodeHandle pnh("~"); + + carma_wm::WMListener wml; + auto wm_ = wml.getWorldModel(); + + LightControlledIntersectionTacticalPluginConfig config; + + pnh.param("trajectory_time_length", config.trajectory_time_length, config.trajectory_time_length); + pnh.param("curve_resample_step_size", config.curve_resample_step_size, config.curve_resample_step_size); + pnh.param("default_downsample_ratio", config.default_downsample_ratio, config.default_downsample_ratio); + pnh.param("turn_downsample_ratio", config.turn_downsample_ratio, config.turn_downsample_ratio); + pnh.param("vehicle_decel_limit_multiplier", config.vehicle_decel_limit_multiplier, config.vehicle_decel_limit_multiplier); + pnh.param("vehicle_accel_limit_multiplier", config.vehicle_accel_limit_multiplier, config.vehicle_accel_limit_multiplier); + pnh.param("lat_accel_multiplier", config.lat_accel_multiplier, config.lat_accel_multiplier); + pnh.param("back_distance", config.back_distance, config.back_distance); + pnh.param("speed_moving_average_window_size", config.speed_moving_average_window_size, + config.speed_moving_average_window_size); + pnh.param("curvature_moving_average_window_size", config.curvature_moving_average_window_size, + config.curvature_moving_average_window_size); + pnh.param("buffer_ending_downtrack", config.buffer_ending_downtrack, config.buffer_ending_downtrack); + pnh.param("/vehicle_deceleration_limit", config.vehicle_decel_limit, config.vehicle_decel_limit); + pnh.param("/vehicle_acceleration_limit", config.vehicle_accel_limit, config.vehicle_accel_limit); + pnh.param("/vehicle_lateral_accel_limit", config.lateral_accel_limit, config.lateral_accel_limit); + pnh.param("stop_line_buffer", config.stop_line_buffer, config.stop_line_buffer); + pnh.param("minimum_speed", config.minimum_speed, config.minimum_speed); + + ROS_INFO_STREAM("LightControlledIntersectionTacticalPlugin Params" << config); + + config.lateral_accel_limit = config.lateral_accel_limit * config.lat_accel_multiplier; + config.vehicle_accel_limit = config.vehicle_accel_limit * config.vehicle_accel_limit_multiplier; + config.vehicle_decel_limit = config.vehicle_decel_limit * config.vehicle_decel_limit_multiplier; + + ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); + + LightControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); + + ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/LightControlledIntersectionTacticalPlugin/plan_trajectory", + &LightControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); + + ros::Timer discovery_pub_timer_ = + pnh.createTimer(ros::Duration(ros::Rate(10.0)), [&worker](const auto&) { worker.onSpin(); }); + + ros::CARMANodeHandle::spin(); + } +}; + +} //namespace light_controlled_intersection_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch new file mode 100644 index 0000000000..b033b535a5 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch @@ -0,0 +1,20 @@ + + + + + + + diff --git a/light_controlled_intersection_tactical_plugin/package.xml b/light_controlled_intersection_tactical_plugin/package.xml new file mode 100644 index 0000000000..7de4b5ca4c --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/package.xml @@ -0,0 +1,35 @@ + + + + + + light_controlled_intersection_tactical_plugin + 1.0.0 + Creates trajectory plan for light controlled intersections + carma + Apache License 2.0 + carma + catkin + roscpp + cav_srvs + cav_msgs + carma_utils + trajectory_utils + basic_autonomy + carma_wm + + \ No newline at end of file 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 new file mode 100644 index 0000000000..f7423ca537 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -0,0 +1,512 @@ +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using oss = std::ostringstream; + +namespace light_controlled_intersection_transit_plugin +{ + +LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,const LightControlledIntersectionTacticalPluginConfig& config, + const PublishPluginDiscoveryCB& plugin_discovery_publisher) + : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) + { + plugin_discovery_msg_.name = "LightControlledIntersectionTacticalPlugin"; + plugin_discovery_msg_.versionId = "v1.0"; + plugin_discovery_msg_.available = true; + plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; + plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; + } + +void LightControlledIntersectionTacticalPlugin::onSpin() +{ + plugin_discovery_publisher_(plugin_discovery_msg_); +} + +bool LightControlledIntersectionTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp) +{ + ROS_DEBUG_STREAM("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) + + " for plan of size: " + std::to_string(req.maneuver_plan.maneuvers.size())); + } + std::vector maneuver_plan; + // expecting only one maneuver for an intersection + for(size_t i = req.maneuver_index_to_plan; i < req.maneuver_plan.maneuvers.size(); i++){ + + if(req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT + || req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN || req.maneuver_plan.maneuvers[i].type ==cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN) + { + maneuver_plan.push_back(req.maneuver_plan.maneuvers[i]); + resp.related_maneuvers.push_back(req.maneuver_plan.maneuvers[i].type); + break; + } + else + { + throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); + } + } + + lanelet::BasicPoint2d veh_pos(req.vehicle_state.X_pos_global, req.vehicle_state.Y_pos_global); + ROS_DEBUG_STREAM("Planning state x:"<routeTrackPos(veh_pos).downtrack; + ROS_DEBUG_STREAM("Current_downtrack"<< current_downtrack_); + + DetailedTrajConfig wpg_detail_config; + GeneralTrajConfig wpg_general_config; + + wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("intersection_transit", + 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, + config_.vehicle_accel_limit, + config_.lateral_accel_limit, + config_.speed_moving_average_window_size, + config_.curvature_moving_average_window_size, config_.back_distance, + config_.buffer_ending_downtrack); + + // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver + auto points_and_target_speeds = create_geometry_profile(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); + + // Change raw speed limit values to target speeds specified by the algorithm + apply_optimized_target_speed_profile(maneuver_plan.front(), req.vehicle_state.longitudinal_vel, points_and_target_speeds); + + ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size()); + + ROS_DEBUG_STREAM("PlanTrajectory"); + + //Trajectory Plan + cav_msgs::TrajectoryPlan trajectory; + trajectory.header.frame_id = "map"; + trajectory.header.stamp = req.header.stamp; + 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_, + wpg_detail_config); // Compute the trajectory + trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel; + + resp.trajectory_plan = trajectory; + + resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); + + return true; +} + +SpeedProfileCase LightControlledIntersectionTacticalPlugin::determineSpeedProfileCase(double estimated_entry_time, double scheduled_entry_time, double speed_before_decel, double speed_before_accel) +{ + SpeedProfileCase case_num; + + ROS_DEBUG_STREAM("estimated_entry_time: " << estimated_entry_time << ", and scheduled_entry_time: " << scheduled_entry_time); + if (estimated_entry_time < scheduled_entry_time) + { + ROS_DEBUG_STREAM("speed_before_accel: " << speed_before_accel << ", and config_.minimum_speed: " << config_.minimum_speed); + + if (speed_before_accel < config_.minimum_speed) + { + case_num = DECEL_CRUISE_ACCEL; + } + else + { + case_num = DECEL_ACCEL; + } + } + else + { + ROS_DEBUG_STREAM("speed_before_decel: " << speed_before_decel << ", and speed_limit_: " << speed_limit_); + + if (speed_before_decel > speed_limit_) + { + case_num = ACCEL_CRUISE_DECEL; + } + else + { + case_num = ACCEL_DECEL; + } + } + + return case_num; +} + +void LightControlledIntersectionTacticalPlugin::apply_optimized_target_speed_profile(const cav_msgs::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) +{ + if(GET_MANEUVER_PROPERTY(maneuver,parameters.float_valued_meta_data).size() < 2){ + throw std::invalid_argument("Manuever timestamp and desired entry time into the signalized intersection are not provided in the meta data."); + } + + double scheduled_entry_time = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); + double maneuver_start_time = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); + double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); + double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist); + double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed); + + double entry_dist = GET_MANEUVER_PROPERTY(maneuver,end_dist) - starting_downtrack; + double estimated_entry_time = calcEstimatedEntryTimeLeft(entry_dist, starting_speed, departure_speed ); + double speed_before_decel = calcSpeedBeforeDecel(estimated_entry_time, entry_dist, starting_speed, departure_speed); + double speed_before_accel = calcSpeedBeforeAccel(estimated_entry_time, entry_dist, starting_speed, departure_speed); + double remaining_time = scheduled_entry_time - maneuver_start_time; + SpeedProfileCase case_num = determineSpeedProfileCase(estimated_entry_time, scheduled_entry_time, speed_before_decel, speed_before_accel); + + // change speed profile depending on algorithm case starting from maneuver start_dist + if(case_num == ACCEL_CRUISE_DECEL || case_num == ACCEL_DECEL){ + // acceleration (cruising if needed) then deceleration to reach desired intersection entry speed/time according to algorithm doc + apply_accel_cruise_decel_speed_profile(wm_, points_and_target_speeds, starting_downtrack, ending_downtrack, remaining_time, starting_speed, speed_before_decel, departure_speed); + } + else if(case_num == DECEL_ACCEL || case_num == DECEL_CRUISE_ACCEL) + { + // deceleration (cruising if needed) then acceleration to reach desired intersection entry speed/time according to algorithm doc + apply_decel_cruise_accel_speed_profile(wm_, points_and_target_speeds, starting_downtrack, ending_downtrack, remaining_time, starting_speed, speed_before_decel, departure_speed); + } + else + { + throw std::invalid_argument("The light controlled intersection tactical plugin doesn't handle the case number requested"); + } +} +std::vector LightControlledIntersectionTacticalPlugin::create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + cav_msgs::VehicleState &ending_state_before_buffer,const cav_msgs::VehicleState& state, + const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) +{ + std::vector points_and_target_speeds; + + bool first = true; + std::unordered_set visited_lanelets; + + ROS_DEBUG_STREAM("VehDowntrack:"< lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, ending_state_before_buffer, general_config, detailed_config, visited_lanelets); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + + break; // expected to receive only one maneuver to plan + } + + return points_and_target_speeds; +} + +double LightControlledIntersectionTacticalPlugin::calcEstimatedEntryTimeLeft(double entry_dist, double current_speed, double departure_speed) const +{ + double t_entry = 0; + // t = 2 * d / (v_i + v_f) + // from TSMO USE CASE 2 Algorithm Doc - Figure 4. Equation: Estimation of t*_nt + t_entry = 2*entry_dist/(current_speed + departure_speed); + ROS_DEBUG_STREAM("Estimated entry time: " << t_entry); + return t_entry; +} + +double LightControlledIntersectionTacticalPlugin::calcSpeedBeforeDecel(double entry_time, double entry_dist, double current_speed, double departure_speed) const +{ + double speed_before_decel = 0; + + double max_comfort_accel = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; + double max_comfort_decel = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + + // from TSMO USE CASE 2 Algorithm Doc - Figure 7. Equation: Trajectory Smoothing Solution (Case 2) + + // a_r = a_acc / a_dec + double acc_dec_ratio = max_comfort_accel/max_comfort_decel; + // v_r = d / t + double required_speed = entry_dist / entry_time; + // sqrt_term = sqrt((1-a_r)^2*v_r^2 - (1-a_r)(a_r*v_f*(v_f-2*v_r) + v_i*(2*v_r - v_i))) + double sqr_term = sqrt(pow(1 - (acc_dec_ratio), 2) * pow(required_speed, 2) - (1 -acc_dec_ratio) * + (acc_dec_ratio * departure_speed * (departure_speed - 2 * required_speed) + current_speed * (2* required_speed - current_speed))); + // v_e = v_r + sqrt_term/(1 - a_r) + speed_before_decel = required_speed + sqr_term/(1 - acc_dec_ratio); + + return speed_before_decel; +} + +double LightControlledIntersectionTacticalPlugin::calcSpeedBeforeAccel(double entry_time, double entry_dist, double current_speed, double departure_speed) const +{ + double speed_before_accel = 0; + + double max_comfort_accel = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; + double max_comfort_decel = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + + // from TSMO USE CASE 2 Algorithm Doc - Figure 11. Equation: Trajectory Smoothing Solution (Case 3) + + // a_r = a_acc / a_dec + double acc_dec_ratio = max_comfort_accel/max_comfort_decel; + // v_r = d / t + double required_speed = entry_dist / entry_time; + // sqrt_term = sqrt((a_r - 1)^2*v_r^2 - (a_r-1)(v_f*(v_f-2*v_r) + a_r*v_i*(2*v_r - v_i))) + double sqr_term = sqrt(pow((acc_dec_ratio - 1), 2) * pow(required_speed, 2) - (acc_dec_ratio - 1) * + (departure_speed * (departure_speed - 2 * required_speed) + acc_dec_ratio * current_speed * (2* required_speed - current_speed))); + // v_e = v_r + sqrt_term / (a_r - 1) + speed_before_accel = required_speed + sqr_term/(acc_dec_ratio - 1); + + return speed_before_accel; +} + +void LightControlledIntersectionTacticalPlugin::apply_accel_cruise_decel_speed_profile(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double end_dist, + double remaining_time, double starting_speed, double speed_before_decel, double departure_speed) +{ + if (points_and_target_speeds.empty()) + { + throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); + } + + double max_comfort_accel = config_.vehicle_accel_limit; + double max_comfort_decel = -1 * config_.vehicle_decel_limit; + double remaining_dist = end_dist - start_dist; + + // a_r = a_acc / a_dec + double acc_dec_ratio = max_comfort_accel/max_comfort_decel; + + double t_cruise = 0.0; // Cruising Time Interval for Case 2. TSMO UC 2 Algorithm draft doc Figure 7. + double t_c_nom = 0.0; + double t_c_den = epsilon_; + + if (speed_before_decel > speed_limit_) + { + ROS_DEBUG_STREAM("Detected that cruising is necessary. Changed speed_before_decel: " << speed_before_decel << ", to : " << speed_limit_); + speed_before_decel = speed_limit_; + + // Cruising Time Interval Equation (case 1) obtained from TSMO UC 2 Algorithm draft doc Figure 8. + // Nominator portion + t_c_nom = 2 * remaining_dist * ((1 - acc_dec_ratio) * speed_before_decel + acc_dec_ratio * departure_speed - starting_speed) - + remaining_time * ((1 - acc_dec_ratio) * pow(speed_before_decel, 2) + acc_dec_ratio * pow(departure_speed, 2) - pow(starting_speed, 2)); + + // Denominator portion + t_c_den = pow(speed_before_decel - starting_speed, 2) - acc_dec_ratio * pow(speed_before_decel - departure_speed, 2); + + if (t_c_den < epsilon_) + { + ROS_WARN_STREAM("Denominator of cruising time interval is too close to zero: " + << t_c_den << ", which may indicate there is only cruising portion available. Returning without any change..."); + return; + } + + t_cruise = t_c_nom / t_c_den; + } + // From TSMO USE CASE 2 Algorithm Doc - Figure 8. Equation: Trajectory Smoothing Solution (Case 1 and 2) + + ROS_DEBUG_STREAM("max_comfort_accel: " << max_comfort_accel << "\n" << + "max_comfort_decel: " << max_comfort_decel << "\n" << + "acc_dec_ratio: " << acc_dec_ratio << "\n" << + "speed_limit_: " << speed_limit_); + + // Rest of the equations for acceleration rates and time intervals for when accelerating or decelerating + double a_acc = ((1 - acc_dec_ratio) * speed_before_decel + acc_dec_ratio * departure_speed - starting_speed) / (remaining_time - t_cruise); + double a_dec = ((max_comfort_decel - max_comfort_accel) * speed_before_decel + max_comfort_accel * departure_speed - max_comfort_decel * starting_speed) / (max_comfort_accel * (remaining_time - t_cruise)); + double t_acc = (speed_before_decel - starting_speed) / a_acc; + double t_dec = (departure_speed - speed_before_decel) / a_dec; + + ROS_DEBUG_STREAM("speed_before_decel: " << speed_before_decel << "\n" << + "departure_speed: " << departure_speed << "\n" << + "remaining_dist: " << remaining_dist << "\n" << + "t_c_nom: " << t_c_nom << "\n" << + "t_c_den: " << t_c_den << "\n" << + "t_cruise: " << t_cruise << "\n" << + "a_acc: " << a_acc << "\n" << + "a_dec: " << a_dec << "\n" << + "t_acc: " << t_acc << "\n" << + "t_dec: " << t_dec); + + if (remaining_time - t_cruise < epsilon_ && remaining_time - t_cruise >= 0.0) + { + ROS_WARN_STREAM("Only Cruising is needed... therefore, no speed modification is required. Returning... "); + return; + } + else if (t_cruise < -epsilon_) + { + throw std::invalid_argument(std::string("Input parameters are not valid or do not qualify conditions " + "of estimated_time >= scheduled_time (case 1 and 2)")); + } + + // 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 + double dist_accel; //Distance over which acceleration happens + double dist_cruise; //Distance over which cruising happens + double dist_decel; //Distance over which deceleration happens + + //Use maneuver parameters to create speed profile + //Kinematic: d = v_0 * t + 1/2 * a * t^2 + dist_accel = starting_speed * t_acc + 0.5 * a_acc * pow(t_acc, 2); + dist_cruise = speed_before_decel * t_cruise; + dist_decel = speed_before_decel * t_dec + 0.5 * a_dec * pow(t_dec, 2); + + //Check calculated total dist against maneuver limits + double total_distance_needed = dist_accel + dist_cruise + dist_decel; + + ROS_DEBUG_STREAM("total_distance_needed: " << total_distance_needed << "\n" << + "dist_accel: " << dist_accel << "\n" << + "dist_decel: " << dist_decel << "\n" << + "dist_cruise: " << dist_cruise); + + if(dist_accel < - epsilon_ ) + { + //Requested maneuver needs to be modified to meet start and end dist req + //Sacrifice on cruising and then acceleration if needed + + //correcting signs. NOTE: Doing so will likely result being over max_comfort_accel + dist_accel = std::fabs(dist_accel); + a_acc = std::fabs(a_acc); + a_dec = -1 * std::fabs(a_dec); + //subtract distance from cruising segment to match original distance + dist_cruise -= 2 * dist_accel; + if(dist_cruise < 0) + { + dist_accel -= dist_cruise; + dist_cruise = 0; + } + ROS_WARN_STREAM("Maneuver needed to be modified with new distance and accelerations: \n" << + "total_distance_needed: " << total_distance_needed << "\n" << + "a_acc: " << a_acc << "\n" << + "a_dec: " << a_dec << "\n" << + "dist_accel: " << dist_accel << "\n" << + "dist_decel: " << dist_decel << "\n" << + "dist_cruise: " << dist_cruise); + // not accounting dist_accel < 0 after this... + } + + if(dist_decel < - epsilon_ ) + { + //Requested maneuver needs to be modified to meet start and end dist req + //Sacrifice on cruising and then acceleration if needed + + //correct signs. NOTE: Doing so will likely result being over max_comfort_accel + dist_decel = std::fabs(dist_decel); + a_acc = std::fabs(a_acc); + a_dec = -1 * std::fabs(a_dec); + //subtract distance from cruising segment to match original distance + dist_cruise -= 2 * dist_decel; + if(dist_cruise < 0) + { + dist_accel -= dist_cruise; + dist_cruise = 0; + } + ROS_WARN_STREAM("Maneuver needed to be modified with new distance and accelerations: \n" << + "total_distance_needed: " << total_distance_needed << "\n" << + "a_acc: " << a_acc << "\n" << + "a_dec: " << a_dec << "\n" << + "dist_accel: " << dist_accel << "\n" << + "dist_decel: " << dist_decel << "\n" << + "dist_cruise: " << dist_cruise); + // not accounting dist_accel < 0 after this... + } + + double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0 + + 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; + ROS_DEBUG_STREAM("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; + + //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_) + { + //Keep target speed same for buffer distance portion + speed_i = starting_speed; + } + else if(total_dist_planned <= dist_accel){ + //Acceleration part + speed_i = sqrt(pow(starting_speed, 2) + 2 * a_acc * total_dist_planned); + } + else if(dist_cruise > 0 && total_dist_planned > dist_accel && total_dist_planned <= (dist_accel + dist_cruise)){ + //Cruising part + speed_i = speed_before_decel; + } + else if (total_dist_planned <= total_distance_needed) + { + //Deceleration part + speed_i = sqrt(std::max(pow(speed_before_decel, 2) + 2 * a_dec * (total_dist_planned - dist_accel - dist_cruise), 0.0)); //std::max to ensure negative value is not sqrt + } + else + { + //buffer points that will be cut + speed_i = prev_speed; + } + + p.speed = std::min(speed_i,speed_before_decel); + ROS_DEBUG_STREAM("Applied speed: " << p.speed << ", at dist: " << total_dist_planned); + + prev_point = p; + prev_speed = speed_i; + } +} + +void LightControlledIntersectionTacticalPlugin::apply_decel_cruise_accel_speed_profile(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double end_dist, + double remaining_time, double starting_speed, double speed_before_decel, double departure_speed){} + + + +} //namespace light_controlled_intersection_transit_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/src/main.cpp b/light_controlled_intersection_tactical_plugin/src/main.cpp new file mode 100644 index 0000000000..cba3a602f6 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/src/main.cpp @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include + +#include + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "light_controlled_intersection_tactical_plugin"); + light_controlled_intersection_transit_plugin::LightControlledIntersectionTransitPluginNode sc; + sc.run(); + return 0; + +}; diff --git a/light_controlled_intersection_tactical_plugin/test/test_lci_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/test/test_lci_tactical_plugin.cpp new file mode 100644 index 0000000000..2d1ea1c825 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/test/test_lci_tactical_plugin.cpp @@ -0,0 +1,352 @@ +/* + * Copyright (C) 2021 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace light_controlled_intersection_transit_plugin +{ +TEST(LCITacticalPluginTest, determineSpeedProfileCasetest) +{ + std::shared_ptr wm = std::make_shared(); + LightControlledIntersectionTacticalPluginConfig config; + config.minimum_speed = 2; + config.vehicle_accel_limit= 2; + config.vehicle_accel_limit_multiplier= 1; + config.vehicle_decel_limit= 1; + config.vehicle_decel_limit_multiplier= 1; + + + LightControlledIntersectionTacticalPlugin lci(wm, config, [&](auto msg) {}); + lci.speed_limit_ = 10; + + auto case_num1 = lci.determineSpeedProfileCase(10, 12, 9, 8); + + EXPECT_EQ(3, case_num1); + + auto case_num2 = lci.determineSpeedProfileCase(10, 12, 12, 1); + + EXPECT_EQ(4, case_num2); + + auto case_num3 = lci.determineSpeedProfileCase(10, 9, 1, 2); + + EXPECT_EQ(2, case_num3); + + auto case_num4 = lci.determineSpeedProfileCase(10, 9, 11, 1.4); + + EXPECT_EQ(1, case_num4); +} + +TEST(LCITacticalPluginTest, apply_accel_cruise_decel_speed_profile_test) +{ + std::shared_ptr wm = std::make_shared(); + LightControlledIntersectionTacticalPluginConfig config; + config.vehicle_accel_limit= 1; + config.vehicle_accel_limit_multiplier= 1; + config.vehicle_decel_limit= 1; + config.vehicle_decel_limit_multiplier= 1; + + LightControlledIntersectionTacticalPlugin lci(wm, config, [&](auto msg) {}); + + auto map = carma_wm::test::buildGuidanceTestMap(2, 2); + + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + + /** + * Total route length should be 100m + * + * |1203|1213|1223| + * | _ _ _ _ _| + * |1202| Ob |1222| + * | _ _ _ _ _| + * |1201|1211|1221| num = lanelet id hardcoded for easier testing + * | _ _ _ _ _| | = lane lines + * |1200|1210|1220| - - - = Lanelet boundary + * | | O = Default Obstacle + * **************** + * START_LINE + */ + + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + PointSpeedPair p; + std::vector points_and_target_speeds; + + // ACCEL-CRUISE START + + lci.speed_limit_ = 1; + double departure_speed = 1; + double start_dist = 0; + double end_dist = 5; + double remaining_dist = end_dist - start_dist; + double starting_speed = 0; + double remaining_time = 8; + int n = 11; + double step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = 5; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + + double speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // ACCEL-CRUISE END + + // CRUISE DECEL START + points_and_target_speeds = {}; + lci.speed_limit_ = 1; + departure_speed = 0.5; + start_dist = 0; + end_dist = 5; + remaining_dist = end_dist - start_dist; + starting_speed = 1; + remaining_time = 6; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = 5; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // CRUISE DECEL end + + // ACCEL DECEL START + points_and_target_speeds = {}; + lci.speed_limit_ = 1; + departure_speed = 0.5; + start_dist = 0; + end_dist = 5; + remaining_dist = end_dist - start_dist; + starting_speed = 0; + remaining_time = 10; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = 5; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // ACCEL DECEL END + + // ACCEL CRUISE DECEL START + points_and_target_speeds = {}; + lci.speed_limit_ = 1; + departure_speed = 0.8; + start_dist = 0; + end_dist = 5; + remaining_dist = end_dist - start_dist; + starting_speed = 0; + remaining_time = 8; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = 5; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // ACCEL CRUISE DECEL END + + // ONLY CRUISE START + points_and_target_speeds = {}; + lci.speed_limit_ = 1; + departure_speed = lci.speed_limit_; + start_dist = 0; + end_dist = 5; + remaining_dist = end_dist - start_dist; + starting_speed = lci.speed_limit_; + remaining_time = 5; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = lci.speed_limit_; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // ONLY CRUISE END + + // BUFFERED ACCEL CRUISE DECEL START + points_and_target_speeds = {}; + lci.speed_limit_ = 1; + departure_speed = 0.8; + start_dist = 1; + end_dist = 6; + starting_speed = 0; + remaining_time = 8; + remaining_dist = end_dist - start_dist; + n = 13; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = lci.speed_limit_; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // BUFFERED ACCEL CRUISE DECEL END + + // SACRIFICE START NEGATIVE ACCEL PART START + points_and_target_speeds = {}; + lci.speed_limit_ = 15; + departure_speed = 15; + start_dist = 0; + end_dist = 225; + starting_speed = 0; + remaining_time = 12.5; + remaining_dist = end_dist - start_dist; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = lci.speed_limit_; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // SACRIFICE START NEGATIVE ACCEL PART END + + // SACRIFICE START NEGATIVE ACCEL PART START + points_and_target_speeds = {}; + lci.speed_limit_ = 15; + departure_speed = 10; + start_dist = 0; + end_dist = 225; + starting_speed = 0; + remaining_time = 12.5; + remaining_dist = end_dist - start_dist; + n = 11; + step = end_dist / (n - 1); + for (auto i = 0; i < n; i++) + { + p.point = {0.5, i * step}; + p.speed = lci.speed_limit_; + points_and_target_speeds.push_back(p); + } + + ROS_DEBUG_STREAM("\n Estimated time: " << lci.calcEstimatedEntryTimeLeft(remaining_dist, starting_speed, departure_speed ) + << ", scheduled time: " << remaining_time); + speed_before_decel = lci.calcSpeedBeforeDecel(remaining_time, remaining_dist, starting_speed, departure_speed); + + lci.apply_accel_cruise_decel_speed_profile(wm, points_and_target_speeds, start_dist, end_dist, + remaining_time, starting_speed, speed_before_decel, departure_speed); + + EXPECT_NEAR(points_and_target_speeds.front().speed, starting_speed, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, departure_speed, 0.001); + + // SACRIFICE START NEGATIVE ACCEL PART END +} + +} + +// Run all the tests +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::Time::init(); + ROSCONSOLE_AUTOINIT; + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { // Change to Debug to enable debug logs + ros::console::notifyLoggerLevelsChanged(); + } + auto res = RUN_ALL_TESTS(); + + return res; +} \ No newline at end of file diff --git a/mobilitypath_publisher/CMakeLists.txt b/mobilitypath_publisher/CMakeLists.txt index 2836eb79c2..f76c2a9edf 100644 --- a/mobilitypath_publisher/CMakeLists.txt +++ b/mobilitypath_publisher/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs cav_msgs carma_utils + bsm_helper lanelet2_extension ) diff --git a/mobilitypath_publisher/include/mobilitypath_publisher.h b/mobilitypath_publisher/include/mobilitypath_publisher.h index 46005084da..9befc831c4 100644 --- a/mobilitypath_publisher/include/mobilitypath_publisher.h +++ b/mobilitypath_publisher/include/mobilitypath_publisher.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -113,13 +114,7 @@ namespace mobilitypath_publisher // sender's dynamic ID which is its BSM id in hex string std::string sender_bsm_id = "FFFF"; - std::string bsmIDtoString(cav_msgs::BSMCoreData bsm_core){ - std::string res = ""; - for (size_t i=0; istd_msgs cav_msgs carma_utils + bsm_helper lanelet2_extension diff --git a/mobilitypath_publisher/src/mobilitypath_publisher.cpp b/mobilitypath_publisher/src/mobilitypath_publisher.cpp index 9783a27cc5..a1735a444d 100644 --- a/mobilitypath_publisher/src/mobilitypath_publisher.cpp +++ b/mobilitypath_publisher/src/mobilitypath_publisher.cpp @@ -76,7 +76,7 @@ namespace mobilitypath_publisher cav_msgs::MobilityPath MobilityPathPublication::mobilityPathMessageGenerator(const cav_msgs::TrajectoryPlan& trajectory_plan) { cav_msgs::MobilityPath mobility_path_msg; - uint64_t millisecs =trajectory_plan.header.stamp.toNSec()/1000000; + uint64_t millisecs = ros::Time::now().toNSec()/1000000; mobility_path_msg.header = composeMobilityHeader(millisecs); if (!map_projector_) { @@ -94,7 +94,7 @@ namespace mobilitypath_publisher cav_msgs::MobilityHeader header; header.sender_id = sender_id; header.recipient_id = recipient_id; - header.sender_bsm_id = bsmIDtoString(bsm_core_); + header.sender_bsm_id = BSMHelper::BSMHelper::bsmIDtoString(bsm_core_.id); // random GUID that identifies this particular plan for future reference header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); header.timestamp = time; //time in millisecond @@ -122,6 +122,7 @@ namespace mobilitypath_publisher offset.offset_z = (int16_t)(new_point.ecef_z - prev_point.ecef_z); prev_point = new_point; traj.offsets.push_back(offset); + if( i >= 60 ){ break;}; } } diff --git a/object_detection_tracking/CMakeLists.txt b/object_detection_tracking/CMakeLists.txt index 2659339792..2b1c19668e 100644 --- a/object_detection_tracking/CMakeLists.txt +++ b/object_detection_tracking/CMakeLists.txt @@ -52,7 +52,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include # LIBRARIES object_detection_tracking - CATKIN_DEPENDS autoware_msgs cav_msgs roscpp carma_utils motion_predict geometry_msgs roscpp rospy std_msgs tf2_msgs wgs84_utils message_filters novatel_gps_msgs carma_utils tf2_geometry_msgs + CATKIN_DEPENDS autoware_msgs cav_msgs roscpp carma_utils motion_predict geometry_msgs roscpp rospy std_msgs tf2_msgs wgs84_utils message_filters carma_utils tf2_geometry_msgs # DEPENDS system_lib ) diff --git a/object_detection_tracking/package.xml b/object_detection_tracking/package.xml index 7af3ef2377..99aed859fa 100644 --- a/object_detection_tracking/package.xml +++ b/object_detection_tracking/package.xml @@ -31,7 +31,6 @@ cav_srvs wgs84_utils message_filters - novatel_gps_msgs carma_utils tf2_geometry_msgs diff --git a/platooning_strategic/include/platoon_manager.hpp b/platooning_strategic/include/platoon_manager.hpp deleted file mode 100644 index 334c48787a..0000000000 --- a/platooning_strategic/include/platoon_manager.hpp +++ /dev/null @@ -1,236 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - - - -namespace platoon_strategic -{ - struct PlatoonPlan { - - bool valid; - long planStartTime; - std::string planId; - std::string peerId; - // PlatoonPlan(){} ; - PlatoonPlan():valid(false), planStartTime(0), planId(""), peerId("") {} ; - PlatoonPlan(bool valid, long planStartTime, std::string planId, std::string peerId): - valid(valid), planStartTime(planStartTime), planId(planId), peerId(peerId) {} - }; - - /** - * A response to an MobilityRequest message. - * ACK - indicates that the plugin accepts the MobilityRequest and will handle making any adjustments needed to avoid a conflict - * NACK - indicates that the plugin rejects the MobilityRequest and would suggest the other vehicle replan - * NO_RESPONSE - indicates that the plugin is indifferent but sees no conflict - */ - - enum MobilityRequestResponse { - ACK, - NACK, - NO_RESPONSE - }; - - enum PlatoonState{ - STANDBY, - LEADERWAITING, - LEADER, - CANDIDATEFOLLOWER, - FOLLOWER - }; - - - struct PlatoonMember{ - // Static ID is permanent ID for each vehicle - std::string staticId; - // Current BSM Id for each CAV - std::string bsmId; - // Vehicle real time command speed in m/s - double commandSpeed; - // Actual vehicle speed in m/s - double vehicleSpeed; - // Vehicle current down track distance on the current route in m - double vehiclePosition; - // The local time stamp when the host vehicle update any informations of this member - long timestamp; - // PlatoonMember(){}; - PlatoonMember(): staticId(""), bsmId(""), commandSpeed(0.0), vehicleSpeed(0.0), vehiclePosition(0.0), timestamp(0) {} - PlatoonMember(std::string staticId, std::string bsmId, double commandSpeed, double vehicleSpeed, double vehiclePosition, long timestamp): staticId(staticId), - bsmId(bsmId), commandSpeed(commandSpeed), vehicleSpeed(vehicleSpeed), vehiclePosition(vehiclePosition), timestamp(timestamp) {} - }; - - - - class PlatoonManager - { - public: - - PlatoonManager(); - - // todo initialize as empty - std::vector platoon{}; - - - - // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; - - double current_downtrack_didtance_ = 0; - - // wm listener pointer and pointer to the actual wm object - // std::shared_ptr wml_; - // carma_wm::WorldModelConstPtr wm_; - - - - void memberUpdates(const std::string& senderId,const std::string& platoonId,const std::string& senderBsmId,const std::string& params, const double& DtD); - - /** - * Given any valid platooning mobility STATUS operation parameters and sender staticId, - * in leader state this method will add/updates the information of platoon member if it is using - * the same platoon ID, in follower state this method will updates the vehicle information who - * is in front of the subject vehicle or update platoon id if the leader is join another platoon - * @param senderId sender ID for the current info - * @param platoonId sender platoon id - * @param senderBsmId sender BSM ID - * @param params strategy params from STATUS message in the format of "CMDSPEED:xx,DOWNTRACK:xx,SPEED:xx" - **/ - void updatesOrAddMemberInfo(std::string senderId, std::string senderBsmId, double cmdSpeed, double dtDistance, double curSpeed); - - int getTotalPlatooningSize() const; - - - PlatoonMember getLeader(); - - /** - * This is the implementation of all predecessor following (APF) algorithm for leader - * selection in a platoon. This function will recognize who is acting as the current leader - * of the subject vehicle. The current leader of the subject vehicle will be any ONE of - * the vehicles in front of it. Having a vehicle further downstream function as the leader - * is more efficient and more stable; however, having a vehicle closer to the subject vehicle - * function as the leader is safer. For this reason, the subject vehicle will monitor - * all time headways between every single set of consecutive vehicles starting from itself - * to the leader. If the time headways are within some safe thresholds then vehicles further - * downstream may function as the leader. Otherwise, for the sake of safety, vehicles closer - * to the subject vehicle, potentially even the predecessor, will function as the leader. - * @return the index of the leader in the platoon list - */ - - int allPredecessorFollowing(); - - void changeFromFollowerToLeader(); - void changeFromLeaderToFollower(std::string newPlatoonId); - int getNumberOfVehicleInFront(); - double getCurrentPlatoonLength(); - double getPlatoonRearDowntrackDistance(); - - double getDistanceFromRouteStart() const; - double getDistanceToFrontVehicle(); - double getCurrentSpeed() const; - double getCommandSpeed(); - double getCurrentDowntrackDistance() const; - - - int platoonSize = 2; - std::string leaderID = ""; - std::string currentPlatoonID = "test_plan"; - bool isFollower = false; - - double current_speed_ = 0; - double command_speed_ = 0; - - PlatoonState current_platoon_state = PlatoonState::STANDBY; - - PlatoonPlan current_plan; - - std::string targetLeaderId = "2"; - - std::string HostMobilityId = "hostid"; - - - private: - - double maxAllowedJoinTimeGap = 15.0; - double maxAllowedJoinGap = 90; - int maxPlatoonSize = 10; - double vehicleLength = 5.0; - int infoMessageInterval = 200; - - std::string targetPlatoonId; - std::string OPERATION_INFO_TYPE = "INFO"; - std::string OPERATION_STATUS_TYPE = "STATUS"; - std::string JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"; - std::string MOBILITY_STRATEGY = "Carma/Platooning"; - - - - - - double minGap_ = 22.0; - double maxGap_ = 32.0; - std::string previousFunctionalLeaderID_ = ""; - int previousFunctionalLeaderIndex_ = -1; - - double maxSpacing_ = 4.0; - double minSpacing_ = 3.9; - double lowerBoundary_ = 1.6; - double upperBoundary_ = 1.7 ; - - double vehicleLength_ = 5.0; // m - - double gapWithFront_ = 0.0; - - double downtrack_progress_ = 0; - - - std::string algorithmType_ = "APF_ALGORITHM"; - - bool insufficientGapWithPredecessor(double distanceToFrontVehicle); - std::vector calculateTimeHeadway(std::vector downtrackDistance, std::vector speed) const; - int determineLeaderBasedOnViolation(std::vector timeHeadways); - - // helper method for APF algorithm - int findLowerBoundaryViolationClosestToTheHostVehicle(std::vector timeHeadways) const; - - // helper method for APF algorithm - int findMaximumSpacingViolationClosestToTheHostVehicle(std::vector timeHeadways) const; - - std::vector getTimeHeadwayFromIndex(std::vector timeHeadways, int start) const; - - - - - - - }; -} \ No newline at end of file diff --git a/platooning_strategic/include/platoon_strategic.h b/platooning_strategic/include/platoon_strategic.h index 4a273af52e..ac07e3085b 100644 --- a/platooning_strategic/include/platoon_strategic.h +++ b/platooning_strategic/include/platoon_strategic.h @@ -223,6 +223,9 @@ namespace platoon_strategic // ECEF position of the host vehicle cav_msgs::LocationECEF pose_ecef_point_; + // Flag to enable/disable platooning plugin + bool platooning_enabled_ = false; + private: diff --git a/platooning_strategic/src/platoon_strategic.cpp b/platooning_strategic/src/platoon_strategic.cpp index c46f67b815..b8b1a55da9 100644 --- a/platooning_strategic/src/platoon_strategic.cpp +++ b/platooning_strategic/src/platoon_strategic.cpp @@ -70,6 +70,12 @@ namespace platoon_strategic cav_msgs::PlatooningInfo platoon_status = composePlatoonInfoMsg(); platooning_info_publisher_(platoon_status); + if (!platooning_enabled_) + { + pm_.current_platoon_state = PlatoonState::STANDBY; + return true; + } + return true; } @@ -250,7 +256,7 @@ namespace platoon_strategic ROS_WARN_STREAM("Platoon size 1 so Empty maneuver sent"); } - if (pm_.current_platoon_state == PlatoonState::STANDBY) + if (pm_.current_platoon_state == PlatoonState::STANDBY && platooning_enabled_) { pm_.current_platoon_state = PlatoonState::LEADER; pm_.currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()()); diff --git a/platooning_strategic/test/test_platoon_manager.cpp b/platooning_strategic/test/test_platoon_manager.cpp index cfd2cd62e6..3d1b8fd11a 100644 --- a/platooning_strategic/test/test_platoon_manager.cpp +++ b/platooning_strategic/test/test_platoon_manager.cpp @@ -39,6 +39,20 @@ TEST(PlatoonManagerTest, test_construct) } +TEST(PlatoonManagerTest, test_enable_platooning) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + plugin.pm_.current_platoon_state = PlatoonState::LEADER; + plugin.platooning_enabled_ = false; + plugin.onSpin(); + EXPECT_EQ(plugin.pm_.current_platoon_state, PlatoonState::STANDBY); + +} + + TEST(PlatoonManagerTest, test_ecef_encode) { ros::Time::init(); diff --git a/sci_strategic_plugin/CMakeLists.txt b/sci_strategic_plugin/CMakeLists.txt index 2302c2acd0..10057b62b2 100644 --- a/sci_strategic_plugin/CMakeLists.txt +++ b/sci_strategic_plugin/CMakeLists.txt @@ -15,6 +15,7 @@ set(PKG_CATKIN_DEPS cav_srvs cav_msgs carma_utils + bsm_helper carma_wm lanelet2_core lanelet2_traffic_rules diff --git a/sci_strategic_plugin/config/parameters.yaml b/sci_strategic_plugin/config/parameters.yaml index ab11ed7287..a9adadc44e 100644 --- a/sci_strategic_plugin/config/parameters.yaml +++ b/sci_strategic_plugin/config/parameters.yaml @@ -1,15 +1,24 @@ # Double: multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities -vehicle_decel_limit_multiplier : 0.50 +vehicle_decel_limit_multiplier : 0.65 # Double: multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabilities -vehicle_accel_limit_multiplier : 0.50 +vehicle_accel_limit_multiplier : 0.65 # Double: A buffer before of the stopping location which will still be considered a valid stop. Units in meters -stop_line_buffer : 2.0 +stop_line_buffer : 3.5 # Double: Update time interval of carma streets. Units in s delta_t : 1.0 +# Double: Update min_gap. Units in m +min_gap : 3.0 + +# Double: Minimum vehicle reaction time. Units in s +reaction_time : 5.0 + +# Double: The distance vehicle drives to be considered out of intersections +intersection_exit_zone_length : 6.0 + # String: The name to use for this plugin during comminications with the arbitrator strategic_plugin_name : SCIStrategicPlugin diff --git a/sci_strategic_plugin/include/sci_strategic_plugin.h b/sci_strategic_plugin/include/sci_strategic_plugin.h index 94cfb80b3b..45a349ee3c 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin.h +++ b/sci_strategic_plugin/include/sci_strategic_plugin.h @@ -24,17 +24,25 @@ #include #include #include +#include #include #include #include #include #include #include +#include +#include #include "sci_strategic_plugin_config.h" namespace sci_strategic_plugin { + enum TurnDirection { + Straight, + Right, + Left + }; /** * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin @@ -101,6 +109,14 @@ class SCIStrategicPlugin * \return cav_msgs::Plugin The plugin discovery message */ cav_msgs::Plugin getDiscoveryMsg() const; + + /** + * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery and mobility operation msgs. + * + * \return True if the node should continue running. False otherwise + */ + bool onSpin(); + /** * \brief callback function for mobility operation @@ -134,11 +150,11 @@ class SCIStrategicPlugin std::vector lane_ids); cav_msgs::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, - const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id, - ros::Time start_time, ros::Time end_time) const; + const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id, + double stopping_accel, ros::Time start_time, ros::Time end_time) const; cav_msgs::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, - double target_speed, ros::Time start_time, ros::Time end_time, + double target_speed, ros::Time start_time, ros::Time end_time, TurnDirection turn_direction, const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id) const; /** @@ -258,37 +274,52 @@ class SCIStrategicPlugin * * \param stop_time time duration to stop in s * - * \param float_metadata_list metadata vector for storing speed profile parameters + * \return deceleration value for case three * */ - void caseThreeSpeedProfile(double stop_dist, double current_speed, double stop_time, std::vector* float_metadata_list) const; + double caseThreeSpeedProfile(double stop_dist, double current_speed, double stop_time) const; /** * \brief Generates Mobility Operation messages + * + * \return mobility operation msg for status and intent */ - void generateMobilityOperation(); + cav_msgs::MobilityOperation generateMobilityOperation(); /** * \brief BSM callback function */ void BSMCb(const cav_msgs::BSMConstPtr& msg); + + /** + * \brief Determine the turn direction at intersection + * + * \param lanelets_list List of lanelets crossed around the intersection area + * + * \return turn direction in format of straight, left, right + * + */ + TurnDirection getTurnDirectionAtIntersection(std::vector lanelets_list); ////////// VARIABLES /////////// // CARMA Streets Variakes // timestamp for msg received from carma streets - uint32_t street_msg_timestamp_ = 0.0; + unsigned long long street_msg_timestamp_ = 0; // scheduled stop time - uint32_t scheduled_stop_time_ = 0.0; + unsigned long long scheduled_stop_time_ = 0; // scheduled enter time - uint32_t scheduled_enter_time_ = 0.0; + unsigned long long scheduled_enter_time_ = 0; // scheduled depart time - uint32_t scheduled_depart_time_ = 0.0; - // scheduled latest depart time - uint32_t scheduled_latest_depart_time_ = 0.0; + unsigned long long scheduled_depart_time_ = 0; + // scheduled latest depart position + uint32_t scheduled_departure_position_ = std::numeric_limits::max(); // flag to show if the vehicle is allowed in intersection bool is_allowed_int_ = false; + TurnDirection intersection_turn_direction_ = TurnDirection::Straight; + bool vehicle_engaged_ = false; + // approximate speed limit double speed_limit_ = 100.0; @@ -298,8 +329,11 @@ class SCIStrategicPlugin bool approaching_stop_controlled_interction_ = false; ros::Publisher mobility_operation_pub; + ros::Publisher plugin_discovery_pub; - std::vector bsm_id; + std::string bsm_id_ = "default_bsm_id"; + uint8_t bsm_msg_count_ = 0; + uint16_t bsm_sec_mark_ = 0; private: //! World Model pointer diff --git a/sci_strategic_plugin/include/sci_strategic_plugin_config.h b/sci_strategic_plugin/include/sci_strategic_plugin_config.h index 1e439e5746..04125ed664 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin_config.h +++ b/sci_strategic_plugin/include/sci_strategic_plugin_config.h @@ -26,16 +26,16 @@ struct SCIStrategicPluginConfig double vehicle_decel_limit = 2.0; //! A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities - double vehicle_decel_limit_multiplier = 0.75; + double vehicle_decel_limit_multiplier = 0.5; //! The maximum allowable vehicle acceleration limit in m/s double vehicle_accel_limit = 2.0; //! A multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabilities - double vehicle_accel_limit_multiplier = 0.75; + double vehicle_accel_limit_multiplier = 0.5; //! A buffer infront of the stopping location which will still be considered a valid stop - double stop_line_buffer = 2.0; + double stop_line_buffer = 3.0; //! The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the whole plan double min_maneuver_planning_period = 15.1; @@ -43,11 +43,23 @@ struct SCIStrategicPluginConfig // Double: Approximate update time interval of carma streets double delta_t = 1.0; + // Double: Minimum inter-vehicle gap + double min_gap = 10.0; + + // Double: Length od the vehicle + double veh_length = 4.0; + + // Double: Vehicle reaction time to a received schedule in seconds (approximate value, only used for communication with the schedule) + double reaction_time = 5.0; + + // Double: The distance vehicle drives to be considered out of intersections + double intersection_exit_zone_length = 15.0; + //! The name to use for this plugin during comminications with the arbitrator std::string strategic_plugin_name = "SCIStrategicPlugin"; //! The name of the tactical plugin to use for Lane Following trajectory planning - std::string lane_following_plugin_name = "SCITacticalPlugin"; + std::string lane_following_plugin_name = "StopControlledIntersectionTacticalPlugin"; //! The name of the plugin to use for stop and wait trajectory planning std::string stop_and_wait_plugin_name = "StopAndWaitPlugin"; diff --git a/sci_strategic_plugin/package.xml b/sci_strategic_plugin/package.xml index a8050b081e..41aa1b1c26 100644 --- a/sci_strategic_plugin/package.xml +++ b/sci_strategic_plugin/package.xml @@ -25,6 +25,7 @@ cav_srvs cav_msgs carma_utils + bsm_helper carma_wm lanelet2_core lanelet2_traffic_rules diff --git a/sci_strategic_plugin/src/main.cpp b/sci_strategic_plugin/src/main.cpp index 811124ec83..ae011909f0 100644 --- a/sci_strategic_plugin/src/main.cpp +++ b/sci_strategic_plugin/src/main.cpp @@ -30,9 +30,6 @@ int main(int argc, char** argv) ros::CARMANodeHandle nh; ros::CARMANodeHandle pnh("~"); - // Create publishers - ros::Publisher plugin_discovery_pub = nh.advertise("plugin_discovery", 1); - // Initialize world model carma_wm::WMListener wml; @@ -40,16 +37,20 @@ int main(int argc, char** argv) sci_strategic_plugin::SCIStrategicPluginConfig config; // clang-format off - pnh.param("/vehicle_deceleration_limit", config.vehicle_decel_limit, config.vehicle_decel_limit); + pnh.param("vehicle_decel_limit_multiplier", config.vehicle_decel_limit_multiplier, config.vehicle_decel_limit_multiplier); - pnh.param("/vehicle_acceleration_limit", config.vehicle_decel_limit, config.vehicle_accel_limit); - pnh.param("vehicle_accel_limit_multiplier", config.vehicle_decel_limit_multiplier, config.vehicle_accel_limit_multiplier); + pnh.param("vehicle_accel_limit_multiplier", config.vehicle_accel_limit_multiplier, config.vehicle_accel_limit_multiplier); pnh.param("stop_line_buffer", config.stop_line_buffer, config.stop_line_buffer); pnh.param("delta_t", config.delta_t, config.delta_t); + pnh.param("min_gap", config.min_gap, config.min_gap); + pnh.param("reaction_time", config.reaction_time, config.reaction_time); pnh.param("strategic_plugin_name", config.strategic_plugin_name, config.strategic_plugin_name); pnh.param("lane_following_plugin_name", config.lane_following_plugin_name, config.lane_following_plugin_name); pnh.param("intersection_transit_plugin_name", config.intersection_transit_plugin_name, config.intersection_transit_plugin_name); pnh.getParam("/vehicle_id", config.vehicle_id); + pnh.getParam("/vehicle_length", config.veh_length); + pnh.getParam("/vehicle_deceleration_limit", config.vehicle_decel_limit); + pnh.getParam("/vehicle_acceleration_limit", config.vehicle_accel_limit); // clang-format on // Construct plugin @@ -65,17 +66,21 @@ int main(int argc, char** argv) // Current Pose Subscriber ros::Subscriber current_pose_sub = nh.subscribe("current_pose", 1, &sci_strategic_plugin::SCIStrategicPlugin::currentPoseCb, &plugin); - ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &sci_strategic_plugin::SCIStrategicPlugin::BSMCb, &plugin); - ros::Timer discovery_pub_timer = - nh.createTimer(ros::Duration(ros::Rate(10.0)), [&plugin, &plugin_discovery_pub](const auto&) { - plugin_discovery_pub.publish(plugin.getDiscoveryMsg()); - }); - - - plugin.mobility_operation_pub = nh.advertise("outgoing_mobility_operation", 5); + // Create publishers + ros::Publisher plugin_discovery_pub = nh.advertise("plugin_discovery", 1); + plugin.mobility_operation_pub = nh.advertise("outgoing_mobility_operation", 1); + ros::Timer pub_timer_ = nh.createTimer( + ros::Duration(ros::Rate(10.0)), + [&plugin, &plugin_discovery_pub](const auto&) { + plugin_discovery_pub.publish(plugin.getDiscoveryMsg()); + plugin.onSpin(); + }); + + + ROS_INFO("Successfully launched node."); // Start ros::CARMANodeHandle::spin(); diff --git a/sci_strategic_plugin/src/sci_strategic_plugin.cpp b/sci_strategic_plugin/src/sci_strategic_plugin.cpp index dff99369e6..51ac1c0548 100644 --- a/sci_strategic_plugin/src/sci_strategic_plugin.cpp +++ b/sci_strategic_plugin/src/sci_strategic_plugin.cpp @@ -86,63 +86,76 @@ void SCIStrategicPlugin::mobilityOperationCb(const cav_msgs::MobilityOperationCo { if (msg->strategy == stop_controlled_intersection_strategy_) { + ROS_DEBUG_STREAM("Received Schedule message with id: " << msg->header.plan_id); approaching_stop_controlled_interction_ = true; - if (msg->strategy_params != previous_strategy_params_) - { - parseStrategyParams(msg->strategy_params); - } + ROS_DEBUG_STREAM("Approaching Stop Controlled Intersection: " << approaching_stop_controlled_interction_); + + if (msg->header.recipient_id == config_.vehicle_id) + { + street_msg_timestamp_ = msg->header.timestamp; + ROS_DEBUG_STREAM("street_msg_timestamp_: " << street_msg_timestamp_); + parseStrategyParams(msg->strategy_params); + } previous_strategy_params_ = msg->strategy_params; - - generateMobilityOperation(); } } void SCIStrategicPlugin::BSMCb(const cav_msgs::BSMConstPtr& msg) { - bsm_id = msg->core_data.id; + std::vector bsm_id_vec = msg->core_data.id; + bsm_id_ = BSMHelper::BSMHelper::bsmIDtoString(bsm_id_vec); + bsm_msg_count_ = msg->core_data.msg_count; + bsm_sec_mark_ = msg->core_data.sec_mark; } void SCIStrategicPlugin::currentPoseCb(const geometry_msgs::PoseStampedConstPtr& msg) { geometry_msgs::PoseStamped pose_msg = geometry_msgs::PoseStamped(*msg.get()); - lanelet::BasicPoint2d current_loc(pose_msg.pose.position.x, pose_msg.pose.position.y); - current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack; - ROS_DEBUG_STREAM("Downtrack from current pose: " << current_downtrack_); + if (vehicle_engaged_) + { + lanelet::BasicPoint2d current_loc(pose_msg.pose.position.x, pose_msg.pose.position.y); + current_downtrack_ = wm_->routeTrackPos(current_loc).downtrack; + ROS_DEBUG_STREAM("Downtrack from current pose: " << current_downtrack_); + } + + } void SCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) { - std::istringstream strategy_params_ss(strategy_params); - boost::property_tree::ptree parser; - boost::property_tree::ptree child; - boost::property_tree::json_parser::read_json(strategy_params_ss, parser); - child = parser.get_child("schedule_plan"); - for(const auto& p : child) - { - if (p.first == "metadata") - { - street_msg_timestamp_ = p.second.get("timestamp"); - } - if (p.first == "payload" && p.second.get("veh_id") == config_.vehicle_id) - { - // parse stop time in ms - scheduled_stop_time_ = p.second.get("est_stop_t"); - ROS_DEBUG_STREAM("scheduled_stop_time_: " << scheduled_stop_time_); - - scheduled_enter_time_ = p.second.get("est_enter_t"); - ROS_DEBUG_STREAM("scheduled_enter_time_: " << scheduled_enter_time_); - - scheduled_depart_time_ = p.second.get("est_depart_t"); - ROS_DEBUG_STREAM("scheduled_depart_time_: " << scheduled_depart_time_); - - scheduled_latest_depart_time_ = p.second.get("latest_depart_p"); - ROS_DEBUG_STREAM("scheduled_latest_depart_time_: " << scheduled_latest_depart_time_); + // sample strategy_params: "st:1634067044,et:1634067059, dt:1634067062.3256602,dp:2,,access: 0" + std::string params = strategy_params; + std::vector inputsParams; + boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); + + std::vector st_parsed; + boost::algorithm::split(st_parsed, inputsParams[0], boost::is_any_of(":")); + scheduled_stop_time_ = std::stoull(st_parsed[1]); + ROS_DEBUG_STREAM("scheduled_stop_time_: " << scheduled_stop_time_); + + std::vector et_parsed; + boost::algorithm::split(et_parsed, inputsParams[1], boost::is_any_of(":")); + scheduled_enter_time_ = std::stoull(et_parsed[1]); + ROS_DEBUG_STREAM("scheduled_enter_time_: " << scheduled_enter_time_); + + std::vector dt_parsed; + boost::algorithm::split(dt_parsed, inputsParams[2], boost::is_any_of(":")); + scheduled_depart_time_ = std::stoull(dt_parsed[1]); + ROS_DEBUG_STREAM("scheduled_depart_time_: " << scheduled_depart_time_); + + + std::vector dp_parsed; + boost::algorithm::split(dp_parsed, inputsParams[3], boost::is_any_of(":")); + scheduled_departure_position_ = std::stoi(dp_parsed[1]); + ROS_DEBUG_STREAM("scheduled_departure_position_: " << scheduled_departure_position_); + + std::vector access_parsed; + boost::algorithm::split(access_parsed, inputsParams[4], boost::is_any_of(":")); + int access = std::stoi(access_parsed[1]); + is_allowed_int_ = (access == 1); + ROS_DEBUG_STREAM("is_allowed_int_: " << is_allowed_int_); - is_allowed_int_ = p.second.get("is_allowed_int"); - ROS_DEBUG_STREAM("is_allowed_int: " << is_allowed_int_); - } - } } int SCIStrategicPlugin::determineSpeedProfileCase(double stop_dist, double current_speed, double schedule_stop_time, double speed_limit) @@ -158,6 +171,7 @@ int SCIStrategicPlugin::determineSpeedProfileCase(double stop_dist, double curre else { double speed_before_stop = calcSpeedBeforeDecel(estimated_stop_time, stop_dist, current_speed); + ROS_DEBUG_STREAM("speed_before_stop: " << speed_before_stop); if (speed_before_stop < speed_limit) { case_num = 1; @@ -217,6 +231,9 @@ std::vector SCIStrategicPlugin::getLaneletsBetweenWithExc bool SCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp) { + ROS_DEBUG("In Maneuver callback..."); + vehicle_engaged_ = true; + if (!wm_->getRoute()) { ROS_ERROR_STREAM("Could not plan maneuvers as route was not available"); @@ -226,29 +243,96 @@ bool SCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav if (!approaching_stop_controlled_interction_) { resp.new_plan.maneuvers = {}; - ROS_WARN_STREAM("Not approaching stop-controlled itnersection so no maneuvers"); + ROS_WARN_STREAM("Not approaching stop-controlled intersection so no maneuvers"); return true; } + bool is_empty_schedule_msg = (scheduled_depart_time_ == 0 && scheduled_stop_time_ == 0 && scheduled_enter_time_ == 0); + if (is_empty_schedule_msg) + { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("Receiving empty schedule message"); + return true; + } + + ROS_DEBUG("Planning for intersection..."); + ROS_DEBUG("Finding car information"); // Extract vehicle data from request VehicleState current_state = extractInitialState(req); // Get current traffic light information - ROS_DEBUG("\n\nFinding intersecction information"); + ROS_DEBUG("\n\nFinding intersection information"); auto stop_intersection_list = wm_->getIntersectionsAlongRoute({ req.veh_x, req.veh_y }); + bool no_intersections = stop_intersection_list.empty(); + if (no_intersections) + { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("There are no stop controlled intersections in the map"); + return true; + } + auto nearest_stop_intersection = stop_intersection_list.front(); - double stop_intersection_down_track = - wm_->routeTrackPos(nearest_stop_intersection->stopLines().front().front().basicPoint2d()).downtrack; - double distance_to_stopline = stop_intersection_down_track - current_downtrack_; - std::cout << "distance_to_stopline " << distance_to_stopline << std::endl; + bool no_stop_lines = nearest_stop_intersection->stopLines().empty(); + if (no_stop_lines) + { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("There are no stop lines on the closest stop-controlled intersections in the map"); + return true; + } + + + // extract the intersection stop line information + std::vector stop_lines; + for (auto l : nearest_stop_intersection->stopLines()) + { + //TODO: temp use veh_length here until planning stack is updated with front bumper pos + double stop_bar_dtd = wm_->routeTrackPos(l.front().basicPoint2d()).downtrack - config_.veh_length; + stop_lines.push_back(stop_bar_dtd); + } + std::sort(stop_lines.begin(), stop_lines.end()); + + double stop_intersection_down_track = stop_lines.front(); + + ROS_DEBUG_STREAM("stop_intersection_down_track " << stop_intersection_down_track); + + + double distance_to_stopline = stop_intersection_down_track - current_downtrack_ - config_.stop_line_buffer; + ROS_DEBUG_STREAM("distance_to_stopline " << distance_to_stopline); - if (distance_to_stopline >= 0) + if (distance_to_stopline < -config_.intersection_exit_zone_length) { - if (distance_to_stopline > config_.stop_line_buffer) - { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("Already passed intersection, sending empty maneuvers"); + return true; + } + + + double intersection_end_downtrack = stop_lines.back(); + // Identify the lanelets of the intersection + std::vector intersection_lanelets = + getLaneletsBetweenWithException(stop_intersection_down_track, intersection_end_downtrack, true, true); + + // find the turn direction at intersection: + + intersection_turn_direction_ = getTurnDirectionAtIntersection(intersection_lanelets); + + uint32_t base_time = street_msg_timestamp_; + + bool time_to_approach_int = int((scheduled_stop_time_) - (street_msg_timestamp_))>0; + ROS_DEBUG_STREAM("time_to_approach_int " << time_to_approach_int); + + cav_msgs::Maneuver maneuver_planned; + + + if (time_to_approach_int) + { + auto tmp = (scheduled_stop_time_) - (street_msg_timestamp_); + ROS_DEBUG_STREAM("tmp " << tmp); + double time_to_schedule_stop = (tmp)/1000.0; + ROS_DEBUG_STREAM("time_to_schedule_stop " << time_to_schedule_stop); // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = getLaneletsBetweenWithException(current_downtrack_, stop_intersection_down_track, true, true); @@ -257,59 +341,192 @@ bool SCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav speed_limit_ = findSpeedLimit(crossed_lanelets.front()); // lane following to intersection - double time_to_schedule_stop = (scheduled_stop_time_ - street_msg_timestamp_)*1000.0; - int case_num = determineSpeedProfileCase(stop_intersection_down_track, current_state.speed, time_to_schedule_stop, speed_limit_); + + ROS_DEBUG_STREAM("time_to_schedule_stop " << time_to_schedule_stop); + + double desired_deceleration = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + + + double safe_distance = pow(current_state.speed, 2)/(2*desired_deceleration); + ROS_DEBUG_STREAM("safe_distance: " << safe_distance); + + if (distance_to_stopline - safe_distance > config_.stop_line_buffer) + { + int case_num = determineSpeedProfileCase(distance_to_stopline , current_state.speed, time_to_schedule_stop, speed_limit_); + ROS_DEBUG_STREAM("case_num: " << case_num); + + if (case_num < 3) + { + maneuver_planned = composeLaneFollowingManeuverMessage( + case_num, current_state.downtrack, stop_intersection_down_track, current_state.speed, 0.0, + current_state.stamp, time_to_schedule_stop, + lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); })); + resp.new_plan.maneuvers.push_back(maneuver_planned); + } + else + { + ROS_DEBUG_STREAM("Decelerating to stop at the intersection"); + + // at the stop line or decelerating to stop line + // stop and wait maneuver + + std::vector crossed_lanelets = + getLaneletsBetweenWithException(current_downtrack_, stop_intersection_down_track, true, true); + + + double stopping_accel = caseThreeSpeedProfile(distance_to_stopline, current_state.speed, time_to_schedule_stop); + stopping_accel = std::max(-stopping_accel, desired_deceleration); + ROS_DEBUG_STREAM("used deceleration for case three: " << stopping_accel); + + maneuver_planned = composeStopAndWaitManeuverMessage( + current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(), + crossed_lanelets[0].id(), stopping_accel, current_state.stamp, + current_state.stamp + ros::Duration(time_to_schedule_stop)); + + resp.new_plan.maneuvers.push_back(maneuver_planned); + + } + } + else + { + ROS_DEBUG_STREAM("Too close to the intersection, constant deceleration to stop"); + + maneuver_planned = composeStopAndWaitManeuverMessage( + current_state.downtrack, stop_intersection_down_track, current_state.speed, crossed_lanelets[0].id(), + crossed_lanelets[0].id(), desired_deceleration, current_state.stamp, + current_state.stamp + ros::Duration(time_to_schedule_stop)); + } + } - resp.new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage( - case_num, current_downtrack_, stop_intersection_down_track, current_state.speed, 0.0, - current_state.stamp, time_to_schedule_stop, - lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); }))); - } - else - { - // at the stop line - // stop and wait maneuver - auto stop_line_lanelet = nearest_stop_intersection->lanelets().front(); - double stop_duration = (scheduled_enter_time_ - scheduled_stop_time_)*1000.0; - ROS_DEBUG_STREAM("Planning stop and wait maneuver"); - resp.new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( - current_downtrack_, stop_intersection_down_track, current_state.speed, stop_line_lanelet.id(), - stop_line_lanelet.id(), current_state.stamp, - current_state.stamp + ros::Duration(stop_duration))); + bool time_to_reach_stopline = int((street_msg_timestamp_) - (scheduled_stop_time_)) >= 0; + ROS_DEBUG_STREAM("time_to_reach_stopline: " << time_to_reach_stopline); + bool not_time_to_intersection_traverse = int((street_msg_timestamp_) - (scheduled_enter_time_)) < 0; + ROS_DEBUG_STREAM("not_time_to_intersection_traverse: " << not_time_to_intersection_traverse); + bool time_to_stop_at_stopline = (time_to_reach_stopline && not_time_to_intersection_traverse); + ROS_DEBUG_STREAM("time_to_stop_at_stopline: " << time_to_stop_at_stopline); - } + if (time_to_stop_at_stopline) + { + base_time = std::max(scheduled_stop_time_, street_msg_timestamp_); + double stop_duration = int(scheduled_enter_time_ - base_time)/1000; + ROS_DEBUG_STREAM("stop_duration: " << stop_duration); + ROS_DEBUG_STREAM("Planning stop and wait maneuver"); + double stopping_accel = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + auto stop_line_lanelet = nearest_stop_intersection->lanelets().front(); + + maneuver_planned = composeStopAndWaitManeuverMessage( + current_state.downtrack, stop_intersection_down_track, current_state.speed, stop_line_lanelet.id(), + stop_line_lanelet.id(), stopping_accel, current_state.stamp, + current_state.stamp + ros::Duration(stop_duration)); + } + + if (!is_allowed_int_) + { + ROS_DEBUG_STREAM("Vehicle at stop line, waiting for access to intersection"); + // vehicle is at the intersection but it is not allowed access, so it must wait + // arbitrary parameters + double stop_acc = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + double stop_duration = 999;//std::numeric_limits::max(); + + ROS_DEBUG_STREAM("current_downtrack_: " << current_downtrack_); + ROS_DEBUG_STREAM("current state dtd: " << current_state.downtrack); + ROS_DEBUG_STREAM("stop_intersection_down_track dtd: " << stop_intersection_down_track); + + maneuver_planned = composeStopAndWaitManeuverMessage( + current_state.downtrack, stop_intersection_down_track, current_state.speed, current_state.lane_id, + current_state.lane_id, stop_acc, current_state.stamp, + current_state.stamp + ros::Duration(stop_duration)); } - else + + bool time_for_crossing = int((street_msg_timestamp_) - (scheduled_enter_time_)) >= 0; + // time to cross intersection + + ROS_DEBUG_STREAM("time for crossing? " << time_for_crossing); + + if (time_for_crossing && is_allowed_int_) { - // Passed the stop line - // Intersection transit maneuver + ROS_DEBUG_STREAM("street_msg_timestamp_ - scheduled_enter_time_ = " << street_msg_timestamp_ - scheduled_enter_time_); + + ROS_DEBUG_STREAM("Vehicle is crossing the intersection"); + // Passing the stop line // Compose intersection transit maneuver - double intersection_transit_time = (scheduled_depart_time_ - scheduled_enter_time_)*1000.0; - - double intersection_end_downtrack = - wm_->routeTrackPos(nearest_stop_intersection->lanelets().back().centerline2d().back()).downtrack; - + base_time = std::max(scheduled_enter_time_, street_msg_timestamp_); + double intersection_transit_time = (scheduled_depart_time_ - base_time)/1000; + ROS_DEBUG_STREAM("intersection_transit_time: " << intersection_transit_time); + + intersection_transit_time = config_.min_maneuver_planning_period;//std::max(intersection_transit_time, config_.min_maneuver_planning_period); + ROS_DEBUG_STREAM("used intersection_transit_time: " << intersection_transit_time); // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver std::vector crossed_lanelets = getLaneletsBetweenWithException(current_downtrack_, intersection_end_downtrack, true, true); + + // find the turn direction at intersection: + + intersection_turn_direction_ = getTurnDirectionAtIntersection(crossed_lanelets); + + ROS_DEBUG_STREAM("turn direction at the intersection is: " << intersection_turn_direction_); + double intersection_speed_limit = findSpeedLimit(nearest_stop_intersection->lanelets().front()); - resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage( - current_downtrack_, intersection_end_downtrack, current_state.speed, intersection_speed_limit, - current_state.stamp, req.header.stamp + ros::Duration(intersection_transit_time), crossed_lanelets.front().id(), crossed_lanelets.back().id())); // when passing intersection, set the flag to false - // TODO: Another option, check the time on mobilityoperation - approaching_stop_controlled_interction_ = false; + double end_of_intersection = std::max(config_.intersection_exit_zone_length, intersection_end_downtrack - stop_intersection_down_track); + ROS_DEBUG_STREAM("Actual length of intersection: " << intersection_end_downtrack - stop_intersection_down_track); + ROS_DEBUG_STREAM("Used length of intersection: " << end_of_intersection); + + maneuver_planned = composeIntersectionTransitMessage( + current_state.downtrack, current_state.downtrack + end_of_intersection, current_state.speed, intersection_speed_limit, + current_state.stamp, req.header.stamp + ros::Duration(intersection_transit_time), intersection_turn_direction_, crossed_lanelets.front().id(), crossed_lanelets.back().id()); + + + if (distance_to_stopline < -end_of_intersection) + { + ROS_DEBUG_STREAM("Vehicle is out of intersection, stop planning..."); + // once the vehicle crosses the intersection, reset the flag to stop planning and publishing status/intent + approaching_stop_controlled_interction_ = false; + // once the intersection is crossed, reset turn direction + intersection_turn_direction_ = TurnDirection::Straight; + } } + resp.new_plan.maneuvers.push_back(maneuver_planned); return true; } +TurnDirection SCIStrategicPlugin::getTurnDirectionAtIntersection(std::vector lanelets_list) +{ + TurnDirection turn_direction = TurnDirection::Straight; + for (auto l:lanelets_list) + { + if(l.hasAttribute("turn_direction")) { + std::string direction_attribute = l.attribute("turn_direction").value(); + if (direction_attribute == "right") + { + turn_direction = TurnDirection::Right; + break; + } + else if (direction_attribute == "left") + { + turn_direction = TurnDirection::Left; + break; + } + else turn_direction = TurnDirection::Straight; + ROS_DEBUG_STREAM("intersection crossed lanelet direction is: " << turn_direction); + } + else + { + // if there is no attribute, assumption is straight + turn_direction = TurnDirection::Straight; + } + + } + return turn_direction; +} + void SCIStrategicPlugin::caseOneSpeedProfile(double speed_before_decel, double current_speed, double stop_time, std::vector* float_metadata_list) const { @@ -318,9 +535,19 @@ void SCIStrategicPlugin::caseOneSpeedProfile(double speed_before_decel, double c // Equations obtained from TSMO UC 1 Algorithm draft doc double a_acc = ((1 - desired_acceleration/desired_deceleration)*speed_before_decel - current_speed)/stop_time; + ROS_DEBUG_STREAM("Case one a_acc: " << a_acc); + a_acc = std::min(a_acc, desired_acceleration); + ROS_DEBUG_STREAM("Used Case one a_acc: " << a_acc); + double a_dec = ((desired_deceleration - desired_acceleration)*speed_before_decel - desired_deceleration * current_speed)/(desired_acceleration * stop_time); + ROS_DEBUG_STREAM("Case one a_dec: " << a_dec); + a_dec = std::max(a_dec, desired_deceleration); + ROS_DEBUG_STREAM("Used Case one a_dec: " << a_dec); + double t_acc = (speed_before_decel - current_speed)/a_acc; + ROS_DEBUG_STREAM("Case one t_acc: " << t_acc); double t_dec = -speed_before_decel/a_dec; // a_dec is negative so a - is used to make the t_dec positive. + ROS_DEBUG_STREAM("Case one t_dec: " << t_dec); float_metadata_list->push_back(a_acc); float_metadata_list->push_back(a_dec); float_metadata_list->push_back(t_acc); @@ -338,18 +565,30 @@ void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_befo if (speed_before_decel > speed_limit) { speed_before_decel = speed_limit; + ROS_DEBUG_STREAM("Case two speed_before_decel: " << speed_before_decel); } double t_c_nom = 2*stop_dist * ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed) - stop_time * ((1 - desired_acceleration/desired_deceleration)*pow(speed_limit,2) - pow(current_speed, 2)); double t_c_den = pow(speed_limit - current_speed, 2) - (desired_acceleration/desired_deceleration) * pow(speed_limit, 2); double t_cruise = t_c_nom / t_c_den; - + ROS_DEBUG_STREAM("Case two t_cruise: " << t_cruise); + // Equations obtained from TSMO UC 1 Algorithm draft doc double a_acc = ((1 - desired_acceleration/desired_deceleration)*speed_limit - current_speed)/(stop_time - t_cruise); + ROS_DEBUG_STREAM("Case two a_acc: " << a_acc); + a_acc = std::min(desired_acceleration, std::abs(a_acc)); + ROS_DEBUG_STREAM("Used Case two a_acc: " << a_acc); + double a_dec = ((desired_deceleration - desired_acceleration)*speed_limit - desired_deceleration * current_speed)/(desired_acceleration*(stop_time - t_cruise)); + ROS_DEBUG_STREAM("Case two a_dec: " << a_dec); + a_dec = -1*std::min(desired_deceleration, std::abs(a_dec)); + ROS_DEBUG_STREAM("Used Case two a_dec: " << a_dec); + double t_acc = (speed_limit - current_speed)/a_acc; + ROS_DEBUG_STREAM("Case two t_acc: " << t_acc); double t_dec = -speed_limit/a_dec; // a_dec is negative so a - is used to make the t_dec positive. + ROS_DEBUG_STREAM("Case two t_dec: " << t_dec); float_metadata_list->push_back(a_acc); float_metadata_list->push_back(a_dec); @@ -360,11 +599,11 @@ void SCIStrategicPlugin::caseTwoSpeedProfile(double stop_dist, double speed_befo } -void SCIStrategicPlugin::caseThreeSpeedProfile(double stop_dist, double current_speed, double stop_time, - std::vector* float_metadata_list) const +double SCIStrategicPlugin::caseThreeSpeedProfile(double stop_dist, double current_speed, double stop_time) const { double a_dec = (2*stop_dist - current_speed*(stop_time + config_.delta_t))/(stop_time * config_.delta_t); - float_metadata_list->push_back(a_dec); + ROS_DEBUG_STREAM("Case three a_dec: " << a_dec); + return a_dec; } cav_msgs::Maneuver SCIStrategicPlugin::composeLaneFollowingManeuverMessage(int case_num, double start_dist, double end_dist, @@ -394,7 +633,7 @@ cav_msgs::Maneuver SCIStrategicPlugin::composeLaneFollowingManeuverMessage(int c std::vector float_metadata_list; - double speed_before_decel = calcSpeedBeforeDecel(time_to_stop, end_dist, start_speed); + double speed_before_decel = calcSpeedBeforeDecel(time_to_stop, end_dist-start_dist, start_speed); switch(case_num) { @@ -402,10 +641,7 @@ cav_msgs::Maneuver SCIStrategicPlugin::composeLaneFollowingManeuverMessage(int c caseOneSpeedProfile(speed_before_decel, start_speed, time_to_stop, &float_metadata_list); break; case 2: - caseTwoSpeedProfile(end_dist, speed_before_decel, start_speed, time_to_stop, speed_limit_, &float_metadata_list); - break; - case 3: - caseThreeSpeedProfile(end_dist, start_speed, time_to_stop, &float_metadata_list); + caseTwoSpeedProfile(end_dist-start_dist, speed_before_decel, start_speed, time_to_stop, speed_limit_, &float_metadata_list); break; default: return empty_msg; @@ -433,28 +669,46 @@ double SCIStrategicPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt) cons } } -void SCIStrategicPlugin::generateMobilityOperation() +cav_msgs::MobilityOperation SCIStrategicPlugin::generateMobilityOperation() { cav_msgs::MobilityOperation mo_; - mo_.header.timestamp = ros::Time::now().toNSec() * 1000000; - - std::string id(bsm_id.begin(), bsm_id.end()); - mo_.header.sender_bsm_id = id; - - int flag = (approaching_stop_controlled_interction_ ? 1 : 0); + mo_.header.timestamp = ros::Time::now().toNSec()/1000000; + mo_.header.sender_id = config_.vehicle_id; + mo_.header.sender_bsm_id = bsm_id_; + mo_.strategy = stop_controlled_intersection_strategy_; double vehicle_acceleration_limit_ = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; double vehicle_deceleration_limit_ = -1 * config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; - mo_.strategy_params = "intersection_box_flag," + std::to_string(flag) + ",acceleration_limit," + std::to_string(vehicle_acceleration_limit_) + ",deceleration_limit," + std::to_string(vehicle_deceleration_limit_); + std::string intersection_turn_direction = "straight"; + if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right"; + if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; + - mobility_operation_pub.publish(mo_); + mo_.strategy_params = "access: " + std::to_string(is_allowed_int_) + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + + ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + std::to_string(scheduled_departure_position_) + + ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); + + + return mo_; +} + +bool SCIStrategicPlugin::onSpin() +{ + if (approaching_stop_controlled_interction_) + { + cav_msgs::MobilityOperation status_msg = generateMobilityOperation(); + mobility_operation_pub.publish(status_msg); + } + return true; } + cav_msgs::Maneuver SCIStrategicPlugin::composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, const lanelet::Id& starting_lane_id, - const lanelet::Id& ending_lane_id, + const lanelet::Id& ending_lane_id, double stopping_accel, ros::Time start_time, ros::Time end_time) const { cav_msgs::Maneuver maneuver_msg; @@ -473,33 +727,86 @@ cav_msgs::Maneuver SCIStrategicPlugin::composeStopAndWaitManeuverMessage(double maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id); // Set the meta data for the stop location buffer maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stop_line_buffer); + maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel); + + ROS_INFO_STREAM("Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist); + return maneuver_msg; } cav_msgs::Maneuver SCIStrategicPlugin::composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, double target_speed, ros::Time start_time, ros::Time end_time, + TurnDirection turn_direction, const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id) const { cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT; - maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin = - config_.strategic_plugin_name; - maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin = - config_.intersection_transit_plugin_name; - maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector = - cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type = - cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist; - maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist; - maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed; - maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed; - maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time; - maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time; - maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id); - maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id); + if (turn_direction == TurnDirection::Left) + { + maneuver_msg.type = cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN; + maneuver_msg.intersection_transit_left_turn_maneuver.parameters.planning_strategic_plugin = + config_.strategic_plugin_name; + maneuver_msg.intersection_transit_left_turn_maneuver.parameters.planning_tactical_plugin = + config_.intersection_transit_plugin_name; + maneuver_msg.intersection_transit_left_turn_maneuver.parameters.presence_vector = + cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; + maneuver_msg.intersection_transit_left_turn_maneuver.parameters.negotiation_type = + cav_msgs::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.intersection_transit_left_turn_maneuver.start_dist = start_dist; + maneuver_msg.intersection_transit_left_turn_maneuver.end_dist = end_dist; + maneuver_msg.intersection_transit_left_turn_maneuver.start_speed = start_speed; + maneuver_msg.intersection_transit_left_turn_maneuver.end_speed = target_speed; + maneuver_msg.intersection_transit_left_turn_maneuver.start_time = start_time; + maneuver_msg.intersection_transit_left_turn_maneuver.end_time = end_time; + maneuver_msg.intersection_transit_left_turn_maneuver.starting_lane_id = std::to_string(starting_lane_id); + maneuver_msg.intersection_transit_left_turn_maneuver.ending_lane_id = std::to_string(ending_lane_id); + + } + else if (turn_direction == TurnDirection::Right) + { + maneuver_msg.type = cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN; + maneuver_msg.intersection_transit_right_turn_maneuver.parameters.planning_strategic_plugin = + config_.strategic_plugin_name; + maneuver_msg.intersection_transit_right_turn_maneuver.parameters.planning_tactical_plugin = + config_.intersection_transit_plugin_name; + maneuver_msg.intersection_transit_right_turn_maneuver.parameters.presence_vector = + cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; + maneuver_msg.intersection_transit_right_turn_maneuver.parameters.negotiation_type = + cav_msgs::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.intersection_transit_right_turn_maneuver.start_dist = start_dist; + maneuver_msg.intersection_transit_right_turn_maneuver.end_dist = end_dist; + maneuver_msg.intersection_transit_right_turn_maneuver.start_speed = start_speed; + maneuver_msg.intersection_transit_right_turn_maneuver.end_speed = target_speed; + maneuver_msg.intersection_transit_right_turn_maneuver.start_time = start_time; + maneuver_msg.intersection_transit_right_turn_maneuver.end_time = end_time; + maneuver_msg.intersection_transit_right_turn_maneuver.starting_lane_id = std::to_string(starting_lane_id); + maneuver_msg.intersection_transit_right_turn_maneuver.ending_lane_id = std::to_string(ending_lane_id); + + } + else + { + maneuver_msg.type = cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT; + maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin = + config_.strategic_plugin_name; + maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin = + config_.intersection_transit_plugin_name; + maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector = + cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; + maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type = + cav_msgs::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist; + maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist; + maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed; + maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed; + maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time; + maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time; + maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id); + maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id); + + } + + // Start time and end time for maneuver are assigned in updateTimeProgress diff --git a/sci_strategic_plugin/test/test_strategic_plugin.cpp b/sci_strategic_plugin/test/test_strategic_plugin.cpp index b1f2ef8aac..b43fbb10f3 100644 --- a/sci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/sci_strategic_plugin/test/test_strategic_plugin.cpp @@ -78,8 +78,9 @@ TEST(SCIStrategicPluginTest, composeIntersectionTransitMessage) std::shared_ptr wm; SCIStrategicPluginConfig config; SCIStrategicPlugin sci(wm, config); + TurnDirection intersection_turn_direction = TurnDirection::Straight; - auto result = sci.composeIntersectionTransitMessage(10.2, 20.4, 5, 10, ros::Time(1.2), ros::Time(2.2), 1200, 1201); + auto result = sci.composeIntersectionTransitMessage(10.2, 20.4, 5, 10, ros::Time(1.2), ros::Time(2.2), intersection_turn_direction, 1200, 1201); ASSERT_EQ(cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT, result.type); ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, @@ -107,7 +108,7 @@ TEST(SCIStrategicPluginTest, composeStopAndWaitManeuverMessage) SCIStrategicPluginConfig config; SCIStrategicPlugin sci(wm, config); - auto result = sci.composeStopAndWaitManeuverMessage(10.2, 20.4, 5, 1200, 1201, ros::Time(1.2), ros::Time(2.2)); + auto result = sci.composeStopAndWaitManeuverMessage(10.2, 20.4, 5, 1200, 1201, 0.56, ros::Time(1.2), ros::Time(2.2)); ASSERT_EQ(cav_msgs::Maneuver::STOP_AND_WAIT, result.type); ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, result.stop_and_wait_maneuver.parameters.negotiation_type); @@ -123,6 +124,7 @@ TEST(SCIStrategicPluginTest, composeStopAndWaitManeuverMessage) ASSERT_EQ(5, result.stop_and_wait_maneuver.start_speed); ASSERT_EQ(ros::Time(1.2), result.stop_and_wait_maneuver.start_time); ASSERT_EQ(ros::Time(2.2), result.stop_and_wait_maneuver.end_time); + ASSERT_EQ(0.56, result.stop_and_wait_maneuver.parameters.float_valued_meta_data[1]); ASSERT_TRUE(result.stop_and_wait_maneuver.starting_lane_id.compare("1200") == 0); ASSERT_TRUE(result.stop_and_wait_maneuver.ending_lane_id.compare("1201") == 0); } @@ -150,40 +152,28 @@ TEST(SCIStrategicPluginTest, findSpeedLimit) ASSERT_NEAR(11.176, sci.findSpeedLimit(*ll_iterator), 0.00001); } -TEST(SCIStrategicPluginTest, parseStrategyParamstest) +TEST(SCIStrategicPluginTest, moboperationcbtest) { + cav_msgs::MobilityOperation msg; + msg.strategy = "Carma/stop_controlled_intersection"; - uint32_t timestamp = 11111; - uint32_t est_stop_t = 123456; - // Encode JSON with Boost Property Tree - using boost::property_tree::ptree; - ptree pt; - - ptree schedule; - - ptree metadata; - metadata.put("timestamp", timestamp); - schedule.put_child("metadata", metadata); - - ptree payload; - payload.put("veh_id", "default_id"); - payload.put("est_stop_t", est_stop_t); - payload.put("est_enter_t", 22222); - payload.put("est_depart_t", 22222); - payload.put("latest_depart_p", 22222); - payload.put("is_allowed_int", 0); - schedule.put_child("payload", payload); + std::shared_ptr wm = std::make_shared(); + SCIStrategicPluginConfig config; + SCIStrategicPlugin sci(wm, config); - pt.put_child("schedule_plan", schedule); + ASSERT_EQ(sci.approaching_stop_controlled_interction_, false); + auto msg_ptr = boost::make_shared(msg); + sci.mobilityOperationCb(msg_ptr); - std::stringstream body_stream; - boost::property_tree::json_parser::write_json(body_stream, pt); - std::string out = body_stream.str(); - std::cout << out << std::endl; + ASSERT_EQ(sci.approaching_stop_controlled_interction_, true); +} +TEST(SCIStrategicPluginTest, parseStrategyParamstest) +{ + cav_msgs::MobilityOperation msg; - msg.strategy_params = body_stream.str(); + msg.strategy_params = "st:16000,et:32000,dt:48000,dp:1,access:0"; std::shared_ptr wm = std::make_shared(); SCIStrategicPluginConfig config; @@ -191,9 +181,17 @@ TEST(SCIStrategicPluginTest, parseStrategyParamstest) sci.parseStrategyParams(msg.strategy_params); - EXPECT_EQ(timestamp, sci.street_msg_timestamp_); - EXPECT_EQ(est_stop_t, sci.scheduled_stop_time_); + + EXPECT_EQ(16000, sci.scheduled_stop_time_); + EXPECT_EQ(32000, sci.scheduled_enter_time_); + EXPECT_EQ(48000, sci.scheduled_depart_time_); + EXPECT_EQ(1, sci.scheduled_departure_position_); EXPECT_EQ(false, sci.is_allowed_int_); + + cav_msgs::MobilityOperation outgoing_msg = sci.generateMobilityOperation(); + EXPECT_EQ(outgoing_msg.strategy, "Carma/stop_controlled_intersection"); + EXPECT_EQ(outgoing_msg.header.sender_id, config.vehicle_id); + std::cout << "strategy_param: " << outgoing_msg.strategy_params << std::endl; } TEST(SCIStrategicPluginTest, calcEstimatedStopTimetest) @@ -265,41 +263,39 @@ TEST(SCIStrategicPluginTest, caseTwoSpeedProfiletest) sci.caseTwoSpeedProfile(250, 21.2, 10, 20, 15, &metadata); - EXPECT_NEAR(2.5, metadata[0], 0.01); - EXPECT_NEAR(-2.5, metadata[1], 0.01); - EXPECT_NEAR(2, metadata[2], 0.01); - EXPECT_NEAR(6, metadata[3], 0.01); + EXPECT_NEAR(1, metadata[0], 0.01); + EXPECT_NEAR(1, metadata[1], 0.01); + EXPECT_NEAR(5, metadata[2], 0.01); + EXPECT_NEAR(-15, metadata[3], 0.01); EXPECT_NEAR(12, metadata[4], 0.01); } -TEST(SCIStrategicPluginTest, mob_op_cb_test) +TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest) { std::shared_ptr wm = std::make_shared(); SCIStrategicPluginConfig config; SCIStrategicPlugin sci(wm, config); - sci.approaching_stop_controlled_interction_ = false; - cav_msgs::MobilityOperation incoming_msg; - incoming_msg.strategy = "Carma/stop_controlled_intersection"; - auto msg = boost::make_shared(incoming_msg); - sci.mobilityOperationCb(msg); - EXPECT_EQ(true, sci.approaching_stop_controlled_interction_); -} + double dec_val = sci.caseThreeSpeedProfile(50, 5, 30); -TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest) + EXPECT_NEAR(-1.83, dec_val, 0.01); +} + +TEST(SCIStrategicPluginTest, testIntersectionturndirection) { std::shared_ptr wm = std::make_shared(); SCIStrategicPluginConfig config; SCIStrategicPlugin sci(wm, config); - std::vector metadata{}; + double dec_val = sci.caseThreeSpeedProfile(50, 5, 30); - sci.caseThreeSpeedProfile(50, 5, 30, &metadata); - - EXPECT_NEAR(-1.83, metadata[0], 0.01); + EXPECT_NEAR(-1.83, dec_val, 0.01); } -TEST(SCIStrategicPluginTest, maneuvercbtest) +// The map in this unit test does not support turn direction and therefore it is disabled. +// The test can be run if the turn direction detection logic (lines 461-467) is commented. + +TEST(SCIStrategicPluginTest, DISABLE_maneuvercbtest) { lanelet::Id id{1200}; // intersection id @@ -346,6 +342,7 @@ TEST(SCIStrategicPluginTest, maneuvercbtest) SCIStrategicPluginConfig config; SCIStrategicPlugin sci(cmw_ptr, config); + sci.current_downtrack_ = 1.0; // pose callback test geometry_msgs::PoseStamped pose_msg; pose_msg.pose.position.x = 1.0; @@ -356,6 +353,11 @@ TEST(SCIStrategicPluginTest, maneuvercbtest) sci.approaching_stop_controlled_interction_ = true; + sci.street_msg_timestamp_ = 2000; + sci.scheduled_stop_time_ = 2500; + sci.scheduled_enter_time_ = 5000; + sci.scheduled_depart_time_ = 7000; + cav_srvs::PlanManeuversRequest req; cav_srvs::PlanManeuversResponse resp; @@ -368,8 +370,7 @@ TEST(SCIStrategicPluginTest, maneuvercbtest) req.veh_logitudinal_velocity = 11.176; req.veh_lane_id = "1200"; - sci.scheduled_stop_time_ = 5000; - sci.street_msg_timestamp_ = 2000; + sci.planManeuverCb(req, resp); @@ -377,7 +378,7 @@ TEST(SCIStrategicPluginTest, maneuvercbtest) ASSERT_EQ(resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids[0], "1200"); ASSERT_NEAR(0.0, resp.new_plan.maneuvers[0].lane_following_maneuver.end_speed, 0.00001); // case 3 - ASSERT_EQ(3, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.int_valued_meta_data[0]); + ASSERT_EQ(2, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.int_valued_meta_data[0]); // at the stop line cav_srvs::PlanManeuversRequest req1; diff --git a/stop_and_wait_plugin/CMakeLists.txt b/stop_and_wait_plugin/CMakeLists.txt index aec6178810..1718a4eb2b 100644 --- a/stop_and_wait_plugin/CMakeLists.txt +++ b/stop_and_wait_plugin/CMakeLists.txt @@ -35,6 +35,7 @@ set( CATKIN_DEPS tf2_geometry_msgs roslib route + basic_autonomy ) ## Find catkin macros and libraries diff --git a/stop_and_wait_plugin/config/parameters.yaml b/stop_and_wait_plugin/config/parameters.yaml index 1458ecd1cc..bac4b83a33 100644 --- a/stop_and_wait_plugin/config/parameters.yaml +++ b/stop_and_wait_plugin/config/parameters.yaml @@ -16,7 +16,7 @@ accel_limit_multiplier : 0.95 # Double: Minimum speed the vehicle can command before being ready to stop # Units: m/s -crawl_speed : 2.2352 +crawl_speed : 0.91 # Double: The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions # Units: m @@ -26,4 +26,7 @@ cernterline_sampling_spacing: 1.0 # Double: 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[] # Units: m -default_stopping_buffer: 5.0 \ No newline at end of file +default_stopping_buffer: 5.0 + +#Double: Window size for the moving average filter used for smoothing the speeds. +moving_average_window_size: 19.0 \ No newline at end of file diff --git a/stop_and_wait_plugin/include/stop_and_wait_config.h b/stop_and_wait_plugin/include/stop_and_wait_config.h index e9bb1058d4..c26740c69d 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_config.h +++ b/stop_and_wait_plugin/include/stop_and_wait_config.h @@ -31,7 +31,7 @@ struct StopandWaitConfig double crawl_speed = 1.34112; // Minimum speed the vehicle can command before being ready to stop double cernterline_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 friend std::ostream& operator<<(std::ostream& output, const StopandWaitConfig& c) { output << "StopandWaitConfig { " << std::endl diff --git a/stop_and_wait_plugin/include/stop_and_wait_plugin.h b/stop_and_wait_plugin/include/stop_and_wait_plugin.h index 426a049fe5..b6256f6f91 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_plugin.h +++ b/stop_and_wait_plugin/include/stop_and_wait_plugin.h @@ -33,6 +33,8 @@ #include #include #include "stop_and_wait_config.h" +#include +#include namespace stop_and_wait_plugin { diff --git a/stop_and_wait_plugin/package.xml b/stop_and_wait_plugin/package.xml index 9205831063..b244326ba9 100644 --- a/stop_and_wait_plugin/package.xml +++ b/stop_and_wait_plugin/package.xml @@ -35,6 +35,7 @@ tf2_geometry_msgs trajectory_utils route - roslib + roslib + basic_autonomy 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 72b035fccb..e3be4f409a 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -235,8 +235,23 @@ std::vector StopandWait::compose_trajectory_from_ double req_dist = (starting_speed * starting_speed) / (2.0 * target_accel); // Distance needed to go from current speed to 0 at target accel + // In cases where the vehicle is not able to stop before the half_stopping_buffer the remaining distance becomes negative + // which will cause the target accel in this loop to be negative and make the vehicle speed up + while (remaining_distance <= 0.0) + { + if(remaining_distance <= (stop_location - starting_downtrack)) + { + //Add additional distance to remaining to allow vehicle to stop within buffer + remaining_distance += 0.2; + } + else{ + break; + } + } + if (req_dist > remaining_distance) { + ROS_DEBUG_STREAM("Target Accel Update From: " << target_accel); target_accel = (starting_speed * starting_speed) / (2.0 * remaining_distance); // If we cannot reach the end point it the @@ -244,6 +259,7 @@ std::vector StopandWait::compose_trajectory_from_ ROS_DEBUG_STREAM("Target Accel Update To: " << target_accel); } + std::vector inverse_downtracks; // Store downtracks in reverse inverse_downtracks.reserve(points.size()); final_points.reserve(points.size()); @@ -315,31 +331,33 @@ std::vector StopandWait::compose_trajectory_from_ bool vehicle_in_buffer = downtracks.back() < stop_location_buffer; - for (size_t i = 0; i < speeds.size(); i++) + std::vector filtered_speeds = basic_autonomy::smoothing::moving_average_filter(speeds, config_.moving_average_window_size); + + for (size_t i = 0; i < filtered_speeds.size(); i++) { // Apply minimum speed constraint double downtrack = downtracks[i]; constexpr double one_mph_in_mps = 0.44704; - if (downtracks.back() - downtrack < stop_location_buffer && speeds[i] < config_.crawl_speed + one_mph_in_mps) + 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 == speeds.size() - 1)) { // Vehicle is in the buffer - speeds[i] = 0.0; + if (vehicle_in_buffer || (i == filtered_speeds.size() - 1)) { // Vehicle is in the buffer + filtered_speeds[i] = 0.0; } else { // Vehicle is not in the buffer so fill buffer with crawl speed - speeds[i] = std::max(speeds[i], config_.crawl_speed); + filtered_speeds[i] = std::max(filtered_speeds[i], config_.crawl_speed); } } else { - speeds[i] = std::max(speeds[i], config_.crawl_speed); + filtered_speeds[i] = std::max(filtered_speeds[i], config_.crawl_speed); } } std::vector times; - trajectory_utils::conversions::speed_to_time(downtracks, speeds, ×); + trajectory_utils::conversions::speed_to_time(downtracks, filtered_speeds, ×); for (size_t i = 0; i < times.size(); i++) { @@ -355,7 +373,7 @@ std::vector StopandWait::compose_trajectory_from_ for (size_t i = 0; i < points.size(); i++) { - ROS_DEBUG_STREAM("1d: " << downtracks[i] << " t: " << times[i] << " v: " << speeds[i]); + ROS_DEBUG_STREAM("1d: " << downtracks[i] << " t: " << times[i] << " v: " << filtered_speeds[i]); } auto traj = trajectory_from_points_times_orientations(raw_points, times, yaws, start_time); diff --git a/stop_controlled_intersection_tactical_plugin/config/parameters.yaml b/stop_controlled_intersection_tactical_plugin/config/parameters.yaml index aeba24a8fb..1c7abd885f 100644 --- a/stop_controlled_intersection_tactical_plugin/config/parameters.yaml +++ b/stop_controlled_intersection_tactical_plugin/config/parameters.yaml @@ -5,5 +5,5 @@ trajectory_time_length: 6.0 # Trajectory length in seconds curve_resample_step_size: 1.0 # Curve re-sampling step size in m curvature_moving_average_window_size: 9 # Size of the window used in the moving average filter to smooth the computed curvature lateral_accel_limit: 2.5 #Maximum allowable lateral acceleration m/s^2 -speed_moving_average_window_size: 5 # Size of the window used in the moving average filter to smooth the output speeds +speed_moving_average_window_size: 19 # Size of the window used in the moving average filter to smooth the output speeds back_distance: 20.0 # Number of meters behind the first maneuver that need to be included in points for curvature calculation \ No newline at end of file diff --git a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin.h b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin.h index 15081a9ada..58fc98c460 100644 --- a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin.h +++ b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin.h @@ -36,6 +36,7 @@ #include #include #include +#include /** @@ -112,7 +113,7 @@ class StopControlledIntersectionTacticalPlugin * \return List of centerline points paired with target speeds */ std::vector create_case_one_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, - std::vector& route_geometry_points, double starting_speed); + std::vector& route_geometry_points, double starting_speed, const cav_msgs::VehicleState& states); /** * \brief Creates a speed profile according to case two of the stop controlled intersection, diff --git a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h index 76b8f972e0..73f79e6bef 100644 --- a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h +++ b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h @@ -85,7 +85,7 @@ class StopControlledIntersectionTransitPluginNode StopControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); // [trajectory_debug_pub](const auto& msg) { trajectory_debug_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/StopControllledIntersectionTacticalPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/StopControlledIntersectionTacticalPlugin/plan_trajectory", &StopControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); ros::Timer discovery_pub_timer_ = diff --git a/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp b/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp index 7d45fc4ba7..2400fedb4d 100644 --- a/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp +++ b/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp @@ -56,7 +56,7 @@ StopControlledIntersectionTacticalPlugin::StopControlledIntersectionTacticalPlug plugin_discovery_msg_.name = "StopControlledIntersectionTacticalPlugin"; plugin_discovery_msg_.versionId = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; } @@ -100,8 +100,9 @@ bool StopControlledIntersectionTacticalPlugin::plan_trajectory_cb(cav_srvs::Plan ROS_DEBUG_STREAM("Current_downtrack"<< current_downtrack); std::vector points_and_target_speeds = maneuvers_to_points( maneuver_plan, wm_, req.vehicle_state); - - //Trajectory Plan + ROS_DEBUG_STREAM("Maneuver to points size:"<< points_and_target_speeds.size()); + // ROS_DEBUG_STREAM("Printing points: "); + // TODO: add print logic cav_msgs::TrajectoryPlan trajectory; trajectory.header.frame_id = "map"; trajectory.header.stamp = req.header.stamp; @@ -111,6 +112,12 @@ bool StopControlledIntersectionTacticalPlugin::plan_trajectory_cb(cav_srvs::Plan trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp); trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel; + // Set the planning plugin field name + for (auto& p : trajectory.trajectory_points) { + p.planner_plugin_name = plugin_discovery_msg_.name; + // p.controller_plugin_name = "PurePursuit"; + } + resp.trajectory_plan = trajectory; resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); @@ -155,7 +162,7 @@ std::vector StopControlledIntersectionTacticalPlugin::maneuvers_ GET_MANEUVER_PROPERTY(maneuver, end_dist), config_.centerline_sampling_spacing); route_points.insert(route_points.begin(), veh_pos); - + ROS_DEBUG_STREAM("Route geometery points size: "< StopControlledIntersectionTacticalPlugin::maneuvers_ int case_num = GET_MANEUVER_PROPERTY(maneuver,parameters.int_valued_meta_data[0]); if(case_num == 1){ - points_and_target_speeds = create_case_one_speed_profile(wm, maneuver, route_points, starting_speed); + points_and_target_speeds = create_case_one_speed_profile(wm, maneuver, route_points, starting_speed, state); } else if(case_num == 2){ points_and_target_speeds = create_case_two_speed_profile(wm, maneuver, route_points, starting_speed); @@ -183,29 +190,41 @@ std::vector StopControlledIntersectionTacticalPlugin::maneuvers_ } std::vector StopControlledIntersectionTacticalPlugin::create_case_one_speed_profile(const carma_wm::WorldModelConstPtr& wm, -const cav_msgs::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed){ +const cav_msgs::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed, const cav_msgs::VehicleState& state){ + + ROS_DEBUG_STREAM("Planning for Case One"); //Derive meta data values from maneuver message - Using order in sci_strategic_plugin double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value double t_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]); double t_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]); double speed_before_decel = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]); + ROS_DEBUG_STREAM("a_acc received: "<< a_acc); + ROS_DEBUG_STREAM("a_dec received: "<< a_dec); + ROS_DEBUG_STREAM("t_acc received: "<< t_acc); + ROS_DEBUG_STREAM("t_dec received: "<< t_dec); + ROS_DEBUG_STREAM("speed before decel received: "<< speed_before_decel); //Derive start and end dist from maneuver double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist); double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist); - //Checking route geometry start against start_dist and adjust profile - double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack; //Starting downtrack based on geometry points + ROS_DEBUG_STREAM("Maneuver starting downtrack: "<< start_dist); + ROS_DEBUG_STREAM("Maneuver ending downtrack: "<< end_dist); + //Checking state against start_dist and adjust profile + lanelet::BasicPoint2d state_point(state.X_pos_global, state.Y_pos_global); + double route_starting_downtrack = wm->routeTrackPos(state_point).downtrack; //Starting downtrack based on geometry points double dist_acc; //Distance for which acceleration lasts if(route_starting_downtrack < start_dist){ //Update parameters //Keeping the deceleration part the same + ROS_DEBUG_STREAM("Starting distance is less than maneuver start, updating parameters"); double dist_decel = pow(speed_before_decel, 2)/(2*std::abs(a_dec)); dist_acc = end_dist - dist_decel; a_acc = (pow(speed_before_decel, 2) - pow(starting_speed,2))/(2*dist_acc); + ROS_DEBUG_STREAM("Updated a_acc: "<< a_acc); } else{ //Use parameters from maneuver message @@ -214,11 +233,11 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge std::vector points_and_target_speeds; PointSpeedPair first_point; - first_point.point = route_geometry_points[0]; + first_point.point = state_point; first_point.speed = starting_speed; points_and_target_speeds.push_back(first_point); - lanelet::BasicPoint2d prev_point = route_geometry_points[0]; + lanelet::BasicPoint2d prev_point = state_point; double total_dist_covered = 0; //Starting dist for maneuver treated as 0.0 for(size_t i = 1; i < route_geometry_points.size(); i++){ @@ -240,11 +259,17 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge } PointSpeedPair p; - p.point = route_geometry_points[i]; - p.speed = speed_i; + if(speed_i < epsilon_){ + p.point = prev_point; + p.speed = 0.0; + } + else{ + p.point = route_geometry_points[i]; + p.speed = speed_i; + prev_point = current_point; //Advance prev point if speed changes + } points_and_target_speeds.push_back(p); - prev_point = route_geometry_points[i]; } return points_and_target_speeds; @@ -253,7 +278,7 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge std::vector StopControlledIntersectionTacticalPlugin::create_case_two_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed){ - + ROS_DEBUG_STREAM("Planning for Case Two"); //Derive meta data values from maneuver message - Using order in sci_strategic_plugin double a_acc = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); //a_dec is a -ve value @@ -265,6 +290,8 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge //Derive start and end dist from maneuver double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist); double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist); + ROS_DEBUG_STREAM("Maneuver starting downtrack: "<< start_dist); + ROS_DEBUG_STREAM("Maneuver ending downtrack: "<< end_dist); //Checking route geometry start against start_dist and adjust profile double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack; //Starting downtrack based on geometry points @@ -275,6 +302,7 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge if(route_starting_downtrack < start_dist){ //update parameters //Keeping acceleration and deceleration part same as planned in strategic plugin + ROS_DEBUG_STREAM("Starting distance is less than maneuver start, updating parameters"); dist_acc = starting_speed*t_acc + 0.5 * a_acc * pow(t_acc,2); dist_decel = speed_before_decel*t_dec + 0.5 * a_dec * pow(t_dec,2); dist_cruise = end_dist - route_starting_downtrack - (dist_acc + dist_decel); @@ -291,6 +319,7 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge if(total_distance_needed - (end_dist - start_dist) > epsilon_ ){ //Requested maneuver needs to be modified to meet start and end dist req //Sacrifice on cruising and then acceleration if needed + ROS_DEBUG_STREAM("Updating maneuver to meet start and end dist req."); double delta_total_dist = total_distance_needed - (end_dist - start_dist); dist_cruise -= delta_total_dist; if(dist_cruise < 0){ @@ -330,11 +359,17 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge } PointSpeedPair p; - p.point = route_point; - p.speed = std::min(speed_i,speed_before_decel); + if(speed_i < epsilon_){ + p.point = prev_point; + p.speed = 0.0; + } + else{ + p.point = route_point; + p.speed = std::min(speed_i,speed_before_decel); + prev_point = current_point; //Advance prev point if speed changes + } points_and_target_speeds.push_back(p); - prev_point = route_point; prev_speed = speed_i; } @@ -344,18 +379,22 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge std::vector StopControlledIntersectionTacticalPlugin::create_case_three_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed){ + ROS_DEBUG_STREAM("Planning for Case three"); //Derive meta data values from maneuver message - Using order in sci_strategic_plugin double a_dec = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); //Derive start and end dist from maneuver double start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist); double end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist); + ROS_DEBUG_STREAM("Maneuver starting downtrack: "<< start_dist); + ROS_DEBUG_STREAM("Maneuver ending downtrack: "<< end_dist); //Checking route geometry start against start_dist and adjust profile double route_starting_downtrack = wm->routeTrackPos(route_geometry_points[0]).downtrack; //Starting downtrack based on geometry points if(route_starting_downtrack < start_dist){ //update parameter + ROS_DEBUG_STREAM("Starting distance is less than maneuver start, updating parameters"); a_dec = pow(starting_speed, 2)/(2*(end_dist - route_starting_downtrack)); } @@ -375,16 +414,21 @@ const cav_msgs::Maneuver& maneuver, std::vector& route_ge //Find speed at dist covered double speed_i = sqrt(std::max(pow(starting_speed,2) + 2 * a_dec * total_dist_covered, 0.0)); //std::max to ensure negative value is not sqrt + PointSpeedPair p; + if(speed_i < epsilon_){ - speed_i = 0.0; + p.point = prev_point; + p.speed = 0.0; } - - PointSpeedPair p; - p.point = route_geometry_points[i]; - p.speed = speed_i; + else{ + p.point = route_geometry_points[i]; + p.speed = speed_i; + prev_point = current_point; //Advance prev point if speed changes + } + points_and_target_speeds.push_back(p); - prev_point = current_point; + } return points_and_target_speeds; @@ -399,10 +443,10 @@ std::vector StopControlledIntersectionTacticalPlu << " speed: " << state.longitudinal_vel); int nearest_pt_index = basic_autonomy::waypoint_generation::get_nearest_point_index(points, state); - + ROS_DEBUG_STREAM("Nearest pt index: "< future_points(points.begin() + nearest_pt_index + 1, points.end()); //Points in front of current vehicle position + ROS_DEBUG_STREAM("Future points size: "< geometry_profile; std::vector speeds; basic_autonomy::waypoint_generation:: split_point_speed_pairs(points_and_target_speeds, &geometry_profile, &speeds); - std::vector case_one_profile = plugin.create_case_one_speed_profile(wm, maneuver, geometry_profile, req.vehicle_state.longitudinal_vel); + std::vector case_one_profile = plugin.create_case_one_speed_profile(wm, maneuver, geometry_profile, req.vehicle_state.longitudinal_vel, req.vehicle_state); //Ensure acceleration and deceleration is happening double dist_to_cover = maneuver.lane_following_maneuver.end_dist - maneuver.lane_following_maneuver.start_dist; double total_dist_covered = 0; diff --git a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h b/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h index dcdd66b8d0..e6aff2d1e0 100644 --- a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h +++ b/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include "wz_strategic_plugin/wz_state_transition_table.h" #include "wz_strategic_plugin/wz_strategic_plugin_config.h" #include "wz_strategic_plugin/wz_states.h" @@ -84,7 +84,7 @@ class WzStrategicPlugin */ void planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights); + const std::vector& traffic_lights); /** * \brief Method for performing maneuver planning when the current plugin state is TransitState::APPROACHING @@ -98,7 +98,7 @@ class WzStrategicPlugin */ void planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights); + const std::vector& traffic_lights); /** * \brief Method for performing maneuver planning when the current plugin state is TransitState::WAITING @@ -112,7 +112,7 @@ class WzStrategicPlugin */ void planWhenWAITING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights); + const std::vector& traffic_lights); /** * \brief Method for performing maneuver planning when the current plugin state is TransitState::DEPARTING @@ -192,7 +192,7 @@ class WzStrategicPlugin * * \return true if the state is supported, flase otherwise */ - bool supportedLightState(lanelet::CarmaTrafficLightState state) const; + bool supportedLightState(lanelet::CarmaTrafficSignalState state) const; /** * \brief Helper method that checks both if the input optional light state is set and if the state it contains is @@ -203,7 +203,7 @@ class WzStrategicPlugin * * \return True if the optional is set and the contained state is supported. False otherwise */ - bool validLightState(const boost::optional& optional_state, + bool validLightState(const boost::optional& optional_state, const ros::Time& source_time) const; /** diff --git a/wz_strategic_plugin/src/wz_strategic_plugin.cpp b/wz_strategic_plugin/src/wz_strategic_plugin.cpp index bb6e97a6df..5427132fd2 100644 --- a/wz_strategic_plugin/src/wz_strategic_plugin.cpp +++ b/wz_strategic_plugin/src/wz_strategic_plugin.cpp @@ -51,29 +51,29 @@ cav_msgs::Plugin WzStrategicPlugin::getDiscoveryMsg() const return plugin_discovery_msg_; } -bool WzStrategicPlugin::supportedLightState(lanelet::CarmaTrafficLightState state) const +bool WzStrategicPlugin::supportedLightState(lanelet::CarmaTrafficSignalState state) const { switch (state) { // NOTE: Following cases are intentional fall through. // Supported light states - case lanelet::CarmaTrafficLightState::STOP_AND_REMAIN: // Solid Red - case lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic - case lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic + case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN: // Solid Red + case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic + case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic // (normally used with arrows) return true; // Unsupported light states - case lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic - case lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting + case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic + case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting // traffic - case lanelet::CarmaTrafficLightState::UNAVAILABLE: // No data available - case lanelet::CarmaTrafficLightState::DARK: // Light is non-functional - case lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED: // Flashing Red + case lanelet::CarmaTrafficSignalState::UNAVAILABLE: // No data available + case lanelet::CarmaTrafficSignalState::DARK: // Light is non-functional + case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED: // Flashing Red - case lanelet::CarmaTrafficLightState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU) + case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU) // (normally used with arrows) - case lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing + case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing default: return false; } @@ -117,7 +117,7 @@ WzStrategicPlugin::VehicleState WzStrategicPlugin::extractInitialState(const cav return state; } -bool WzStrategicPlugin::validLightState(const boost::optional& optional_state, +bool WzStrategicPlugin::validLightState(const boost::optional& optional_state, const ros::Time& source_time) const { if (!optional_state) @@ -126,11 +126,11 @@ bool WzStrategicPlugin::validLightState(const boost::optional WzStrategicPlugin::getLaneletsBetweenWithExce void WzStrategicPlugin::planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) + const std::vector& traffic_lights) { // Reset intersection state since in this state we are not yet known to be in or approaching an intersection intersection_speed_ = boost::none; @@ -201,7 +201,7 @@ void WzStrategicPlugin::planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest // TODO should we handle when the vehicle is not going to make the light but doesn't have space to stop? void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) + const std::vector& traffic_lights) { if (traffic_lights.empty()) // If we are in the approaching state and there is no longer any lights ahead of us then // the vehicle must have crossed the stop bar @@ -231,12 +231,12 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest // At this point we know the vehicle is within the activation distance and we know the current and next light phases // All we need to now determine is if we should stop or if we should continue - intersection_speed_ = findSpeedLimit(nearest_traffic_light->getControlledLanelets().front()); + intersection_speed_ = findSpeedLimit(nearest_traffic_light->getControlStartLanelets().front()); ROS_DEBUG_STREAM("intersection_speed_: " << intersection_speed_.get()); intersection_end_downtrack_ = - wm_->routeTrackPos(nearest_traffic_light->getControlledLanelets().back().centerline2d().back()).downtrack; + wm_->routeTrackPos(nearest_traffic_light->getControlEndLanelets().back().centerline2d().back()).downtrack; ROS_DEBUG_STREAM("intersection_end_downtrack_: " << intersection_end_downtrack_.get()); @@ -280,9 +280,9 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true); // We will cross the light on the green phase even if we arrive early or late - if (early_arrival_state_at_freeflow_optional.get() == lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED && + if (early_arrival_state_at_freeflow_optional.get() == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED && late_arrival_state_at_freeflow_optional.get() == - lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) // Green light + lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) // Green light { ROS_DEBUG_STREAM("Planning lane follow and intersection transit maneuvers"); @@ -302,7 +302,7 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage( traffic_light_down_track - config_.vehicle_length, intersection_end_downtrack_.get(), intersection_speed_.get(), intersection_speed_.get(), light_arrival_time_at_freeflow, intersection_exit_time, crossed_lanelets.back().id(), - nearest_traffic_light->getControlledLanelets().back().id())); + nearest_traffic_light->getControlEndLanelets().back().id())); } else // Red or Yellow light { @@ -318,7 +318,7 @@ void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest void WzStrategicPlugin::planWhenWAITING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) + const std::vector& traffic_lights) { if (traffic_lights.empty()) { @@ -337,7 +337,7 @@ void WzStrategicPlugin::planWhenWAITING(const cav_srvs::PlanManeuversRequest& re if (!validLightState(current_light_state_optional, req.header.stamp)) return; - if (current_light_state_optional.get() == lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED) + if (current_light_state_optional.get() == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) { transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT); // If the light is green send the light transition // signal @@ -402,7 +402,7 @@ bool WzStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_ // Get current traffic light information ROS_DEBUG("\n\nFinding traffic_light information"); - auto traffic_list = wm_->getLightsAlongRoute({ req.veh_x, req.veh_y }); + auto traffic_list = wm_->getSignalsAlongRoute({ req.veh_x, req.veh_y }); TransitState prev_state; diff --git a/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp b/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp index f384dfaa85..5f079c1868 100644 --- a/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp +++ b/wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp @@ -43,17 +43,17 @@ TEST_F(WorkZoneTestFixture, supportedLightState) WzStrategicPluginConfig config; WzStrategicPlugin wzp(cmw_, config); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED)); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE)); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::STOP_AND_REMAIN)); - - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::UNAVAILABLE)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::DARK)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PRE_MOVEMENT)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC)); + ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); + ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); + ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); + + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::UNAVAILABLE)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::DARK)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PRE_MOVEMENT)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE)); + ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC)); } TEST_F(WorkZoneTestFixture, estimate_distance_to_stop) @@ -96,17 +96,17 @@ TEST_F(WorkZoneTestFixture, validLightState) WzStrategicPluginConfig config; WzStrategicPlugin wzp(cmw_, config); - ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_MOVEMENT_ALLOWED, ros::Time(1))); - ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::PROTECTED_CLEARANCE, ros::Time(1))); - ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficLightState::STOP_AND_REMAIN, ros::Time(1))); - - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::UNAVAILABLE, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::DARK, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::STOP_THEN_PROCEED, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PRE_MOVEMENT, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_MOVEMENT_ALLOWED, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::PERMISSIVE_CLEARANCE, ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficLightState::CAUTION_CONFLICTING_TRAFFIC, ros::Time(1))); + ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED, ros::Time(1))); + ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE, ros::Time(1))); + ASSERT_TRUE(wzp.validLightState(lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN, ros::Time(1))); + + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::UNAVAILABLE, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::DARK, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::PRE_MOVEMENT, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE, ros::Time(1))); + ASSERT_FALSE(wzp.validLightState(lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC, ros::Time(1))); ASSERT_FALSE(wzp.validLightState(boost::none, ros::Time(1))); }