diff --git a/.circleci/config.yml b/.circleci/config.yml index 54dcaebb13..f777779c2d 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,7 +28,7 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastol/autoware.ai:carma-system-4.1.0 + - image: usdotfhwastol/autoware.ai:carma-system-4.2.0 user: carma environment: TERM: xterm diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 5658936a30..b6bc2bd1ee 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -72,7 +72,9 @@ sonar.modules= bsm_generator, \ frame_transformer, \ object_visualizer, \ points_map_filter, \ - light_controlled_intersection_tactical_plugin + light_controlled_intersection_tactical_plugin, \ + platoon_control_ihp, \ + carma_guidance_plugins guidance.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/guidance bsm_generator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/bsm_generator @@ -117,7 +119,9 @@ frame_transformer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/frame_tran object_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/object_visualizer points_map_filter.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/points_map_filter light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/light_controlled_intersection_tactical_plugin +platoon_control_ihp.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_control_ihp lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/lci_strategic_plugin +carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_guidance_plugins # C++ Package differences # Sources @@ -164,7 +168,9 @@ frame_transformer.sonar.sources = src object_visualizer.sonar.sources = src points_map_filter.sonar.sources = src light_controlled_intersection_tactical_plugin.sonar.sources = src +platoon_control_ihp.sonar.sources = src lci_strategic_plugin.sonar.sources = src +carma_guidance_plugins.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. @@ -201,6 +207,9 @@ basic_autonomy.sonar.tests = test mobilitypath_visualizer.sonar.tests = test wz_strategic_plugin.sonar.tests = test sci_strategic_plugin.sonar.tests = test +platoon_control_ihp.sonar.tests = test + + stop_controlled_intersection_tactical_plugin.sonar.tests = test system_controller.sonar.sources = test subsystem_controllers.sonar.sources = test @@ -208,4 +217,7 @@ frame_transformer.sonar.sources = test object_visualizer.sonar.sources = test points_map_filter.sonar.sources = test light_controlled_intersection_tactical_plugin.sonar.tests = test +platoon_control_ihp_plugin.sonar.tests = test lci_strategic_plugin.sonar.sources = src +lci_strategic_plugin.sonar.sources = test +carma_guidance_plugins.sonar.sources = test diff --git a/Dockerfile b/Dockerfile index 3ac031e050..559b61a25e 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-4.1.0 AS base-image +FROM usdotfhwastol/autoware.ai:carma-system-4.2.0 AS base-image FROM base-image AS source-code @@ -47,6 +47,10 @@ RUN ~/src/carma-platform/docker/checkout.bash FROM base-image AS install +ARG ROS1_PACKAGES="" +ENV ROS1_PACKAGES=${ROS1_PACKAGES} +ARG ROS2_PACKAGES="" +ENV ROS2_PACKAGES=${ROS2_PACKAGES} # Copy the source files from the previous stage and build/install RUN mkdir ~/carma_ws diff --git a/arbitrator/config/arbitrator_params.yaml b/arbitrator/config/arbitrator_params.yaml index 7d7b3ac9fc..c14f6834a6 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: {LCIStrategicPlugin: 5.0, SCIStrategicPlugin: 4.0, RouteFollowingPlugin: 2.0, WorkZonePlugin: 4.0, PlatooningStrategicPlugin: 1.0} +plugin_priorities: {PlatooningStrategicIHPPlugin: 4.0, LCIStrategicPlugin: 3.0, SCIStrategicPlugin: 2.0, PlatooningStrategicPlugin: 2.0, RouteFollowingPlugin: 1.0} \ No newline at end of file diff --git a/basic_autonomy/include/basic_autonomy/basic_autonomy.h b/basic_autonomy/include/basic_autonomy/basic_autonomy.h index 9548490e78..dcc537b6fe 100644 --- a/basic_autonomy/include/basic_autonomy/basic_autonomy.h +++ b/basic_autonomy/include/basic_autonomy/basic_autonomy.h @@ -83,6 +83,7 @@ namespace basic_autonomy // computed curvature and output speeds 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; //The additional downtrack beyond requested end dist used to fit points along spline + std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory }; @@ -145,12 +146,13 @@ namespace basic_autonomy * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s * \param yaws The orientation the vehicle should achieve at each point. Units radians * \param startTime The absolute start time which will be used to update the input relative times. Units s + * \param desired_controller_plugin The name of the controller plugin for the generated trajectory. * * \return A list of trajectory points built from the provided inputs. */ std::vector trajectory_from_points_times_orientations( const std::vector &points, const std::vector ×, - const std::vector &yaws, ros::Time startTime); + const std::vector &yaws, ros::Time startTime, const std::string &desired_controller_plugin); /** * \brief Attaches back_distance length of points behind the future points @@ -337,7 +339,8 @@ namespace basic_autonomy int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, - double buffer_ending_downtrack); + double buffer_ending_downtrack, + std::string desired_controller_plugin = "default"); GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type, int default_downsample_ratio, diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index e6912a2374..3dec684ad5 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -75,9 +75,35 @@ namespace basic_autonomy std::vector points_and_target_speeds; cav_msgs::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver; - + auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack, true, true); + //TODO: This fix is temporary and only applies on lane follow maneuvers from Platooning Strategic Plugin IHP. + // A general fix will be implemented soon. (issue #1863) + bool lanelets_defined = !maneuver.lane_following_maneuver.lane_ids.empty(); + ROS_DEBUG_STREAM("lanelets_defined: " << lanelets_defined); + bool isFromPlatooning = maneuver.lane_following_maneuver.parameters.planning_strategic_plugin == "PlatooningStrategicIHPPlugin"; + ROS_DEBUG_STREAM("isFromPlatooning: " << isFromPlatooning); + + if (lanelets_defined && isFromPlatooning) + { + lanelets = {}; + int lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[0]); + ROS_DEBUG_STREAM("extracted id: " << lane_id); + lanelet::ConstLanelet new_lanelet = wm->getMap()->laneletLayer.get(lane_id); + lanelets.push_back(new_lanelet); + + if (maneuver.lane_following_maneuver.lane_ids.size()>1) + { + lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[1]); + ROS_DEBUG_STREAM("more extracted id: " << lane_id); + new_lanelet = wm->getMap()->laneletLayer.get(lane_id); + lanelets.push_back(new_lanelet); + } + } + + + if (lanelets.empty()) { ROS_ERROR_STREAM("Detected no lanelets between starting downtrack: "<< starting_downtrack << ", and lane_following_maneuver.end_dist: "<< lane_following_maneuver.end_dist); @@ -594,6 +620,7 @@ namespace basic_autonomy size_t time_boundary_exclusive_index = trajectory_utils::time_boundary_index(downtracks, speeds, time_span); + ROS_DEBUG_STREAM("time_boundary_exclusive_index = " << time_boundary_exclusive_index); if (time_boundary_exclusive_index == 0) { throw std::invalid_argument("No points to fit in timespan"); @@ -701,7 +728,7 @@ namespace basic_autonomy std::vector trajectory_from_points_times_orientations( const std::vector &points, const std::vector ×, const std::vector &yaws, - ros::Time startTime) + ros::Time startTime, const std::string &desired_controller_plugin) { if (points.size() != times.size() || points.size() != yaws.size()) { @@ -720,7 +747,7 @@ namespace basic_autonomy tpp.y = points[i].y(); tpp.yaw = yaws[i]; - tpp.controller_plugin_name = "default"; + tpp.controller_plugin_name = desired_controller_plugin; //tpp.planner_plugin_name //Planner plugin name is filled in the tactical plugin traj.push_back(tpp); @@ -795,6 +822,7 @@ namespace basic_autonomy ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index); std::vector future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position + ROS_DEBUG_STREAM("Ready to call constrain_to_time_boundary: future_points size = " << future_points.size() << ", trajectory_time_length = " << detailed_config.trajectory_time_length); auto time_bound_points = constrain_to_time_boundary(future_points, detailed_config.trajectory_time_length); ROS_DEBUG_STREAM("Got time_bound_points with size:" << time_bound_points.size()); @@ -914,7 +942,7 @@ namespace basic_autonomy std::vector yaw = {state.orientation, state.orientation}; //Keep current orientation std::vector traj_points = - trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time); + trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time, detailed_config.desired_controller_plugin); return traj_points; @@ -971,7 +999,7 @@ namespace basic_autonomy // Build trajectory points std::vector traj_points = - trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time); + trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin); //debug msg carma_debug_msgs::TrajectoryCurvatureSpeeds msg; @@ -1002,7 +1030,8 @@ namespace basic_autonomy int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, - double buffer_ending_downtrack) + double buffer_ending_downtrack, + std::string desired_controller_plugin) { DetailedTrajConfig detailed_config; @@ -1015,6 +1044,7 @@ namespace basic_autonomy detailed_config.curvature_moving_average_window_size = curvature_moving_average_window_size; detailed_config.back_distance = back_distance; detailed_config.buffer_ending_downtrack = buffer_ending_downtrack; + detailed_config.desired_controller_plugin = desired_controller_plugin; return detailed_config; } @@ -1101,7 +1131,7 @@ namespace basic_autonomy ROS_DEBUG_STREAM("After removing extra buffer points, future_geom_points.size():"<< future_geom_points.size()); std::vector traj_points = - trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time); + trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin); return traj_points; } diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp index 9d663dc608..b753a676ff 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -83,7 +83,7 @@ namespace basic_autonomy std::vector yaws = {0.2, 0.5, 0.6, 1.0}; ros::Time startTime(1.0); std::vector traj_points = - basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime); + basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime, "default"); ASSERT_EQ(4, traj_points.size()); ASSERT_NEAR(1.0, traj_points[0].target_time.toSec(), 0.0000001); diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index 6cdce44431..a0a7ca5a0d 100644 --- a/carma/launch/carma_src.launch.py +++ b/carma/launch/carma_src.launch.py @@ -126,6 +126,8 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/environment.launch.py']), launch_arguments = { 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'], + 'vehicle_config_param_file' : vehicle_config_param_file, + 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file }.items() ), ] @@ -166,6 +168,7 @@ def generate_launch_description(): launch_arguments={ 'route_file_folder' : route_file_folder, 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file, + 'vehicle_config_param_file' : vehicle_config_param_file, 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator, 'strategic_plugins_to_validate' : strategic_plugins_to_validate, 'tactical_plugins_to_validate' : tactical_plugins_to_validate, diff --git a/carma/launch/environment.launch b/carma/launch/environment.launch index 5ca62794b7..c26683b19c 100755 --- a/carma/launch/environment.launch +++ b/carma/launch/environment.launch @@ -42,18 +42,4 @@ - - - - - - - - - - - - - - diff --git a/carma/launch/environment.launch.py b/carma/launch/environment.launch.py index 71dd2714f5..9362439b90 100644 --- a/carma/launch/environment.launch.py +++ b/carma/launch/environment.launch.py @@ -1,4 +1,4 @@ -# Copyright (C) 2021 LEIDOS. +# Copyright (C) 2021-2022 LEIDOS. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -31,6 +31,21 @@ def generate_launch_description(): Launch perception nodes. """ + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') + declare_vehicle_config_param_file_arg = DeclareLaunchArgument( + name = 'vehicle_config_param_file', + default_value = "/opt/carma/vehicle/config/VehicleConfigParams.yaml", + description = "Path to file contain vehicle configuration parameters" + ) + + vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file') + declare_vehicle_characteristics_param_file_arg = DeclareLaunchArgument( + name = 'vehicle_characteristics_param_file', + default_value = "/opt/carma/vehicle/calibration/identifiers/UniqueVehicleParams.yaml", + description = "Path to file containing unique vehicle calibrations" + ) + + autoware_auto_launch_pkg_prefix = get_package_share_directory( 'autoware_auto_launch') @@ -69,6 +84,10 @@ def generate_launch_description(): get_package_share_directory('motion_computation'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + + carma_wm_ctrl_param_file = os.path.join( + get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml') + # lidar_perception_container contains all nodes for lidar based object perception # a failure in any one node in the chain would invalidate the rest of it, so they can all be @@ -206,7 +225,25 @@ def generate_launch_description(): executable='carma_component_container_mt', namespace=GetCurrentNamespace(), composable_node_descriptions=[ - + ComposableNode( + package='carma_wm_ctrl', + plugin='carma_wm_ctrl::WMBroadcasterNode', + name='carma_wm_broadcaster', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('carma_wm_ctrl', env_log_levels) } + ], + remappings=[ + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ), + ("incoming_map", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_map" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), + ("outgoing_geofence_ack", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + ("outgoing_geofence_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_geofence_request" ] ) + ], + parameters=[ carma_wm_ctrl_param_file, vehicle_config_param_file, vehicle_characteristics_param_file ] + ), ComposableNode( package='object_detection_tracking', plugin='object::ObjectDetectionTrackingNode', @@ -273,6 +310,30 @@ def generate_launch_description(): ], remappings=[ ("external_objects", "external_object_predictions"), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), + ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ) + ], + parameters = [ + vehicle_config_param_file + ] + ), + ComposableNode( + package='traffic_incident_parser', + plugin='traffic_incident_parser::TrafficIncidentParserNode', + name='traffic_incident_parser_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('traffic_incident_parser', env_log_levels) } + ], + remappings=[ + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("geofence", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_geofence_control" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), + ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ) + ], + parameters = [ + vehicle_config_param_file ] ), ] @@ -289,6 +350,8 @@ def generate_launch_description(): ) return LaunchDescription([ + declare_vehicle_characteristics_param_file_arg, + declare_vehicle_config_param_file_arg, declare_subsystem_controller_param_file_arg, lidar_perception_container, carma_external_objects_container, diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 6f6a69f082..d4a0462127 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -1,6 +1,6 @@ - - - - - - @@ -115,12 +109,12 @@ - - + + + @@ -181,12 +175,6 @@ - - - - - - @@ -207,11 +195,19 @@ - + + + + + + + + diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 7e0c26853a..e9014ccec9 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -42,11 +42,24 @@ def generate_launch_description(): tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate') control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate') + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') + declare_vehicle_config_param_file_arg = DeclareLaunchArgument( + name = 'vehicle_config_param_file', + default_value = "/opt/carma/vehicle/config/VehicleConfigParams.yaml", + description = "Path to file contain vehicle configuration parameters" + ) + subsystem_controller_default_param_file = os.path.join( get_package_share_directory('subsystem_controllers'), 'config/guidance_controller_config.yaml') mobilitypath_visualizer_param_file = os.path.join( get_package_share_directory('mobilitypath_visualizer'), 'config/params.yaml') + + trajectory_executor_param_file = os.path.join( + get_package_share_directory('trajectory_executor'), 'config/parameters.yaml') + + route_param_file = os.path.join( + get_package_share_directory('route'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -77,11 +90,50 @@ def generate_launch_description(): remappings = [ ("mobility_path_msg", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/mobility_path_msg" ] ), ("incoming_mobility_path", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_path" ] ), - ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NZ', default_value=''), "/map_param_loader/georeference"]) + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference"]) ], parameters=[ vehicle_characteristics_param_file, - mobilitypath_visualizer_param_file + mobilitypath_visualizer_param_file, + vehicle_config_param_file + ] + ), + ComposableNode( + package='trajectory_executor', + plugin='trajectory_executor::TrajectoryExecutor', + name='trajectory_executor_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('trajectory_executor', env_log_levels) } + ], + remappings = [ + ("trajectory", "plan_trajectory"), + ], + parameters=[ + trajectory_executor_param_file, + vehicle_config_param_file + ] + ), + ComposableNode( + package='route', + plugin='route::Route', + name='route_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route', env_log_levels) } + ], + remappings = [ + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), + ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), + ("roadway_objects", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/roadway_objects" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ) + ], + parameters=[ + {'route_file_path': route_file_folder}, + route_param_file, + vehicle_config_param_file ] ), ] @@ -98,6 +150,7 @@ def generate_launch_description(): ) return LaunchDescription([ + declare_vehicle_config_param_file_arg, declare_subsystem_controller_param_file_arg, carma_guidance_container, subsystem_controller diff --git a/carma_guidance_plugins/CMakeLists.txt b/carma_guidance_plugins/CMakeLists.txt new file mode 100644 index 0000000000..339fea3a6f --- /dev/null +++ b/carma_guidance_plugins/CMakeLists.txt @@ -0,0 +1,59 @@ + +# Copyright (C) 2022 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.5) +project(carma_guidance_plugins) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Name build targets +set(node_lib carma_guidance_plugins_node) + +# Includes +include_directories( + include +) + +# Build +ament_auto_add_library(${node_lib} SHARED + src/plugin_base_node.cpp + src/strategic_plugin.cpp + src/tactical_plugin.cpp + src/control_plugin.cpp +) + +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_carma_guidance_plugins test/node_test.cpp) + + ament_target_dependencies(test_carma_guidance_plugins ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_carma_guidance_plugins ${node_lib}) + +endif() + +# Install +ament_auto_package() diff --git a/carma_guidance_plugins/README.md b/carma_guidance_plugins/README.md new file mode 100644 index 0000000000..98fde956b4 --- /dev/null +++ b/carma_guidance_plugins/README.md @@ -0,0 +1,5 @@ +# carma_guidance_plugins + +This package provides a set of base classes to implement CARMA Platform Guidance Plugins API. You can read about plugins in the [CARMA Platform Architecture](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/89587713/CARMA+Platform+System+Architecture). The design of these base classes can be found [here](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2182545409/Detailed+Design+-+Plugin+Library). Using this library is not required as the plugin API is implemented entirely through ROS interfaces, however, using this package will minimize implementation errors. + +NOTE: At the moment these bases classes are single threaded only. diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp new file mode 100644 index 0000000000..7bd8efd77a --- /dev/null +++ b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/plugin_base_node.hpp" + +namespace carma_guidance_plugins +{ + + /** + * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API. + * + * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory. + * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system. + * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency. + * + */ + class ControlPlugin : public PluginBaseNode + { + + private: + // Subscriptions + carma_ros2_utils::SubPtr current_pose_sub_; + carma_ros2_utils::SubPtr current_velocity_sub_; + carma_ros2_utils::SubPtr trajectory_plan_sub_; + + // Publishers + carma_ros2_utils::PubPtr vehicle_cmd_pub_; + + // Timers + rclcpp::TimerBase::SharedPtr command_timer_; + + // These callbacks do direct assignment into their respective member variables + void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg); + void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg); + void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); + + + protected: + + //! The most recent pose message received by this node + boost::optional current_pose_; + + //! The most recent velocity message received by this node + // NOTE: Only the twist.linear.x and header are guaranteed to be populated + boost::optional current_twist_; + + //! The most recent trajectory received by this plugin + boost::optional current_trajectory_; + + + public: + /** + * \brief ControlPlugin constructor + */ + explicit ControlPlugin(const rclcpp::NodeOptions &); + + //! Virtual destructor for safe deletion + virtual ~ControlPlugin() = default; + + /** + * \brief Extending class provided method which should generate a command message + * which will be published to the required topic by the base class + * + * NOTE: Implementer can determine if trajectory has changed based on current_trajectory_->trajectory_id + * + * \return The command message to publish + */ + virtual autoware_msgs::msg::ControlCommandStamped generate_command() = 0; + + //// + // Overrides + //// + + // Non-Final to allow extending plugins to provide more detailed capabilities + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; + }; + +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp new file mode 100644 index 0000000000..72dac6ce1d --- /dev/null +++ b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2022 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 + +namespace carma_guidance_plugins +{ + + /** + * \brief PluginBaseNode provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to ROS2 lifecycle behavior), required interfaces, and plugin discovery + * + * Extending classes must implement the on_configure_plugin method to load parameters, and my override the other state transitions methods on__plugin if desired. + * Additionally, extending classes must implement the methods such as get_plugin_name() which are used to populate the plugin discovery message. + * + * NOTE: At the moment, this class and all extending classes are setup to support only single threading. + * + */ + class PluginBaseNode : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Publishers + //! Publisher for plugin discovery. This publisher will begin publishing + //immediately on node construction regardless of lifecycle state. This is meant to allow for fast plugin activation on startup. + rclcpp::Publisher::SharedPtr plugin_discovery_pub_; + + // Timers + //! Timer to trigger publication of the plugin discovery message at a fixed frequency + rclcpp::TimerBase::SharedPtr discovery_timer_; + + // WorldModel listener + // This variable is intentionally private, so that is can be lazily initialized + // when the extending class calls get_world_model_listener(); or get_world_model(); + std::shared_ptr wm_listener_; + + // World Model populated by the listener at runtime + // This variable is intentionally private, so that is can be lazily initialized + // when the extending class calls get_world_model_listener(); or get_world_model(); + carma_wm::WorldModelConstPtr wm_; + + /** + * \brief Callback for the plugin discovery timer which will publish the plugin discovery message + */ + void discovery_timer_callback(); + + /** + * \brief Helper function for lazy initialization of wm_listener_. If already initialized method returns (ie. not a reset) + */ + void lazy_wm_initialization(); + + public: + /** + * \brief PluginBaseNode constructor + */ + explicit PluginBaseNode(const rclcpp::NodeOptions &); + + //! Virtual destructor for safe deletion + virtual ~PluginBaseNode() = default; + + /** + * \brief Method to return the default world model listener provided as a convience by this base class + * If this method or get_world_model() are not called then the world model remains uninitialized and + * will not create unnecessary subscriptions. + * + * \return Pointer to an initialized world model listener + */ + virtual std::shared_ptr get_world_model_listener() final; + + /** + * \brief Method to return the default world model provided as a convience by this base class + * If this method or get_world_model_listener() are not called then the world model remains uninitialized and + * will not create unnecessary subscriptions. + * + * \return Pointer to an initialized world model. Returned instance is that same as get_world_model_listener()->getWorldModel(); + */ + virtual carma_wm::WorldModelConstPtr get_world_model() final; + + /** + * \brief Returns the activation status of this plugin. + * The plugins API callbacks will only be triggered when this method returns true. + * + * \return True if plugin is in the ACTIVE state. False otherwise. + */ + virtual bool get_activation_status() final; + + + /** + * \brief Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. + * Extending classes for the specific type should override this method. + * + * \return The extending class type or UNKOWN if the class or no parent class has implement this method. + */ + virtual uint8_t get_type(); + + /** + * \brief Get the availability status of this plugin based on the current operating environment. + * Method must be overriden by extending classes. + * + * \return This method should return true if the plugin's current understanding of the world + * means it would be capable of planning or executing its capability. + */ + virtual bool get_availability() = 0; + + /** + * \brief Get the capability string representing this plugins capabilities + * Method must be overriden by extending classes. + * Expectation is that abstract plugin type parent classes will provide a default implementation. + * + * \return Forward slash separated string describing the plugin's capabilities per the plugin capabilites API + */ + virtual std::string get_capability() = 0; + + /** + * \brief Returns the name of this plugin. + * This name may be automatically prepended to plugin API service or topic names. + * + * \return Name of this plugin + */ + virtual std::string get_plugin_name() = 0; + + /** + * \brief Returns the version id of this plugin. + * + * \return The version id represented as a string + */ + virtual std::string get_version_id() = 0; + + /** + * \brief Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. + * This method should be used to load parameters and is required to be implemented. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_configure_plugin() = 0; + + /** + * \brief Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. + * This method should be used to prepare for future callbacks for plugin's capabilites. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to ACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_activate_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. + * This method should be used to disable any functionality which should cease execution when plugin is inactive. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_deactivate_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. + * This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin + * in a fresh state as though just launched. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to UNCONFIGURED will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_cleanup_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from any state to FINALIZED + * This method should be used to generate any shutdown logs or final cleanup. + * + * \return SUCCESS, FAILURE, or ERROR. On ERROR, may temporarily go to on_error_plugin() before calling finalized. + */ + virtual carma_ros2_utils::CallbackReturn on_shutdown_plugin(); + + /** + * \brief Method which is triggered when an unhandled exception occurs in this plugin + * This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. + * + * \param exception_string The exception description + * + * \return SUCCESS, FAILURE, or ERROR. On SUCCESS plugin moves to UNCONFIGURED state else FINALIZED. Default behavior is to move to FINALIZED. + */ + virtual carma_ros2_utils::CallbackReturn on_error_plugin(const std::string &exception_string); + + //// + // Overrides + //// + // For simplicity of managing the plugin state machine all lifecycle callbacks are implemented as final by the core extending classes (strategic, tactical, control) + // Plugins will use the plugin specific callbacks via the template pattern + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override; + + // Unit Test Accessors + FRIEND_TEST(carma_guidance_plugins_test, connections_test); + + }; + +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp new file mode 100644 index 0000000000..1fd79f4753 --- /dev/null +++ b/carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/plugin_base_node.hpp" + +namespace carma_guidance_plugins +{ + + /** + * \brief StrategicPlugin base class which can be extended by user provided plugins which wish to implement the Strategic Plugin ROS API. + * A strategic plugin is responsible for planning sequences of maneuvers to achieve high-level objects for the vehicle (follow the route, efficiently get through an intersection, etc.) + * + */ + class StrategicPlugin : public PluginBaseNode + { + + private: + // Services + + //! The service which will be called when a strategic plugin needs to plan maneuvers + carma_ros2_utils::ServicePtr plan_maneuvers_service_; + + public: + /** + * \brief StrategicPlugin constructor + */ + explicit StrategicPlugin(const rclcpp::NodeOptions &); + + //! Virtual destructor for safe deletion + virtual ~StrategicPlugin() = default; + + /** + * \brief Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. + * + * \param srv_header RCL header for services calls. Can usually be ignored by implementers. + * \param req The service request containing the previously planned maneuvers and vehicle state + * \param resp The response containing the additional maneuvers generated by this plugin + */ + virtual void plan_maneuvers_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) = 0; + + + //// + // Overrides + //// + + // Non-Final to allow extending plugins to provide more detailed capabilities + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; + }; + +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp new file mode 100644 index 0000000000..f3c9593960 --- /dev/null +++ b/carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/plugin_base_node.hpp" + +namespace carma_guidance_plugins +{ + + /** + * \brief TacticalPlugin base class which can be extended by user provided plugins which wish to implement the Tactical Plugin ROS API. + * + * A tactical plugin is responsible for planning the detailed trajectory which a vehicle should execute to complete a maneuver. + * + */ + class TacticalPlugin : public PluginBaseNode + { + + private: + // Services + //! The ros service which can be called by the arbitrator or other plugins to have this plugin generate a trajectory plan + carma_ros2_utils::ServicePtr plan_trajectory_service_; + + public: + /** + * \brief TacticalPlugin constructor + */ + explicit TacticalPlugin(const rclcpp::NodeOptions &); + + //! Virtual destructor for safe deletion + virtual ~TacticalPlugin() = default; + + /** + * \brief Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. + * + * \param srv_header RCL header for services calls. Can usually be ignored by implementers. + * \param req The service request containing the maneuvers to plan trajectories for and current vehicle state + * \param resp The response containing the planned trajectory + */ + virtual void plan_trajectory_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) = 0; + + + //// + // Overrides + //// + + // Non-Final to allow extending plugins to provide more detailed capabilities + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; + }; + +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/package.xml b/carma_guidance_plugins/package.xml new file mode 100644 index 0000000000..9db826f225 --- /dev/null +++ b/carma_guidance_plugins/package.xml @@ -0,0 +1,41 @@ + + + + + + carma_guidance_plugins + 1.0.0 + This package provides a set of base classes to implement CARMA Platform Guidance Plugins API + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + carma_planning_msgs + autoware_msgs + carma_wm_ros2 + + ament_lint_auto + ament_cmake_gtest + + + ament_cmake + + diff --git a/carma_guidance_plugins/src/control_plugin.cpp b/carma_guidance_plugins/src/control_plugin.cpp new file mode 100644 index 0000000000..401a748be6 --- /dev/null +++ b/carma_guidance_plugins/src/control_plugin.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/control_plugin.hpp" + + +namespace carma_guidance_plugins +{ + namespace std_ph = std::placeholders; + + ControlPlugin::ControlPlugin(const rclcpp::NodeOptions &options) + : PluginBaseNode(options) + {} + + std::string ControlPlugin::get_capability() + { + return "control/trajectory_control"; + } + + uint8_t ControlPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::CONTROL; + } + + void ControlPlugin::current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg) + { + RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received pose message"); + current_pose_ = *msg; + } + + void ControlPlugin::current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg) + { + RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received twist message"); + current_twist_ = *msg; + } + + void ControlPlugin::current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) + { + RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received trajectory message"); + current_trajectory_ = *msg; + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize subscribers and publishers + current_pose_sub_ = create_subscription("current_pose", 1, + std::bind(&ControlPlugin::current_pose_callback, this, std_ph::_1)); + + current_velocity_sub_ = create_subscription("vehicle/twist", 1, + std::bind(&ControlPlugin::current_twist_callback, this, std_ph::_1)); + + trajectory_plan_sub_ = create_subscription(get_plugin_name() + "/plan_trajectory", 1, + std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1)); + + vehicle_cmd_pub_ = create_publisher("ctrl_raw", 1); + + command_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(33), // Spin at 30 Hz per plugin API + [this]() { + if (this->get_activation_status()) // Only trigger when activated + { + this->vehicle_cmd_pub_->publish(this->generate_command()); + } + }); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) + { + return PluginBaseNode::handle_on_error(prev_state, exception_string); + } + +} // carma_guidance_plugins + diff --git a/carma_guidance_plugins/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp new file mode 100644 index 0000000000..358a3ed835 --- /dev/null +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/plugin_base_node.hpp" + +namespace carma_guidance_plugins +{ + namespace std_ph = std::placeholders; + + PluginBaseNode::PluginBaseNode(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) + { + + // Setup discovery timer to publish onto the plugin_discovery_pub + discovery_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(500), // 2 Hz frequency to account for 1Hz maneuver planning frequency + std::bind(&PluginBaseNode::discovery_timer_callback, this)); + } + + void PluginBaseNode::lazy_wm_initialization() + { + if (wm_listener_) + return; // Already initialized + + + wm_listener_ = std::make_shared( + this->get_node_base_interface(), this->get_node_logging_interface(), + this->get_node_topics_interface(), this->get_node_parameters_interface() + ); + + wm_ = wm_listener_->getWorldModel(); + } + + std::shared_ptr PluginBaseNode::get_world_model_listener() + { + lazy_wm_initialization(); + return wm_listener_; + } + + carma_wm::WorldModelConstPtr PluginBaseNode::get_world_model() + { + lazy_wm_initialization(); + return wm_; + } + + bool PluginBaseNode::get_activation_status() { + // Determine the plugin activation state by checking which lifecycle state we are in. + // If we are active then the plugin is active otherwise the plugin is inactive + return get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_activate_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_deactivate_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_cleanup_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_shutdown_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_error_plugin(const std::string &) + { + // On error should default to failure so user must explicitly implement error handling to get any other behavior + return carma_ros2_utils::CallbackReturn::FAILURE; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_configure(const rclcpp_lifecycle::State &) + { + return on_configure_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_activate(const rclcpp_lifecycle::State &) + { + return on_activate_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_deactivate(const rclcpp_lifecycle::State &) + { + return on_deactivate_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_cleanup(const rclcpp_lifecycle::State &) + { + return on_cleanup_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_shutdown(const rclcpp_lifecycle::State &) + { + return on_shutdown_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) + { + return on_error_plugin(exception_string); + } + + uint8_t PluginBaseNode::get_type() + { + // Base class returns unknown. + // Its expected that 2nd level extending classes (strategic, tactical, control) will return correct type by overriding + return carma_planning_msgs::msg::Plugin::UNKNOWN; + } + + void PluginBaseNode::discovery_timer_callback() + { + + if (!plugin_discovery_pub_) // If the publisher has not been initalized then initialize it. + { + // Setup plugin publisher discovery which should bypass lifecycle behavior to ensure plugins are found + // prior to them needing to be activated. + // NOTE: Any other topics which need to be setup in the future should use handle_on_configure and the default this->create_publisher method to get a lifecycle publisher instead + auto this_shared = shared_from_this(); // Usage of shared_from_this() means this cannot be done in the constructor thus it is delegated to the timer callback + plugin_discovery_pub_ = rclcpp::create_publisher(this_shared, "plugin_discovery", 1); + + } + + carma_planning_msgs::msg::Plugin msg; + msg.name = get_plugin_name(); + msg.version_id = get_version_id(); + msg.type = get_type(); + msg.available = get_availability(); + msg.activated = get_activation_status(); + msg.capability = get_capability(); + + plugin_discovery_pub_->publish(msg); + } + +} // carma_guidance_plugins + diff --git a/carma_guidance_plugins/src/strategic_plugin.cpp b/carma_guidance_plugins/src/strategic_plugin.cpp new file mode 100644 index 0000000000..9eb2868af8 --- /dev/null +++ b/carma_guidance_plugins/src/strategic_plugin.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/strategic_plugin.hpp" + +namespace carma_guidance_plugins +{ + namespace std_ph = std::placeholders; + + StrategicPlugin::StrategicPlugin(const rclcpp::NodeOptions &options) + : PluginBaseNode(options) + {} + + std::string StrategicPlugin::get_capability() + { + return "strategic_plan/plan_maneuvers"; + } + + uint8_t StrategicPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::STRATEGIC; + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize plan maneuvers service + plan_maneuvers_service_ = create_service(get_plugin_name() + "/plan_maneuvers", + [this] (auto header, auto req, auto resp) { + if (this->get_activation_status()) // Only trigger when activated + { + this->plan_maneuvers_callback(header, req, resp); + } + }); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) + { + return PluginBaseNode::handle_on_error(prev_state, exception_string); + } + +} // carma_guidance_plugins + diff --git a/carma_guidance_plugins/src/tactical_plugin.cpp b/carma_guidance_plugins/src/tactical_plugin.cpp new file mode 100644 index 0000000000..024e3250e6 --- /dev/null +++ b/carma_guidance_plugins/src/tactical_plugin.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/tactical_plugin.hpp" + +namespace carma_guidance_plugins +{ + namespace std_ph = std::placeholders; + + TacticalPlugin::TacticalPlugin(const rclcpp::NodeOptions &options) + : PluginBaseNode(options) + {} + + std::string TacticalPlugin::get_capability() + { + return "tactical_plan/plan_trajectory"; + } + + uint8_t TacticalPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::TACTICAL; + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize plan trajectory service + plan_trajectory_service_ = create_service(get_plugin_name() + "/plan_trajectory", + [this] (auto header, auto req, auto resp) { + if (this->get_activation_status()) // Only trigger when activated + { + this->plan_trajectory_callback(header, req, resp); + } + }); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) + { + return PluginBaseNode::handle_on_error(prev_state, exception_string); + } + +} // carma_guidance_plugins + diff --git a/carma_guidance_plugins/test/TestPlugins.h b/carma_guidance_plugins/test/TestPlugins.h new file mode 100644 index 0000000000..5a8b0daf29 --- /dev/null +++ b/carma_guidance_plugins/test/TestPlugins.h @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/strategic_plugin.hpp" +#include "carma_guidance_plugins/tactical_plugin.hpp" +#include "carma_guidance_plugins/control_plugin.hpp" + +namespace carma_guidance_plugins +{ + class TestStrategicPlugin : public StrategicPlugin + { + public: + + using StrategicPlugin::StrategicPlugin; + + ~TestStrategicPlugin() override = default; + + + void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override + { + // Intentionally unimplemented... + } + + bool get_availability() override { + return true; + } + + std::string get_plugin_name() override { + return "TestStrategicPlugin"; + } + + std::string get_version_id() override { + return "1.0"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestStrategicPlugin"); + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return StrategicPlugin::get_capability() + "/test_capability"; + }; + + }; + + class TestTacticalPlugin : public TacticalPlugin + { + public: + + using TacticalPlugin::TacticalPlugin; + + ~TestTacticalPlugin() override = default; + + + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override + { + // Intentionally unimplemented... + } + + bool get_availability() override { + return true; + } + + std::string get_plugin_name() override { + return "TestTacticalPlugin"; + } + + std::string get_version_id() override { + return "1.1"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestTacticalPlugin"); + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return TacticalPlugin::get_capability() + "/test_capability"; + }; + + }; + + class TestControlPlugin : public ControlPlugin + { + public: + + using ControlPlugin::ControlPlugin; + + ~TestControlPlugin() override = default; + + + autoware_msgs::msg::ControlCommandStamped generate_command() override + { + autoware_msgs::msg::ControlCommandStamped msg; + return msg; + } + + bool get_availability() override { + return true; + } + + std::string get_plugin_name() override { + return "TestControlPlugin"; + } + + std::string get_version_id() override { + return "1.2"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestControlPlugin"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return ControlPlugin::get_capability() + "/test_capability"; + }; + + }; +} \ No newline at end of file diff --git a/carma_guidance_plugins/test/node_test.cpp b/carma_guidance_plugins/test/node_test.cpp new file mode 100644 index 0000000000..0beb9e7659 --- /dev/null +++ b/carma_guidance_plugins/test/node_test.cpp @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2022 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 "carma_guidance_plugins/plugin_base_node.hpp" +#include "TestPlugins.h" + +bool has_publisher(std::shared_ptr node, const std::string& topic_name, const std::string& type) +{ + bool found = false; + for (const auto& endpoint : node->get_publishers_info_by_topic(topic_name)) { + + std::cerr << "name: " << endpoint.node_name() << " type: " << endpoint.topic_type() << std::endl; + + if (endpoint.node_name() == node->get_name() && endpoint.topic_type() == type) + found = true; + } + + return found; +} + +bool has_subscriber(std::shared_ptr node, const std::string& topic_name, const std::string& type) +{ + bool found = false; + for (const auto& endpoint : node->get_subscriptions_info_by_topic(topic_name)) { + + std::cerr << "name: " << endpoint.node_name() << " type: " << endpoint.topic_type() << std::endl; + + if (endpoint.node_name() == node->get_name() && endpoint.topic_type() == type) + found = true; + } + + return found; +} + +bool has_service(std::shared_ptr node, std::string_view service_name, std::string_view type) +{ + bool found = false; + for (const auto& [service_k, type_v] : node->get_service_names_and_types_by_node(node->get_name(), "")) { + + std::cerr << "service: " << service_k << " type: " << type_v[0] << std::endl; + + if ( service_k == service_name && type_v[0] == type) + found = true; + } + + return found; +} + +namespace carma_guidance_plugins +{ + +TEST(carma_guidance_plugins_test, connections_test) { + + + std::vector str_remaps = {"--ros-args", "-r", "__node:=strategic_plugin_test"}; + std::vector tac_remaps = {"--ros-args", "-r", "__node:=tactical_plugin_test"}; + std::vector ctrl_remaps = {"--ros-args", "-r", "__node:=control_plugin_test"}; + + rclcpp::NodeOptions str_options; + str_options.arguments(str_remaps); + str_options.use_intra_process_comms(true); + + rclcpp::NodeOptions tac_options; + tac_options.arguments(tac_remaps); + tac_options.use_intra_process_comms(true); + + rclcpp::NodeOptions ctrl_options; + ctrl_options.arguments(ctrl_remaps); + ctrl_options.use_intra_process_comms(true); + + auto strategic_plugin = std::make_shared(str_options); + auto tactical_plugin = std::make_shared(tac_options); + auto control_plugin = std::make_shared(ctrl_options); + + ASSERT_TRUE(strategic_plugin->get_availability()); + ASSERT_TRUE(tactical_plugin->get_availability()); + ASSERT_TRUE(control_plugin->get_availability()); + + ASSERT_EQ(strategic_plugin->get_plugin_name(), "TestStrategicPlugin"); + ASSERT_EQ(tactical_plugin->get_plugin_name(), "TestTacticalPlugin"); + ASSERT_EQ(control_plugin->get_plugin_name(), "TestControlPlugin"); + + ASSERT_EQ(strategic_plugin->get_version_id(), "1.0"); + ASSERT_EQ(tactical_plugin->get_version_id(), "1.1"); + ASSERT_EQ(control_plugin->get_version_id(), "1.2"); + + ASSERT_EQ(strategic_plugin->get_capability(), "strategic_plan/plan_maneuvers/test_capability"); + ASSERT_EQ(tactical_plugin->get_capability(), "tactical_plan/plan_trajectory/test_capability"); + ASSERT_EQ(control_plugin->get_capability(), "control/trajectory_control/test_capability"); + + // Trigger the discovery callback once to ensure discovery publisher is setup + strategic_plugin->discovery_timer_callback(); + tactical_plugin->discovery_timer_callback(); + control_plugin->discovery_timer_callback(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Give a bit for publisher registration to go through + + ASSERT_TRUE(has_publisher(strategic_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_service(strategic_plugin, "/TestStrategicPlugin/plan_maneuvers", "carma_planning_msgs/srv/PlanManeuvers")); + + ASSERT_TRUE(has_publisher(tactical_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_service(tactical_plugin, "/TestTacticalPlugin/plan_trajectory", "carma_planning_msgs/srv/PlanTrajectory")); + + ASSERT_TRUE(has_publisher(control_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_publisher(control_plugin, "ctrl_raw", "autoware_msgs/msg/ControlCommandStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "current_pose", "geometry_msgs/msg/PoseStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "vehicle/twist", "geometry_msgs/msg/TwistStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "TestControlPlugin/plan_trajectory", "carma_planning_msgs/msg/TrajectoryPlan")); + + strategic_plugin->configure(); + tactical_plugin->configure(); + control_plugin->configure(); + + ASSERT_TRUE(has_publisher(strategic_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_TRUE(has_service(strategic_plugin, "/TestStrategicPlugin/plan_maneuvers", "carma_planning_msgs/srv/PlanManeuvers")); + + ASSERT_TRUE(has_publisher(tactical_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_TRUE(has_service(tactical_plugin, "/TestTacticalPlugin/plan_trajectory", "carma_planning_msgs/srv/PlanTrajectory")); + + ASSERT_TRUE(has_publisher(control_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); + ASSERT_TRUE(has_publisher(control_plugin, "ctrl_raw", "autoware_msgs/msg/ControlCommandStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "current_pose", "geometry_msgs/msg/PoseStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "vehicle/twist", "geometry_msgs/msg/TwistStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "TestControlPlugin/plan_trajectory", "carma_planning_msgs/msg/TrajectoryPlan")); + +} + +} // carma_guidance_plugins + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/carma_wm/include/carma_wm/CARMAWorldModel.h b/carma_wm/include/carma_wm/CARMAWorldModel.h index 4bca4ebc01..1fe4285a1e 100644 --- a/carma_wm/include/carma_wm/CARMAWorldModel.h +++ b/carma_wm/include/carma_wm/CARMAWorldModel.h @@ -160,6 +160,20 @@ class CARMAWorldModel : public WorldModel */ lanelet::CarmaTrafficSignalPtr getTrafficSignal(const lanelet::Id& id) const; + /*! \brief update minimum end time to account for minute of the year + * \param min_end_time minimum end time of the spat movement event list + * \param moy_exists tells weather minute of the year exist or not + * \param moy value of the minute of the year + */ + boost::posix_time::ptime min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy=0); + + /*! \brief for cheking previous rate to avoid repetation. + * \param min_end_time_dynamic dynamic spat processing minimum end time + * \param received_state_dynamic phase rate of the movement event list event state + * \param mov_id id of the traffic signal states + * \param mov_signal_group signal group of the traffic signal states + */ + bool check_if_seen_before_movement_state(boost::posix_time::ptime min_end_time_dynamic,lanelet::CarmaTrafficSignalState received_state_dynamic,uint16_t mov_id, uint8_t mov_signal_group); /** * \brief (non-const version) Gets the underlying lanelet, given the cartesian point on the map * diff --git a/carma_wm/include/carma_wm/SignalizedIntersectionManager.h b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h index 63054fe85e..c42b617a82 100644 --- a/carma_wm/include/carma_wm/SignalizedIntersectionManager.h +++ b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h @@ -141,6 +141,9 @@ class SignalizedIntersectionManager */ lanelet::Lanelets identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr& map); + // SignalizedIntersection's reference point correction pair of (x, y) for each intersection_id + std::unordered_map> intersection_coord_correction_; + // SignalizedIntersection quick id lookup std::unordered_map intersection_id_to_regem_id_; @@ -168,6 +171,8 @@ class SignalizedIntersectionManager // 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/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index 96b382f141..d26958505c 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -468,8 +468,8 @@ namespace carma_wm double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis double theta = sigma + M_PI_2; // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point // to the target point - double delta_x = cos(theta) * crosstrack; - double delta_y = sin(theta) * crosstrack; + double delta_x = cos(theta) * -crosstrack; // Sign of cross track must be flipped since map frame is right handed but TrackPos frame is left handed + double delta_y = sin(theta) * -crosstrack; return lanelet::BasicPoint2d(x + delta_x, y + delta_y); } @@ -502,8 +502,8 @@ namespace carma_wm double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis double theta = sigma + M_PI_2; // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point // to the target point - double delta_x = cos(theta) * crosstrack; - double delta_y = sin(theta) * crosstrack; + double delta_x = cos(theta) * -crosstrack; // Sign of cross track must be flipped since map frame is right handed but TrackPos frame is left handed + double delta_y = sin(theta) * -crosstrack; x += delta_x; // Adjust x and y of target point to account for crosstrack y += delta_y; @@ -1378,6 +1378,66 @@ namespace carma_wm return curr_light; } + bool CARMAWorldModel::check_if_seen_before_movement_state(boost::posix_time::ptime min_end_time_dynamic,lanelet::CarmaTrafficSignalState received_state_dynamic,uint16_t mov_id, uint8_t mov_signal_group) + { + + if(sim_.traffic_signal_states_[mov_id][mov_signal_group].empty()) + { + return false; + } + + std::vector> temp_signal_states; + + for(auto mov_check:sim_.traffic_signal_states_[mov_id][mov_signal_group]) + { + if (lanelet::time::timeFromSec(ros::Time::now().toSec()) > mov_check.first) + { + temp_signal_states.push_back(std::make_pair(mov_check.first, mov_check.second )); + } + + auto last_time_difference = mov_check.first - min_end_time_dynamic; + bool is_duplicate = last_time_difference.total_milliseconds() >= -500 && last_time_difference.total_milliseconds() <= 500; + + if(received_state_dynamic == mov_check.second && is_duplicate) + { + return true; + } + + } + sim_.traffic_signal_states_[mov_id][mov_signal_group]=temp_signal_states; + return false; + + } + + boost::posix_time::ptime CARMAWorldModel::min_end_time_converter_minute_of_year(boost::posix_time::ptime min_end_time,bool moy_exists,uint32_t moy) + { + if (moy_exists) //account for minute of the year + { + auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch + auto duration_since_inception(lanelet::time::durationFromSec(ros::Time::now().toSec())); + auto curr_time_boost = inception_boost + duration_since_inception; + + int curr_year = curr_time_boost.date().year(); + auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000")); + + auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((int)moy); + + int hours_of_day = curr_minute_stamp_boost.time_of_day().hours(); + int curr_month = curr_minute_stamp_boost.date().month(); + int curr_day = curr_minute_stamp_boost.date().day(); + + auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard + auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day); + + min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost)); + return min_end_time; + } + else + { + return min_end_time; // return unchanged + } + } + void CARMAWorldModel::processSpatFromMsg(const cav_msgs::SPAT &spat_msg) { if (!semantic_map_) @@ -1424,30 +1484,31 @@ namespace carma_wm continue; } + if(current_movement_state.movement_event_list.size()>1) // Dynamic Spat Processing with future phases + { + for(auto current_movement_event:current_movement_state.movement_event_list) + { + + // raw min_end_time in seconds measured from the most recent full hour + boost::posix_time::ptime min_end_time_dynamic = lanelet::time::timeFromSec(current_movement_event.timing.min_end_time); + min_end_time_dynamic=min_end_time_converter_minute_of_year(min_end_time_dynamic,curr_intersection.moy_exists,curr_intersection.moy); // Accounting minute of the year + auto received_state_dynamic = static_cast(current_movement_event.event_state.movement_phase_state); + + bool recorded = check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,curr_intersection.id.id,current_movement_state.signal_group); + if (!recorded) + { + sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time_dynamic, received_state_dynamic)); + } + } + } + else // Fixed Spat Processing without future phases + { + // raw min_end_time in seconds measured from the most recent full hour boost::posix_time::ptime min_end_time = lanelet::time::timeFromSec(current_movement_state.movement_event_list[0].timing.min_end_time); auto received_state = static_cast(current_movement_state.movement_event_list[0].event_state.movement_phase_state); - if (curr_intersection.moy_exists) //account for minute of the year - { - auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch - auto duration_since_inception(lanelet::time::durationFromSec(ros::Time::now().toSec())); - auto curr_time_boost = inception_boost + duration_since_inception; - - int curr_year = curr_time_boost.date().year(); - auto curr_year_start_boost(boost::posix_time::time_from_string(std::to_string(curr_year)+ "-01-01 00:00:00.000")); - - auto curr_minute_stamp_boost = curr_year_start_boost + boost::posix_time::minutes((int)curr_intersection.moy); - - int hours_of_day = curr_minute_stamp_boost.time_of_day().hours(); - int curr_month = curr_minute_stamp_boost.date().month(); - int curr_day = curr_minute_stamp_boost.date().day(); - - auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard - auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day); - - min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost)); - } + min_end_time=min_end_time_converter_minute_of_year(min_end_time,curr_intersection.moy_exists,curr_intersection.moy); auto last_time_difference = sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group].first - min_end_time; bool is_duplicate = last_time_difference.total_milliseconds() >= -500 && last_time_difference.total_milliseconds() <= 500; @@ -1576,7 +1637,7 @@ namespace carma_wm sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group].push_back(std::make_pair(min_end_time, received_state)); sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group]++; ROS_DEBUG_STREAM("Counter now: " << sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] << ", for id: "<< curr_light_id); - } + } } } } diff --git a/carma_wm/src/SignalizedIntersectionManager.cpp b/carma_wm/src/SignalizedIntersectionManager.cpp index f92c139f90..ff38d678d2 100644 --- a/carma_wm/src/SignalizedIntersectionManager.cpp +++ b/carma_wm/src/SignalizedIntersectionManager.cpp @@ -88,6 +88,14 @@ namespace carma_wm double curr_x = ref_node.x(); double curr_y = ref_node.y(); + if (intersection_coord_correction_.find(intersection.id.id) != intersection_coord_correction_.end()) + { + curr_x += intersection_coord_correction_[intersection.id.id].first; + curr_y += intersection_coord_correction_[intersection.id.id].second; + ROS_DEBUG_STREAM("Applied reference point correction, delta_x: " << intersection_coord_correction_[intersection.id.id].first << + ", delta_y: " << intersection_coord_correction_[intersection.id.id].second << ", to intersection id: " << (int)lane.lane_id); + } + ROS_DEBUG_STREAM("Processing Lane id: " << (int)lane.lane_id); size_t min_number_of_points = 2; // two points minimum are required @@ -122,7 +130,7 @@ namespace carma_wm for (auto node : node_list) { - ROS_DEBUG_STREAM("x: " << node.x() << ", y: " << node.y()); + ROS_DEBUG_STREAM(node.x() << ", " << node.y()); } // save which signal group connect to which exit lanes @@ -431,6 +439,7 @@ namespace carma_wm this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_; this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_; this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_; + this->intersection_coord_correction_ = other.intersection_coord_correction_; return *this; } @@ -441,5 +450,6 @@ namespace carma_wm this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_; this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_; this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_; + this->intersection_coord_correction_ = other.intersection_coord_correction_; } } // namespace carma_wm diff --git a/carma_wm/src/WorldModelUtils.cpp b/carma_wm/src/WorldModelUtils.cpp index 1bbc5981ff..edc625c478 100644 --- a/carma_wm/src/WorldModelUtils.cpp +++ b/carma_wm/src/WorldModelUtils.cpp @@ -168,6 +168,24 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& 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()); + + if (affected_lanelets.empty() && !possible_lanelets.empty() && idx != 0) + { + ROS_DEBUG_STREAM("Checking if it is the edge case where only last point falls on a valid (correct direction) lanelet"); + for (auto llt: possible_lanelets) + { + ROS_DEBUG_STREAM("Evaluating lanelet: " << llt.id()); + lanelet::BasicLineString2d gf_dir_line({gf_pts[idx - 1].basicPoint2d(), gf_pts[idx].basicPoint2d()}); + lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().begin())->basicPoint2d(), (llt.rightBound2d().begin())->basicPoint2d()}); + + // record the llts that are on the same dir + if (boost::geometry::intersects(llt_boundary, gf_dir_line)) + { + ROS_DEBUG_STREAM("Overlaps starting line... Picking llt: " << llt.id()); + affected_lanelets.insert(llt); + } + } + } break; } @@ -214,7 +232,8 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& 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); + if (interior_angle < M_PI_2 && interior_angle >= 0) + affected_lanelets.insert(llt); } else { diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index 2b7c0d4cd8..83e2d63159 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -1159,7 +1159,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().front().y(), 0.001); pos.downtrack = 40.0; @@ -1168,7 +1168,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1203); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 12.5; @@ -1177,7 +1177,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), 12.5, 0.001); pos.downtrack = 10.0; @@ -1185,9 +1185,13 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) if (!point) { FAIL() << "No point returned"; } + auto alt_point = wm->routeTrackPos(*point); // Verify the output is equivalent to the inverse function lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); + + ASSERT_NEAR(alt_point.downtrack, pos.downtrack, 0.001); + ASSERT_NEAR(alt_point.crosstrack, pos.crosstrack, 0.001); pos.downtrack = 20.0; point = wm->pointFromRouteTrackPos(pos); // Test lanelet connection point @@ -1195,7 +1199,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1201); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); //////////// @@ -1208,7 +1212,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().front().y(), 0.001); pos.downtrack = 40.0; @@ -1217,7 +1221,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1203); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 12.5; @@ -1226,7 +1230,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), 12.5, 0.001); pos.downtrack = 10.0; @@ -1235,7 +1239,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 20.0; @@ -1244,7 +1248,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1201); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); } @@ -1545,5 +1549,48 @@ TEST(CARMAWorldModelTest, getIntersectionAlongRoute) } +TEST(CARMAWorldModelTest, checkIfSeenBeforeMovementState) +{ + carma_wm::CARMAWorldModel cmw; + + cmw.sim_.traffic_signal_states_[13][15].push_back(std::make_pair(boost::posix_time::time_from_string("1970-01-01 00:00:00.000"), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); + + boost::posix_time::ptime min_end_time_dynamic = boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + auto received_state_dynamic=lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN; + int mov_id=13; + int mov_signal_group=15; + + ASSERT_EQ(cmw.check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,mov_id,mov_signal_group), 1); + + min_end_time_dynamic=boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + received_state_dynamic= lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE; + mov_id=13; + mov_signal_group=15; + + ASSERT_EQ(cmw.check_if_seen_before_movement_state(min_end_time_dynamic,received_state_dynamic,mov_id,mov_signal_group), 0); +} + +TEST(CARMAWorldModelTest, minEndTimeConverterMinuteOfYear) +{ + carma_wm::CARMAWorldModel cmw; + boost::posix_time::ptime min_end_time=boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + bool moy_exists=1; + double moy=1; + ros::Time::setNow(ros::Time(1)); + + ASSERT_EQ(cmw.min_end_time_converter_minute_of_year(min_end_time,moy_exists,moy), boost::posix_time::time_from_string("1970-01-01 00:00:01.000")); + + min_end_time=boost::posix_time::time_from_string("1970-01-01 00:00:01.000"); + moy_exists=1; + ros::Time::setNow(ros::Time(3601)); + + ASSERT_EQ(cmw.min_end_time_converter_minute_of_year(min_end_time,moy_exists,moy), boost::posix_time::time_from_string("1970-01-01 01:00:01.000")); + + min_end_time=boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + moy_exists=0; + ros::Time::setNow(ros::Time(0)); + + ASSERT_EQ(cmw.min_end_time_converter_minute_of_year(min_end_time,moy_exists,moy), boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); +} } // namespace carma_wm diff --git a/carma_wm_ctrl/CMakeLists.txt b/carma_wm_ctrl/CMakeLists.txt index dbd9af2b4c..893d9fcac8 100644 --- a/carma_wm_ctrl/CMakeLists.txt +++ b/carma_wm_ctrl/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2020-2021 LEIDOS. +# Copyright (C) 2022 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 @@ -13,44 +13,20 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(carma_wm_ctrl) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Set catkin dependencies -set ( PKG_CATKIN_DEPS - autoware_lanelet2_msgs - lanelet2_core - lanelet2_maps - lanelet2_routing - lanelet2_traffic_rules - lanelet2_extension - carma_utils - carma_wm - cav_msgs - roscpp - autoware_lanelet2_ros_interface -) - -## Find required catkin packages -find_package(catkin REQUIRED COMPONENTS - ${PKG_CATKIN_DEPS} -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen3 REQUIRED) -## Catkin export configuration -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${PKG_CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) ########### ## Build ## @@ -63,93 +39,63 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -## Declare C++ library -add_library(${PROJECT_NAME} - src/WMBroadcaster.cpp - src/GeofenceScheduler.cpp - src/GeofenceSchedule.cpp -) +# Name build targets +set(node_lib carma_wm_ctrl_node_lib) +set(worker_lib carma_wm_ctrl_worker_lib) +set(node_exec carma_wm_ctrl_node_exec) -## Add cmake target dependencies of the library -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# Build -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_auto_add_library(${worker_lib} SHARED + src/WMBroadcaster.cpp + src/GeofenceScheduler.cpp + src/GeofenceSchedule.cpp ) -message(INFO "LIBS: " ${catkin_LIBRARIES}) - -## Create C++ Node Executable -add_executable(${PROJECT_NAME}_node - src/main.cpp - src/WMBroadcasterNode.cpp +ament_auto_add_library(${node_lib} SHARED + src/WMBroadcasterNode.cpp ) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} - ${catkin_LIBRARIES} +ament_auto_add_executable(${node_exec} + src/main.cpp ) -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "carma_wm_ctrl::WMBroadcasterNode") -# Mark libraries for installation -# See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_lib} + ${worker_lib} ) -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +target_link_libraries(${node_exec} + ${node_lib} ) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -############# -## Testing ## -############# - -catkin_add_gmock(${PROJECT_NAME}-test - test/TestMain.cpp - test/GeofenceSchedulerTest.cpp - test/GeofenceScheduleTest.cpp - test/WMBroadcasterTest.cpp - test/MapToolsTest.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests -) +# Testing +if(BUILD_TESTING) -catkin_add_gmock(map-tools - test/TestMain.cpp - test/MapToolsTest.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests -) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -catkin_add_gmock(tcm-test - test/TestMain.cpp - test/TCMTest.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests -) + ament_add_gtest(${PROJECT_NAME}-test + test/TestMain.cpp + test/GeofenceSchedulerTest.cpp + test/GeofenceScheduleTest.cpp + test/WMBroadcasterTest.cpp + test/MapToolsTest.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests + ) -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) -endif() + ament_target_dependencies(${PROJECT_NAME}-test ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -if(TARGET map-tools) - target_link_libraries(map-tools ${PROJECT_NAME} ${catkin_LIBRARIES}) + target_link_libraries(${PROJECT_NAME}-test ${node_lib}) + endif() -if(TARGET tcm-test) - target_link_libraries(tcm-test ${PROJECT_NAME} ${catkin_LIBRARIES}) -endif() +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/carma_wm_ctrl/config/parameters.yaml b/carma_wm_ctrl/config/parameters.yaml index 420504cf8a..9b923708b1 100644 --- a/carma_wm_ctrl/config/parameters.yaml +++ b/carma_wm_ctrl/config/parameters.yaml @@ -4,4 +4,10 @@ ack_pub_times: 1 max_lane_width: 4.0 #Double: Period in seconds between traffic control requests after route selection -traffic_control_request_period: 1.0 \ No newline at end of file +traffic_control_request_period: 3.0 + +#List of Int: Every element corresponds to intersection_id of every two elements (x,y) in intersection_coord_correction (id must be [0, +65535] ranges) +intersection_ids_for_correction: [9945, 9709] + +#List of Double: Every 2 element describes coordinate correction [delta_x, delta_y] for each intersection_id in intersection_ids_for_correction in same order +intersection_coord_correction: [2.5, -2.5, -0.5, -1.0] diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.hpp similarity index 91% rename from carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h rename to carma_wm_ctrl/include/carma_wm_ctrl/Geofence.hpp index f0ff13041c..11fcfd9520 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/Geofence.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -15,7 +15,7 @@ * the License. */ #include -#include "GeofenceSchedule.h" +#include "GeofenceSchedule.hpp" #include #include #include @@ -28,8 +28,8 @@ #include #include #include -#include -#include +#include +#include namespace carma_wm_ctrl @@ -43,8 +43,6 @@ const std::string MAP_MSG_TF_SIGNAL = "MAP_MSG_TF_SIGNAL"; /** * @brief An object representing a geofence use for communications with CARMA Cloud - * - * TODO: This is currently a place holder class which needs to be updated based on the final geofence specification */ class Geofence { @@ -84,10 +82,10 @@ specific type of regulatory element (such as digital speed limit, passing contro lanelet::ConstLaneletOrAreas affected_parts_; // original traffic control message for this geofence - cav_msgs::TrafficControlMessageV01 msg_; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_; // original MAP message for this geofence - cav_msgs::MapData map_msg_; + carma_v2x_msgs::msg::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/GeofenceSchedule.h b/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceSchedule.hpp similarity index 69% rename from carma_wm_ctrl/include/carma_wm_ctrl/GeofenceSchedule.h rename to carma_wm_ctrl/include/carma_wm_ctrl/GeofenceSchedule.hpp index 97f62857e5..ffeb3fa0a7 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceSchedule.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceSchedule.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,24 +14,26 @@ * License for the specific language governing permissions and limitations under * the License. */ -#include #include #include #include +#include +#include +#include namespace carma_wm_ctrl { class GeofenceSchedule { public: - ros::Time schedule_start_; // Overall schedule start - ros::Time schedule_end_; // Overall schedule end + rclcpp::Time schedule_start_; // Overall schedule start + rclcpp::Time schedule_end_; // Overall schedule end - ros::Duration control_start_; // Duration from start of day - ros::Duration control_duration_; // Duration from start of control_start - ros::Duration control_offset_; // Duration from start of day (specifies start time for repeat parameters - span, period) - ros::Duration control_span_; // Duration of active status. Starts from offset - ros::Duration control_period_; // Interval between active status within control_duration since control_start + rclcpp::Duration control_start_{0, 0}; // Duration from start of day + rclcpp::Duration control_duration_{0, 0}; // Duration from start of control_start + rclcpp::Duration control_offset_{0, 0}; // Duration from start of day (specifies start time for repeat parameters - span period) + rclcpp::Duration control_span_{0, 0}; // Duration of active status. Starts from offset + rclcpp::Duration control_period_{0, 0}; // Interval between active status within control_duration since control_start using DayOfTheWeekSet = std::unordered_set>; // Set of week days where geofence is active. @@ -55,8 +57,8 @@ class GeofenceSchedule * @param control_period Interval between active status within control_duration since control_start * @param week_day_set Set of days of the week which this schedule applies to. Defaults as all days of the week */ - GeofenceSchedule(ros::Time schedule_start, ros::Time schedule_end, ros::Duration control_start, - ros::Duration control_duration, ros::Duration control_offset, ros::Duration control_span, ros::Duration control_period, + GeofenceSchedule(rclcpp::Time schedule_start, rclcpp::Time schedule_end, rclcpp::Duration control_start, + rclcpp::Duration control_duration, rclcpp::Duration control_offset, rclcpp::Duration control_span, rclcpp::Duration control_period, DayOfTheWeekSet week_day_set = { 0, 1, 2, 3, 4, 5, 6 }); // Include all weekdays as default (0-6) -> // (Sun-Sat) @@ -67,7 +69,7 @@ class GeofenceSchedule * * @return True if time > schedule_end */ - bool scheduleExpired(const ros::Time& time = ros::Time::now()) const; + bool scheduleExpired(const rclcpp::Time& time) const; /** * @brief Returns true if the schedule has started by the provided time @@ -76,7 +78,7 @@ class GeofenceSchedule * * @return True if time > schedule_start */ - bool scheduleStarted(const ros::Time& time = ros::Time::now()) const; + bool scheduleStarted(const rclcpp::Time& time) const; /** * @brief Returns the start time of the next active interval defined by this schedule @@ -86,11 +88,11 @@ class GeofenceSchedule * @return Returns a pair with the following semantics * First Element: A boolean indicating if the provided time is within a currently active control period * Second Element: The start time of the next scheduled active control interval within the current day. - * Returns ros::Time(0) when the schedule is expired or the next interval will be on a different day of the week or + * Returns rclcpp::Time(0) when the schedule is expired or the next interval will be on a different day of the week or * after schedule end * * TODO the UTC offset is provided in the geofence spec but for now we will ignore and assume all times are UTC */ - std::pair getNextInterval(const ros::Time& time) const; + std::pair getNextInterval(const rclcpp::Time& time) const; }; } // namespace carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.h b/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.hpp similarity index 77% rename from carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.h rename to carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.hpp index 21be290ebb..c795aa0c5b 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/GeofenceScheduler.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -15,13 +15,15 @@ * the License. */ #include -#include #include #include #include -#include -#include -#include +#include +#include +#include +#include +#include + namespace carma_wm_ctrl { @@ -31,17 +33,19 @@ namespace carma_wm_ctrl */ class GeofenceScheduler { - using Timer = carma_utils::timers::Timer; - using TimerFactory = carma_utils::timers::TimerFactory; + using Timer = carma_ros2_utils::timers::Timer; + using TimerFactory = carma_ros2_utils::timers::TimerFactory; + using ROSTimerFactory = carma_ros2_utils::timers::ROSTimerFactory; using TimerPtr = std::unique_ptr; std::mutex mutex_; - std::unique_ptr timerFactory_; + std::shared_ptr timerFactory_; std::unordered_map> timers_; // Pairing of timers with their Id and valid status std::unique_ptr deletion_timer_; std::function)> active_callback_; std::function)> inactive_callback_; uint32_t next_id_ = 0; // Timer id counter + rcl_clock_type_t clock_type_ = RCL_SYSTEM_TIME; public: /** @@ -50,7 +54,7 @@ class GeofenceScheduler * * @param timerFactory A pointer to a TimerFactory which can be used to generate timers for geofence triggers. */ - GeofenceScheduler(std::unique_ptr timerFactory); + GeofenceScheduler(std::shared_ptr timerFactory); /** * @brief Add a geofence to the scheduler. This will cause it to trigger an event when it becomes active or goes @@ -78,6 +82,16 @@ class GeofenceScheduler */ void clearTimers(); + /** + * @brief Get the clock type of the clock being created by the timer factory + */ + rcl_clock_type_t getClockType(); + + /** + * @brief Get current time used by scheduler + */ + rclcpp::Time now(); + private: /** * @brief Generates the next id to be used for a timer @@ -95,7 +109,7 @@ class GeofenceScheduler * @param schedule_id index number of the schedule being used corresponding to this geofence * @param timer_id The id of the timer which caused this callback to occur */ - void startGeofenceCallback(const ros::TimerEvent& event, std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id); + void startGeofenceCallback(std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id); /** * @brief The callback which is triggered when a geofence becomes in-active * This will call the user set inactive_callback set from the onGeofenceInactive function @@ -105,6 +119,7 @@ class GeofenceScheduler * @param schedule_id index number of the schedule being used corresponding to this geofence * @param timer_id The id of the timer which caused this callback to occur */ - void endGeofenceCallback(const ros::TimerEvent& event, std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id); + void endGeofenceCallback(std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id); + }; } // namespace carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp similarity index 81% rename from carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h rename to carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp index ba3f82f8e0..856b449421 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.h +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -19,45 +19,41 @@ #include #include #include -#include #include #include #include #include -#include "ros/ros.h" -#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 #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include namespace carma_wm_ctrl @@ -76,11 +72,11 @@ namespace carma_wm_ctrl class WMBroadcaster { public: - using PublishMapCallback = std::function; - using PublishMapUpdateCallback = std::function; - using PublishCtrlRequestCallback = std::function; - using PublishActiveGeofCallback = std::function; - using PublishMobilityOperationCallback = std::function; + using PublishMapCallback = std::function; + using PublishMapUpdateCallback = std::function; + using PublishCtrlRequestCallback = std::function; + using PublishActiveGeofCallback = std::function; + using PublishMobilityOperationCallback = std::function; /*! @@ -88,14 +84,14 @@ class WMBroadcaster */ WMBroadcaster(const PublishMapCallback& map_pub, const PublishMapUpdateCallback& map_update_pub, const PublishCtrlRequestCallback& control_msg_pub, - const PublishActiveGeofCallback& active_pub, std::unique_ptr timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub); + const PublishActiveGeofCallback& active_pub, std::shared_ptr timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub); /*! * \brief Callback to set the base map when it has been loaded * * \param map_msg The map message to use as the base map */ - void baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr& map_msg); + void baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg); /*! * \brief Callback to set the base map georeference (proj string) @@ -103,21 +99,21 @@ class WMBroadcaster * \param georef_msg Proj string that specifies the georeference of the map. * It is used for transfering frames between that of geofence and that of the vehicle */ - void geoReferenceCallback(const std_msgs::String& geo_ref); + void geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref); /*! * \brief Callback to add a geofence to the map. Currently only supports version 1 TrafficControlMessage * * \param geofence_msg The ROS msg of the geofence to add. */ - void geofenceCallback(const cav_msgs::TrafficControlMessage& geofence_msg); + void geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr 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); + void externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniquePtr map_msg); /*! * \brief Adds a geofence to the current map and publishes the ROS msg @@ -133,19 +129,19 @@ class WMBroadcaster * \brief Calls controlRequestFromRoute() and publishes the TrafficControlRequest Message returned after the completed operations * \param route_msg The message containing route information */ - void routeCallbackMessage(const cav_msgs::Route& route_msg); + void routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg); /*! * \brief composeTCMMarkerVisualizer() compose TCM Marker visualization * \param input The message containing tcm information */ - visualization_msgs::Marker composeTCMMarkerVisualizer(const std::vector& input); + visualization_msgs::msg::Marker composeTCMMarkerVisualizer(const std::vector& input); /*! * \brief composeTCRStatus() compose TCM Request visualization on UI * \param input The message containing tcr information */ - cav_msgs::TrafficControlRequestPolygon composeTCRStatus(const lanelet::BasicPoint3d& localPoint, const cav_msgs::TrafficControlBounds& cB, const lanelet::projection::LocalFrameProjector& local_projector); + carma_v2x_msgs::msg::TrafficControlRequestPolygon composeTCRStatus(const lanelet::BasicPoint3d& localPoint, const carma_v2x_msgs::msg::TrafficControlBounds& cB, const lanelet::projection::LocalFrameProjector& local_projector); /*! * \brief Pulls vehicle information from CARMA Cloud at startup by providing its selected route in a TrafficControlRequest message that is published after a route is selected. @@ -153,7 +149,7 @@ class WMBroadcaster * \param route_msg The message containing route information pulled from routeCallbackMessage() * \param req_id_for_testing this ptr is optional. it gives req_id for developer to test TrafficControlMessage as it needs it */ - cav_msgs::TrafficControlRequest controlRequestFromRoute(const cav_msgs::Route& route_msg, std::shared_ptr req_id_for_testing = NULL); + carma_v2x_msgs::msg::TrafficControlRequest controlRequestFromRoute(const carma_planning_msgs::msg::Route& route_msg, std::shared_ptr req_id_for_testing = NULL); /*! * \brief Extract geofence points from geofence message using its given proj and datum fields @@ -161,7 +157,7 @@ class WMBroadcaster * \throw InvalidObjectStateError if base_map is not set or the base_map's georeference is empty * \return lanelet::Points3d in local frame */ - lanelet::Points3d getPointsInLocalFrame(const cav_msgs::TrafficControlMessageV01& geofence_msg); + lanelet::Points3d getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01& geofence_msg); /*! * \brief Gets the affected lanelet or areas based on the points in local frame @@ -177,6 +173,13 @@ class WMBroadcaster */ void setMaxLaneWidth(double max_lane_width); + /*! + * \brief Sets the coordinate correction for intersection + \param list of intersection_ids corresponding to every two elements in correction list + \param list of intersection coord correction parameters in double in every 2 elements: delta_x, delta_y + */ + void setIntersectionCoordCorrection(const std::vector& intersection_ids_for_correction, const std::vector& intersection_correction); + /*! * \brief Sets the configured speed limit. */ @@ -202,7 +205,7 @@ class WMBroadcaster * \param geofence_msg The ROS msg that contains geofence information * \throw InvalidObjectStateError if base_map is not set or the base_map's georeference is empty */ - void geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& geofence_msg); + void geofenceFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& geofence_msg); /*! * \brief Fills geofence object from MAP Data ROS Msg which contains intersections' static data such as geometry and signal_group @@ -210,7 +213,7 @@ class WMBroadcaster * \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); + std::vector> geofenceFromMapMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::MapData& map_msg); /*! * \brief Returns the route distance (downtrack or crosstrack in meters) to the nearest active geofence lanelet @@ -222,20 +225,20 @@ class WMBroadcaster double distToNearestActiveGeofence(const lanelet::BasicPoint2d& curr_pos); - void currentLocationCallback(const geometry_msgs::PoseStamped& current_pos); + void currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos); /*! * \brief Returns a message indicating whether or not the vehicle is inside of an active geofence lanelet * \param current_pos Current position of the vehicle * \return 0 if vehicle is not on an active geofence */ - cav_msgs::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::PoseStamped& current_pos); + carma_perception_msgs::msg::CheckActiveGeofence checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped& current_pos); /*! * \brief Adds RegionAccessRule to the map * \param gf_ptr geofence pointer * \param msg_v01 message type * \param afffected_llts affected lanelets */ - void addRegionAccessRule(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; + void addRegionAccessRule(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; /*! * \brief Adds Minimum Gap to the map * \param gf_ptr geofence pointer @@ -243,41 +246,33 @@ class WMBroadcaster * \param afffected_llts affected lanelets * \param affected_areas affected areas */ - void addRegionMinimumGap(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector& affected_llts, const std::vector& affected_areas) const; + void addRegionMinimumGap(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector& affected_llts, const std::vector& affected_areas) const; /*! * \brief Generates participants list * \param msg_v01 message type */ - ros::V_string participantsChecker(const cav_msgs::TrafficControlMessageV01& msg_v01) const; + std::vector participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) const; /*! * \brief Generates inverse participants list of the given participants - * \param ros::V_string participants vector of strings + * \param std::vector participants vector of strings */ - ros::V_string invertParticipants(const ros::V_string& input_participants) const; + std::vector invertParticipants(const std::vector& input_participants) const; /*! * \brief Combines a list of the given participants into a single "vehicle" type if participants cover all possible vehicle types. Returns the input with no change if it doesn't cover all. - * \param ros::V_string participants vector of strings - */ - ros::V_string combineParticipantsToVehicle(const ros::V_string& input_participants) const; - - /*! - * \brief Callback triggered whenever a new subscriber connects to the map_update topic of this node. - * This callback will publish the all updates for the current map to that node so that any missed updates are already included. - * - * \param single_sub_pub A publisher which will publish exclusively to the new subscriber + * \param std::vector participants vector of strings */ - void newUpdateSubscriber(const ros::SingleSubscriberPublisher& single_sub_pub) const; + std::vector combineParticipantsToVehicle(const std::vector& input_participants) const; /*! * \brief Returns the most recently recieved route message. * * \return The most recent route message. */ - cav_msgs::Route getRoute(); + carma_planning_msgs::msg::Route getRoute(); /*! * \brief Creates a single workzone geofence (in the vector) that includes all additional lanelets (housing traffic lights) and update_list that blocks old lanelets. @@ -399,16 +394,16 @@ class WMBroadcaster /*! * \brief Construct TCM acknowledgement object and populate it with params. Publish the object for a configured number of times. */ - void pubTCMACK(j2735_msgs::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason); + void pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason); /*! \brief populate upcoming_intersection_ids_ from local traffic lanelet ids */ void updateUpcomingSGIntersectionIds(); - visualization_msgs::MarkerArray tcm_marker_array_; - cav_msgs::TrafficControlRequestPolygon tcr_polygon_; - std_msgs::Int32MultiArray upcoming_intersection_ids_; + visualization_msgs::msg::MarkerArray tcm_marker_array_; + carma_v2x_msgs::msg::TrafficControlRequestPolygon tcr_polygon_; + std_msgs::msg::Int32MultiArray upcoming_intersection_ids_; private: double error_distance_ = 5; //meters @@ -421,8 +416,8 @@ class WMBroadcaster void removeGeofenceHelper(std::shared_ptr gf_ptr) const; void addGeofenceHelper(std::shared_ptr gf_ptr); bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr gf_ptr) const; - void addPassingControlLineFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; - void addScheduleFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01); + void addPassingControlLineFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; + void addScheduleFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01); void scheduleGeofence(std::shared_ptr gf_ptr_list); lanelet::LineString3d createLinearInterpolatingLinestring(const lanelet::Point3d& front_pt, const lanelet::Point3d& back_pt, double increment_distance = 0.25); lanelet::Lanelet createLinearInterpolatingLanelet(const lanelet::Point3d& left_front_pt, const lanelet::Point3d& right_front_pt, @@ -446,7 +441,7 @@ class WMBroadcaster PublishMobilityOperationCallback tcm_ack_pub_; std::string base_map_georef_; double max_lane_width_; - std::vector workzone_remaining_msgs_; + std::vector workzone_remaining_msgs_; bool workzone_geometry_published_ = false; /* Version ID of the current_map_ variable. Monotonically increasing value * NOTE: This parameter needs to be incremented any time a new map is ready to be published. @@ -454,13 +449,13 @@ class WMBroadcaster */ size_t current_map_version_ = 0; - cav_msgs::Route current_route; // Most recently received route message + carma_planning_msgs::msg::Route current_route; // Most recently received route message /** * Queue which stores the map updates applied to the current map version as a sequence of diffs * This queue is implemented as a vector because it gets reused by each new subscriber connection * NOTE: This queue should be cleared each time the current_map_version changes */ - std::vector map_update_message_queue_; + std::vector map_update_message_queue_; size_t update_count_ = -1; // Records the total number of sent map updates. Used as the set value for update.seq_id diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterConfig.hpp b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterConfig.hpp new file mode 100644 index 0000000000..6b2dbd0fb0 --- /dev/null +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterConfig.hpp @@ -0,0 +1,56 @@ +#pragma once + +/* + * Copyright (C) 2022 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 + +namespace carma_wm_ctrl +{ + + /** + * \brief Config struct + */ + struct Config + { + int ack_pub_times = 1; // The number of times it publishes Geofence Acknowledgement. + double max_lane_width = 4.0; // Max lane width in meters within which geofence points are associated to a lanelet as those points are guaranteed to apply to a single lane + double traffic_control_request_period = 1.0; //Period in seconds between traffic control requests after route selection + std::vector intersection_coord_correction = {}; // Every element corresponds to intersection_id of every two elements (x,y) in intersection_coord_correction (id must be [0, +65535] ranges) + std::vector intersection_ids_for_correction = {}; //Every 2 element describes coordinate correction [delta_x, delta_y] for each intersection_id in intersection_ids_for_correction in same order + double config_limit = 6.67; //config speed limit in m/s + std::string vehicle_id = "CARMA"; + std::string participant = "vehicle:car"; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "WMBroadcaster::Config { " << std::endl + << "ack_pub_times: " << c.ack_pub_times << std::endl + << "max_lane_width: " << c.max_lane_width << std::endl + << "traffic_control_request_period: " << c.traffic_control_request_period << std::endl + << "intersection_coord_correction.size(): " << c.intersection_coord_correction.size() << std::endl + << "intersection_ids_for_correction.size(): " << c.intersection_ids_for_correction.size() << std::endl + << "vehicle_id: " << c.vehicle_id << std::endl + << "participant: " << c.participant << std::endl + << "config_limit: " << c.config_limit << std::endl + << "}" << std::endl; + return output; + } + }; + +} // carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h deleted file mode 100644 index 34c5762641..0000000000 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.h +++ /dev/null @@ -1,118 +0,0 @@ -#pragma once - -/* - * 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. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace carma_wm_ctrl -{ -/*! - * \brief Node which provies exposes map publication and carma_wm update logic - * - * The WMBroadcasterNode handles updating the lanelet2 base map and publishing the new versions to the rest of the CARMA - * Platform ROS network. The broadcaster also provides functions for adding or removing geofences from the map and - * notifying the rest of the system. - * - */ -class WMBroadcasterNode -{ -public: - /** - * @brief Constructor - */ - WMBroadcasterNode(); - - /** - * @brief Starts the Node - * - * @return 0 on exit with no errors - */ - int run(); - - /** - * @brief Callback to publish a map - * - * @param map_msg The map message to publish - */ - void publishMap(const autoware_lanelet2_msgs::MapBin& map_msg); - - /** - * @brief Callback to publish TrafficControlRequest Messages - * - * @param route_msg The TrafficControlRequest message to publish - */ - void publishCtrlReq(const cav_msgs::TrafficControlRequest& ctrlreq_msg) const; - - /** - * @brief Callback to publish map updates (geofences) - * - * @param geofence_msg The geofence message to publish - */ - void publishMapUpdate(const autoware_lanelet2_msgs::MapBin& geofence_msg) const; - - /** - * @brief Callback to publish active geofence information - * - * @param active_geof_msg The geofence information to publish - */ - void publishActiveGeofence(const cav_msgs::CheckActiveGeofence& active_geof_msg); - - /** - * @brief Callback to publish traffic control acknowledgement information - * - * @param mom_msg The acknowledgement information to publish - */ - void publishTCMACK(const cav_msgs::MobilityOperation& mom_msg); - -private: - ros::CARMANodeHandle cnh_; - ros::CARMANodeHandle pnh_{"~"}; - - ros::CARMANodeHandle pnh2_{"/"}; //Global Scope - - ros::Publisher map_pub_; - ros::Publisher map_update_pub_; - ros::Publisher control_msg_pub_; - ros::Publisher tcm_visualizer_pub_; - ros::Publisher tcr_visualizer_pub_; - ros::Publisher upcoming_intersection_ids_pub_; - - ros::Publisher active_pub_; - ros::Publisher tcm_ack_pub_; - ros::Subscriber base_map_sub_; - - ros::Subscriber route_callmsg_sub_; - - - ros::Subscriber georef_sub_; - ros::Subscriber geofence_sub_; - ros::Subscriber incoming_map_sub_; - ros::Subscriber curr_location_sub_; - ros::Subscriber route_cache_sub_; - ros::Timer timer; - ros::Timer timer1; - - WMBroadcaster wmb_; -}; -} // namespace carma_wm_ctrl diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.hpp b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.hpp new file mode 100644 index 0000000000..49b004204f --- /dev/null +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcasterNode.hpp @@ -0,0 +1,133 @@ +#pragma once + +/* + * Copyright (C) 2022 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 + +namespace carma_wm_ctrl +{ + +class WMBroadcaster; + +/*! + * \brief Node which provies exposes map publication and carma_wm update logic + * + * The WMBroadcasterNode handles updating the lanelet2 base map and publishing the new versions to the rest of the CARMA + * Platform ROS network. The broadcaster also provides functions for adding or removing geofences from the map and + * notifying the rest of the system. + * + */ +class WMBroadcasterNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + /** + * @brief Constructor + */ + explicit WMBroadcasterNode(const rclcpp::NodeOptions& options); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + /** + * @brief Callback to publish a map + * + * @param map_msg The map message to publish + */ + void publishMap(const autoware_lanelet2_msgs::msg::MapBin& map_msg); + + /** + * @brief Initializes the WMBroadcaster worker with reference to the CarmaLifecycleNode itself + * + * @param weak_ptr to the node of type CarmaLifecycleNode that owns this worker + */ + void initializeWorker(std::weak_ptr weak_node_pointer); + + /** + * @brief Callback to publish TrafficControlRequest Messages + * + * @param route_msg The TrafficControlRequest message to publish + */ + void publishCtrlReq(const carma_v2x_msgs::msg::TrafficControlRequest& ctrlreq_msg) const; + + /** + * @brief Callback to publish map updates (geofences) + * + * @param geofence_msg The geofence message to publish + */ + void publishMapUpdate(const autoware_lanelet2_msgs::msg::MapBin& geofence_msg) const; + + /** + * @brief Callback to publish active geofence information + * + * @param active_geof_msg The geofence information to publish + */ + void publishActiveGeofence(const carma_perception_msgs::msg::CheckActiveGeofence& active_geof_msg); + + /** + * @brief Callback to publish traffic control acknowledgement information + * + * @param mom_msg The acknowledgement information to publish + */ + void publishTCMACK(const carma_v2x_msgs::msg::MobilityOperation& mom_msg); + +private: + + /** + * \brief Spin callback, which will be called frequently based on the configured spin rate + */ + bool spin_callback(); + + // Node configuration + Config config_; + + carma_ros2_utils::PubPtr map_pub_; + carma_ros2_utils::PubPtr map_update_pub_; + carma_ros2_utils::PubPtr control_msg_pub_; + carma_ros2_utils::PubPtr tcm_visualizer_pub_; + carma_ros2_utils::PubPtr tcr_visualizer_pub_; + carma_ros2_utils::PubPtr upcoming_intersection_ids_pub_; + carma_ros2_utils::PubPtr active_pub_; + carma_ros2_utils::PubPtr tcm_ack_pub_; + + carma_ros2_utils::SubPtr base_map_sub_; + carma_ros2_utils::SubPtr route_callmsg_sub_; + carma_ros2_utils::SubPtr georef_sub_; + carma_ros2_utils::SubPtr geofence_sub_; + carma_ros2_utils::SubPtr incoming_map_sub_; + carma_ros2_utils::SubPtr curr_location_sub_; + + // Timer for publishing TCR and SignalGroup/IntersectionID + rclcpp::TimerBase::SharedPtr timer_; + + std::shared_ptr ptr_; + std::unique_ptr wmb_; +}; +} // namespace carma_wm_ctrl diff --git a/carma_wm_ctrl/launch/carma_wm_broadcaster.launch b/carma_wm_ctrl/launch/carma_wm_broadcaster.launch deleted file mode 100644 index e21711c757..0000000000 --- a/carma_wm_ctrl/launch/carma_wm_broadcaster.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - diff --git a/carma_wm_ctrl/launch/carma_wm_broadcaster.launch.py b/carma_wm_ctrl/launch/carma_wm_broadcaster.launch.py new file mode 100644 index 0000000000..7171786f78 --- /dev/null +++ b/carma_wm_ctrl/launch/carma_wm_broadcaster.launch.py @@ -0,0 +1,66 @@ +# Copyright (C) 2022 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA World Model Broadcaster Node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + carma_wm_ctrl_param_file = os.path.join( + get_package_share_directory('carma_wm_ctrl'), 'config/parameters.yaml') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='carma_wm_ctrl_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='carma_wm_ctrl', + plugin='carma_wm_ctrl::WMBroadcasterNode', + name='carma_wm_broadcaster', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ carma_wm_ctrl_param_file ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/carma_wm_ctrl/package.xml b/carma_wm_ctrl/package.xml index 9208aee12e..5d414e645c 100644 --- a/carma_wm_ctrl/package.xml +++ b/carma_wm_ctrl/package.xml @@ -1,29 +1,40 @@ carma_wm_ctrl - 1.0.0 + 4.0.0 The carma_wm_ctrl package carma Apache License 2.0 Michael McConnell - catkin + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components autoware_lanelet2_msgs lanelet2_core lanelet2_maps lanelet2_routing lanelet2_traffic_rules lanelet2_extension - carma_utils - carma_wm - cav_msgs - roscpp + carma_wm_ros2 + carma_perception_msgs + carma_planning_msgs + carma_v2x_msgs + j2735_v2x_msgs + carma_debug_ros2_msgs autoware_lanelet2_ros_interface - carma_cmake_common + std_msgs + ament_lint_auto + ament_cmake_gtest - - - + launch + launch_ros + + ament_cmake diff --git a/carma_wm_ctrl/src/GeofenceSchedule.cpp b/carma_wm_ctrl/src/GeofenceSchedule.cpp index 2f0e1e7724..d5734d0db9 100644 --- a/carma_wm_ctrl/src/GeofenceSchedule.cpp +++ b/carma_wm_ctrl/src/GeofenceSchedule.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,92 +14,98 @@ * the License. */ -#include -#include +#include #include -#include + namespace carma_wm_ctrl { GeofenceSchedule::GeofenceSchedule() { } -GeofenceSchedule::GeofenceSchedule(ros::Time schedule_start, ros::Time schedule_end, ros::Duration control_start, - ros::Duration control_duration, ros::Duration control_offset, - ros::Duration control_span, ros::Duration control_period, DayOfTheWeekSet week_day_set): +GeofenceSchedule::GeofenceSchedule(rclcpp::Time schedule_start, rclcpp::Time schedule_end, rclcpp::Duration control_start, + rclcpp::Duration control_duration, rclcpp::Duration control_offset, + rclcpp::Duration control_span, rclcpp::Duration control_period, DayOfTheWeekSet week_day_set): schedule_start_(schedule_start), schedule_end_(schedule_end), control_start_ (control_start), control_duration_ (control_duration), control_offset_ (control_offset), control_span_ (control_span), control_period_ (control_period), week_day_set_(week_day_set) {} -bool GeofenceSchedule::scheduleExpired(const ros::Time& time) const +bool GeofenceSchedule::scheduleExpired(const rclcpp::Time& time) const { return schedule_end_ < time; } -bool GeofenceSchedule::scheduleStarted(const ros::Time& time) const +bool GeofenceSchedule::scheduleStarted(const rclcpp::Time& time) const { return schedule_start_ <= time; } -// returns ros::Time(0) when the schedule is expired or the next interval will be on a different day of the week +// returns rclcpp::Time(0) when the schedule is expired or the next interval will be on a different day of the week // Argument provided as absolute time (since 1970) -std::pair GeofenceSchedule::getNextInterval(const ros::Time& time) const +std::pair GeofenceSchedule::getNextInterval(const rclcpp::Time& time) const { + auto clock_type = time.get_clock_type(); if (scheduleExpired(time)) { - ROS_DEBUG_STREAM("Geofence schedule expired"); - return std::make_pair(false, ros::Time(0)); // If the schedule has expired or was never started + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule expired"); + return std::make_pair(false, rclcpp::Time(0, 0, clock_type)); // If the schedule has expired or was never started } - - boost::posix_time::ptime boost_time = time.toBoost(); + + boost::posix_time::ptime boost_time = boost::posix_time::from_time_t(time.seconds()); + boost_time += boost::posix_time::microseconds((int)((time.seconds()-std::floor(time.seconds()))*1e6)); boost::gregorian::date date = boost_time.date(); if (week_day_set_.find(date.day_of_week()) == week_day_set_.end()) { - ROS_DEBUG_STREAM("Geofence wrong day of the week"); - return std::make_pair(false, ros::Time(0)); // This geofence is not active on this day + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence wrong day of the week"); + return std::make_pair(false, rclcpp::Time(0, 0, clock_type)); // This geofence is not active on this day } auto time_of_day = boost_time.time_of_day(); + // Convert schedule into workable components boost::posix_time::ptime ptime_start_of_day(date, boost::posix_time::hours(0)); // Get absolute start time of the day + + rclcpp::Time ros_time_of_day = rclcpp::Time(lanelet::time::toSec(time_of_day) * 1e9, clock_type); - ros::Time ros_time_of_day = ros::Time::fromBoost(time_of_day); - ros::Time abs_day_start = ros::Time::fromBoost(ptime_start_of_day); - ros::Duration cur_start = control_start_ + control_offset_; // accounting for the shift of repetition start + rclcpp::Time abs_day_start = rclcpp::Time(lanelet::time::toSec(ptime_start_of_day) * 1e9, clock_type); + rclcpp::Duration cur_start = control_start_ + control_offset_; // accounting for the shift of repetition start // Check if current time is after end of control - if (ros_time_of_day > ros::Time((control_start_ + control_duration_).toSec())) + if (ros_time_of_day > rclcpp::Time((control_start_ + control_duration_).nanoseconds(), clock_type)) { - ROS_DEBUG_STREAM("Geofence schedule too late in the day"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule too late in the day"); // The requested time is after control end so there will not be another interval - return std::make_pair(false, ros::Time(0)); + return std::make_pair(false, rclcpp::Time(0, 0, clock_type)); } // Iterate over the day to find the next control interval constexpr int num_sec_in_day = 86400; - const ros::Duration full_day(num_sec_in_day); + const rclcpp::Duration full_day(num_sec_in_day* 1e9); bool time_in_active_period = false; // Flag indicating if the requested time is within an active control span - while (cur_start < full_day && ros_time_of_day > ros::Time(cur_start.toSec())) + while (cur_start < full_day && ros_time_of_day > rclcpp::Time(cur_start.nanoseconds(), clock_type)) { // Check if the requested time is within the control period being evaluated - if (ros::Time(cur_start.toSec()) < ros_time_of_day && - ros_time_of_day < ros::Time((cur_start + control_span_).toSec())) + if (rclcpp::Time(cur_start.nanoseconds(), clock_type) < ros_time_of_day && + ros_time_of_day < rclcpp::Time((cur_start + control_span_).nanoseconds(), clock_type)) { - ROS_DEBUG_STREAM("Geofence schedule active!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule active!"); + time_in_active_period = true; } - cur_start += control_period_; + cur_start = cur_start + control_period_; + } + // check if the only next interval is after the schedule end or past the end of the day if (abs_day_start + cur_start > schedule_end_ || cur_start > full_day || cur_start > (control_start_ + control_duration_)) { - ROS_DEBUG_STREAM("Geofence schedule beyond end time"); - return std::make_pair(time_in_active_period, ros::Time(0)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Geofence schedule beyond end time"); + return std::make_pair(time_in_active_period, rclcpp::Time(0, 0, clock_type)); } // At this point we should have the next start time which is still within the schedule and day diff --git a/carma_wm_ctrl/src/GeofenceScheduler.cpp b/carma_wm_ctrl/src/GeofenceScheduler.cpp index 293ae4e89f..d754dd10ef 100644 --- a/carma_wm_ctrl/src/GeofenceScheduler.cpp +++ b/carma_wm_ctrl/src/GeofenceScheduler.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,18 +14,24 @@ * the License. */ -#include +#include namespace carma_wm_ctrl { using std::placeholders::_1; -GeofenceScheduler::GeofenceScheduler(std::unique_ptr timerFactory) - : timerFactory_(std::move(timerFactory)) +GeofenceScheduler::GeofenceScheduler(std::shared_ptr timerFactory) + : timerFactory_(timerFactory) { // Create repeating loop to clear geofence timers which are no longer needed deletion_timer_ = - timerFactory_->buildTimer(nextId(), ros::Duration(1), std::bind(&GeofenceScheduler::clearTimers, this)); + timerFactory_->buildTimer(nextId(), rclcpp::Duration(1), std::bind(&GeofenceScheduler::clearTimers, this)); + clock_type_ = timerFactory_->now().get_clock_type(); +} + +rclcpp::Time GeofenceScheduler::now() +{ + return timerFactory_->now(); } uint32_t GeofenceScheduler::nextId() @@ -34,6 +40,11 @@ uint32_t GeofenceScheduler::nextId() return next_id_; } +rcl_clock_type_t GeofenceScheduler::getClockType() +{ + return clock_type_; +} + void GeofenceScheduler::clearTimers() { std::lock_guard guard(mutex_); @@ -60,16 +71,18 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) { std::lock_guard guard(mutex_); - ROS_INFO_STREAM("Attempting to add Geofence with Id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Attempting to add Geofence with Id: " << gf_ptr->id_); // Create timer for next start time for (size_t schedule_idx = 0; schedule_idx < gf_ptr->schedules.size(); schedule_idx++) { - auto interval_info = gf_ptr->schedules[schedule_idx].getNextInterval(ros::Time::now()); - ros::Time startTime = interval_info.second; - if (!interval_info.first && startTime == ros::Time(0)) + // resolve clock type + auto interval_info = gf_ptr->schedules[schedule_idx].getNextInterval(timerFactory_->now()); + rclcpp::Time startTime = interval_info.second; + + if (!interval_info.first && startTime == rclcpp::Time(0, 0, clock_type_)) { - ROS_WARN_STREAM( + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to add geofence as its schedule did not contain an active or upcoming control period. GF Id: " << gf_ptr->id_); return; @@ -77,60 +90,65 @@ void GeofenceScheduler::addGeofence(std::shared_ptr gf_ptr) // If this geofence is currently active set the start time to now if (interval_info.first) { - startTime = ros::Time::now(); + startTime = timerFactory_->now(); } int32_t timer_id = nextId(); + rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + // Build timer to trigger when this geofence becomes active TimerPtr timer = timerFactory_->buildTimer( - timer_id, startTime - ros::Time::now(), - std::bind(&GeofenceScheduler::startGeofenceCallback, this, _1, gf_ptr, schedule_idx, timer_id), true, true); + timer_id, control_duration, + std::bind(&GeofenceScheduler::startGeofenceCallback, this, gf_ptr, schedule_idx, timer_id), true, true); timers_[timer_id] = std::make_pair(std::move(timer), false); // Add start timer to map by Id } } -void GeofenceScheduler::startGeofenceCallback(const ros::TimerEvent& event, std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id) +void GeofenceScheduler::startGeofenceCallback(std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id) { std::lock_guard guard(mutex_); - ros::Time endTime = ros::Time::now() + gf_ptr->schedules[schedule_id].control_span_; + rclcpp::Time endTime = timerFactory_->now() + gf_ptr->schedules[schedule_id].control_span_; - ROS_INFO_STREAM("Activating Geofence with Id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Activating Geofence with Id: " << gf_ptr->id_ << ", which will end at:" << endTime.seconds()); active_callback_(gf_ptr); // Build timer to trigger when this geofence becomes inactive int32_t ending_timer_id = nextId(); + + rclcpp::Duration control_duration = rclcpp::Duration(std::max((endTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + TimerPtr timer = timerFactory_->buildTimer( - ending_timer_id, endTime - ros::Time::now(), - std::bind(&GeofenceScheduler::endGeofenceCallback, this, _1, gf_ptr, schedule_id, ending_timer_id), true, true); + ending_timer_id, control_duration, + std::bind(&GeofenceScheduler::endGeofenceCallback, this, gf_ptr, schedule_id, ending_timer_id), true, true); timers_[ending_timer_id] = std::make_pair(std::move(timer), false); // Add end timer to map by Id timers_[timer_id].second = true; // Mark start timer for deletion } -void GeofenceScheduler::endGeofenceCallback(const ros::TimerEvent& event, std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id) +void GeofenceScheduler::endGeofenceCallback(std::shared_ptr gf_ptr, const unsigned int schedule_id, const int32_t timer_id) { std::lock_guard guard(mutex_); - ROS_INFO_STREAM("Deactivating Geofence with Id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Deactivating Geofence with Id: " << gf_ptr->id_); inactive_callback_(gf_ptr); timers_[timer_id].second = true; // Mark timer for deletion // Determine if a new timer is needed for this geofence - auto interval_info = gf_ptr->schedules[schedule_id].getNextInterval(ros::Time::now()); - ros::Time startTime = interval_info.second; + auto interval_info = gf_ptr->schedules[schedule_id].getNextInterval(timerFactory_->now()); + rclcpp::Time startTime = interval_info.second; // If this geofence should currently be active set the start time to now if (interval_info.first) { - startTime = ros::Time::now(); + startTime = timerFactory_->now(); } - if (!interval_info.first && startTime == ros::Time(0)) + if (!interval_info.first && startTime == rclcpp::Time(0, 0, clock_type_)) { // No more active periods for this geofence so return return; @@ -139,9 +157,11 @@ void GeofenceScheduler::endGeofenceCallback(const ros::TimerEvent& event, std::s // Build timer to trigger when this geofence becomes active int32_t start_timer_id = nextId(); + rclcpp::Duration control_duration = rclcpp::Duration(std::max((startTime - timerFactory_->now()).seconds() * 1e9, 0.0)); //guard for negative value + TimerPtr timer = timerFactory_->buildTimer( - start_timer_id, startTime - ros::Time::now(), - std::bind(&GeofenceScheduler::startGeofenceCallback, this, _1, gf_ptr, schedule_id, start_timer_id), true, true); + start_timer_id, control_duration, + std::bind(&GeofenceScheduler::startGeofenceCallback, this, gf_ptr, schedule_id, start_timer_id), true, true); timers_[start_timer_id] = std::make_pair(std::move(timer), false); // Add start timer to map by Id } diff --git a/carma_wm_ctrl/src/RoutingGraphAccessor.h b/carma_wm_ctrl/src/RoutingGraphAccessor.hpp similarity index 79% rename from carma_wm_ctrl/src/RoutingGraphAccessor.h rename to carma_wm_ctrl/src/RoutingGraphAccessor.hpp index 47afddde2c..1479259c63 100644 --- a/carma_wm_ctrl/src/RoutingGraphAccessor.h +++ b/carma_wm_ctrl/src/RoutingGraphAccessor.hpp @@ -41,10 +41,10 @@ class RoutingGraphAccessor : public lanelet::routing::RoutingGraph { * * \return The ros message which can be used to regenerate this routing graph structure. */ - autoware_lanelet2_msgs::RoutingGraph routingGraphToMsg(const std::string& participant) { + autoware_lanelet2_msgs::msg::RoutingGraph routingGraphToMsg(const std::string& participant) { - autoware_lanelet2_msgs::RoutingGraph msg; // output message + autoware_lanelet2_msgs::msg::RoutingGraph msg; // output message // Assign base fields msg.num_unique_routing_cost_ids = this->graph_->numRoutingCosts(); @@ -60,7 +60,7 @@ class RoutingGraphAccessor : public lanelet::routing::RoutingGraph { for(boost::tie(vi, vi_end) = boost::vertices(underlying_graph); vi != vi_end; ++vi) { - autoware_lanelet2_msgs::RoutingGraphVertexAndEdges vertex_and_edges; + autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges vertex_and_edges; lanelet::routing::internal::GraphType::vertex_descriptor source = *vi; @@ -81,21 +81,21 @@ class RoutingGraphAccessor : public lanelet::routing::RoutingGraph { uint8_t relation = 0; switch(underlying_graph[edge].relation) { case lanelet::routing::RelationType::Successor: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_SUCCESSOR; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_SUCCESSOR; break; case lanelet::routing::RelationType::Left: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_LEFT; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_LEFT; break; case lanelet::routing::RelationType::Right: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_RIGHT; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_RIGHT; break; case lanelet::routing::RelationType::AdjacentLeft: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_ADJACENT_LEFT; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_LEFT; break; case lanelet::routing::RelationType::AdjacentRight: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_ADJACENT_RIGHT; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_ADJACENT_RIGHT; break; case lanelet::routing::RelationType::Conflicting: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_CONFLICTING; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_CONFLICTING; break; case lanelet::routing::RelationType::Area: - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_AREA; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_AREA; break; default: // None relation will be default - relation = autoware_lanelet2_msgs::RoutingGraphVertexAndEdges::RELATION_NONE; break; + relation = autoware_lanelet2_msgs::msg::RoutingGraphVertexAndEdges::RELATION_NONE; break; } vertex_and_edges.edge_relations.push_back(relation); diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index 3bb9d0dfe0..7bf87922da 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -16,44 +16,44 @@ #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 "RoutingGraphAccessor.h" +#include "RoutingGraphAccessor.hpp" namespace carma_wm_ctrl { using std::placeholders::_1; WMBroadcaster::WMBroadcaster(const PublishMapCallback& map_pub, const PublishMapUpdateCallback& map_update_pub, const PublishCtrlRequestCallback& control_msg_pub, -const PublishActiveGeofCallback& active_pub, std::unique_ptr timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub) - : map_pub_(map_pub), map_update_pub_(map_update_pub), control_msg_pub_(control_msg_pub), active_pub_(active_pub), scheduler_(std::move(timer_factory)), tcm_ack_pub_(tcm_ack_pub) +const PublishActiveGeofCallback& active_pub, std::shared_ptr timer_factory, const PublishMobilityOperationCallback& tcm_ack_pub) + : map_pub_(map_pub), map_update_pub_(map_update_pub), control_msg_pub_(control_msg_pub), active_pub_(active_pub), scheduler_(timer_factory), tcm_ack_pub_(tcm_ack_pub) { scheduler_.onGeofenceActive(std::bind(&WMBroadcaster::addGeofence, this, _1)); scheduler_.onGeofenceInactive(std::bind(&WMBroadcaster::removeGeofence, this, _1)); std::bind(&WMBroadcaster::routeCallbackMessage, this, _1); }; -void WMBroadcaster::baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr& map_msg) +void WMBroadcaster::baseMapCallback(autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg) { std::lock_guard guard(map_mutex_); @@ -62,11 +62,11 @@ void WMBroadcaster::baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr if (firstCall) { firstCall = false; - ROS_INFO("WMBroadcaster::baseMapCallback called for first time with new map message"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called for first time with new map message"); } else { - ROS_WARN("WMBroadcaster::baseMapCallback called multiple times in the same node"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "WMBroadcaster::baseMapCallback called multiple times in the same node"); } lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); @@ -81,28 +81,28 @@ void WMBroadcaster::baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr lanelet::MapConformer::ensureCompliance(base_map_, config_limit); // Update map to ensure it complies with expectations lanelet::MapConformer::ensureCompliance(current_map_, config_limit); - ROS_INFO_STREAM("Building routing graph for base map"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Building routing graph for base map"); 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 building routing graph for base map"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done building routing graph for base map"); // Publish map current_map_version_ += 1; // Increment the map version. It should always start from 1 for the first map - map_update_message_queue_.clear(); // Clear the update queue as the map version has changed - autoware_lanelet2_msgs::MapBin compliant_map_msg; + + autoware_lanelet2_msgs::msg::MapBin compliant_map_msg; // Populate the routing graph message - ROS_INFO_STREAM("Creating routing graph message."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message."); auto readable_graph = std::static_pointer_cast(current_routing_graph_); compliant_map_msg.routing_graph = readable_graph->routingGraphToMsg(participant_); compliant_map_msg.has_routing_graph = true; - ROS_INFO_STREAM("Done creating routing graph message."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message."); lanelet::utils::conversion::toBinMsg(current_map_, &compliant_map_msg); compliant_map_msg.map_version = current_map_version_; @@ -114,15 +114,17 @@ void WMBroadcaster::baseMapCallback(const autoware_lanelet2_msgs::MapBinConstPtr * \param gf_ptr geofence pointer * \param msg_v01 TrafficControlMessageV01 (geofence msg) */ -void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01) +void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) { // Handle schedule processing - cav_msgs::TrafficControlSchedule msg_schedule = msg_v01.params.schedule; + carma_v2x_msgs::msg::TrafficControlSchedule msg_schedule = msg_v01.params.schedule; + double ns_to_sec = 1.0e9; + auto clock_type = scheduler_.getClockType(); - ros::Time end_time = msg_schedule.end; + rclcpp::Time end_time = {msg_schedule.end, clock_type}; if (!msg_schedule.end_exists) { - ROS_DEBUG_STREAM("No end time for geofence, using ros::TIME_MAX"); - end_time = ros::TIME_MAX; // If there is no end time use the max time + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No end time for geofence, using rclcpp::Time::max()"); + end_time = {rclcpp::Time::max(), clock_type}; // If there is no end time use the max time } // If the days of the week are specified then convert them to the boost format. @@ -166,22 +168,22 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c for (auto daily_schedule : msg_schedule.between) { if (msg_schedule.repeat_exists) { - gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - daily_schedule.begin, - daily_schedule.duration, - msg_schedule.repeat.offset, - msg_schedule.repeat.span, - msg_schedule.repeat.period, + rclcpp::Duration(daily_schedule.begin), + rclcpp::Duration(daily_schedule.duration), + rclcpp::Duration(msg_schedule.repeat.offset), + rclcpp::Duration(msg_schedule.repeat.span), + rclcpp::Duration(msg_schedule.repeat.period), week_day_set)); } else { - gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - daily_schedule.begin, - daily_schedule.duration, - ros::Duration(0.0), // No offset - daily_schedule.duration - daily_schedule.begin, // Compute schedule portion end time - daily_schedule.duration - daily_schedule.begin, // No repetition so same as portion end time + rclcpp::Duration(daily_schedule.begin), + rclcpp::Duration(daily_schedule.duration), + rclcpp::Duration(0.0), // No offset + rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // Compute schedule portion end time + rclcpp::Duration(daily_schedule.duration) - rclcpp::Duration(daily_schedule.begin), // No repetition so same as portion end time week_day_set)); } @@ -189,22 +191,22 @@ void WMBroadcaster::addScheduleFromMsg(std::shared_ptr gf_ptr, const c } else { if (msg_schedule.repeat_exists) { - gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - ros::Duration(0.0), - ros::Duration(86400.0), // 24 hr daily application - msg_schedule.repeat.offset, - msg_schedule.repeat.span, - msg_schedule.repeat.period, + rclcpp::Duration(0.0), + rclcpp::Duration(86400.0e9), // 24 hr daily application + rclcpp::Duration(msg_schedule.repeat.offset), + rclcpp::Duration(msg_schedule.repeat.span), + rclcpp::Duration(msg_schedule.repeat.period), week_day_set)); } else { - gf_ptr->schedules.push_back(GeofenceSchedule(msg_schedule.start, + gf_ptr->schedules.push_back(GeofenceSchedule({msg_schedule.start, clock_type}, end_time, - ros::Duration(0.0), - ros::Duration(86400.0), // 24 hr daily application - ros::Duration(0.0), // No offset - ros::Duration(86400.0), // Applied for full lenth of 24 hrs - ros::Duration(86400.0), // No repetition + rclcpp::Duration(0.0), + rclcpp::Duration(86400.0e9), // 24 hr daily application + rclcpp::Duration(0.0), // No offset + rclcpp::Duration(86400.0e9), // Applied for full lenth of 24 hrs + rclcpp::Duration(86400.0e9), // No repetition week_day_set)); } @@ -212,18 +214,19 @@ 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> WMBroadcaster::geofenceFromMapMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::MapData& map_msg) { std::vector> updates_to_send; std::vector> intersections; std::vector> traffic_signals; auto sim_copy = sim_; + sim_.createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_); if (sim_ == sim_copy) // if no change { - ROS_DEBUG_STREAM(">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), ">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_); return {}; } @@ -256,10 +259,10 @@ std::vector> WMBroadcaster::geofenceFromMapMsg(std::sh return updates_to_send; } -void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01) +void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) { bool detected_workzone_signal = msg_v01.package.label_exists && msg_v01.package.label.find("SIG_WZ") != std::string::npos; - cav_msgs::TrafficControlDetail msg_detail = msg_v01.params.detail; + carma_v2x_msgs::msg::TrafficControlDetail msg_detail = msg_v01.params.detail; // Get ID std::copy(msg_v01.id.id.begin(), msg_v01.id.id.end(), gf_ptr->id_.begin()); @@ -269,7 +272,7 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_ gf_ptr->affected_parts_ = getAffectedLaneletOrAreas(gf_ptr->gf_pts); if (gf_ptr->affected_parts_.size() == 0) { - ROS_WARN_STREAM("There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "There is no applicable component in map for the new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); return; // Return empty geofence list } @@ -290,7 +293,7 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_ lanelet::Velocity sL; - if (msg_detail.choice == cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) { //Acquire speed limit information from TafficControlDetail msg sL = lanelet::Velocity(msg_detail.maxspeed * lanelet::units::MPS()); @@ -304,20 +307,20 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_ // @SONAR_STOP@ if(sL > 80_mph ) { - ROS_WARN_STREAM("Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital maximum speed limit is invalid. Value capped at max speed limit."); //Output warning message sL = 80_mph; //Cap the speed limit to the predetermined maximum value } if(sL < 0_mph) { - ROS_WARN_STREAM("Digital speed limit is invalid. Value set to 0mph."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph."); sL = 0_mph; }// @SONAR_START@ gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } - if (msg_detail.choice == cav_msgs::TrafficControlDetail::MINSPEED_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE) { //Acquire speed limit information from TafficControlDetail msg sL = lanelet::Velocity(msg_detail.minspeed * lanelet::units::MPS()); @@ -332,55 +335,55 @@ void WMBroadcaster::geofenceFromMsg(std::shared_ptr gf_ptr, const cav_ // @SONAR_STOP@ if(sL > 80_mph ) { - ROS_WARN_STREAM("Digital speed limit is invalid. Value capped at max speed limit."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value capped at max speed limit."); sL = 80_mph; } if(sL < 0_mph) { - ROS_WARN_STREAM("Digital speed limit is invalid. Value set to 0mph."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital speed limit is invalid. Value set to 0mph."); sL = 0_mph; }// @SONAR_START@ gf_ptr->regulatory_element_ = std::make_shared(lanelet::DigitalSpeedLimit::buildData(lanelet::utils::getId(), sL, affected_llts, affected_areas, participantsChecker(msg_v01), reason)); } - if (msg_detail.choice == cav_msgs::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATPERM_CHOICE || msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE) { addPassingControlLineFromMsg(gf_ptr, msg_v01, affected_llts); } - if (msg_detail.choice == cav_msgs::TrafficControlDetail::MINHDWY_CHOICE) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE) { double min_gap = (double)msg_detail.minhdwy; if(min_gap < 0) { - ROS_WARN_STREAM("Digital min gap is invalid. Value set to 0 meter."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Digital min gap is invalid. Value set to 0 meter."); min_gap = 0; } addRegionMinimumGap(gf_ptr,msg_v01, min_gap, affected_llts, affected_areas); } - if (detected_workzone_signal && msg_detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later + if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later { gf_ptr->label_ = msg_v01.package.label; // to extract intersection, and signal group id - if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == cav_msgs::TrafficControlDetail::CLOSED || - msg_detail.closed == cav_msgs::TrafficControlDetail::TAPERRIGHT || - msg_detail.closed == cav_msgs::TrafficControlDetail::OPENRIGHT)) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED || + msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT || + msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT)) { work_zone_geofence_cache_[msg_detail.closed] = gf_ptr; } - else if (msg_detail.choice == cav_msgs::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == cav_msgs::TrafficControlDetail::REVERSE) + else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE) { work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr; } return; } - else if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==cav_msgs::TrafficControlDetail::CLOSED) // if stand-alone closed signal apart from Workzone + else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && msg_detail.closed==carma_v2x_msgs::msg::TrafficControlDetail::CLOSED) // if stand-alone closed signal apart from Workzone { addRegionAccessRule(gf_ptr,msg_v01,affected_llts); } - if (msg_detail.choice == cav_msgs::TrafficControlDetail::RESTRICTED_CHOICE) { + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE) { addRegionAccessRule(gf_ptr, msg_v01, affected_llts); } @@ -407,7 +410,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeofence(std::unordered_m // erase cache now that it is processed for (auto pair : work_zone_geofence_cache) { - ROS_INFO_STREAM("Workzone geofence finished processing. Therefore following geofence id is being dropped from cache as it is processed as part of it: " << pair.second->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone geofence finished processing. Therefore following geofence id is being dropped from cache as it is processed as part of it: " << pair.second->id_); } work_zone_geofence_cache.clear(); @@ -425,7 +428,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m lanelet::Lanelet front_llt_diag = createLinearInterpolatingLanelet(parallel_llt_front.leftBound3d().back(), parallel_llt_front.rightBound3d().back(), middle_opposite_lanelets->back().rightBound3d().back(), middle_opposite_lanelets->back().leftBound3d().back()); - ROS_DEBUG_STREAM("Created diag front_llt_diag id:" << front_llt_diag.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created diag front_llt_diag id:" << front_llt_diag.id()); for (auto regem : middle_opposite_lanelets->back().regulatoryElements()) //copy existing regem into the new llts { front_llt_diag.addRegulatoryElement(regem); @@ -437,7 +440,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m lanelet::Lanelet back_llt_diag = createLinearInterpolatingLanelet(middle_opposite_lanelets->front().rightBound3d().front(), middle_opposite_lanelets->front().leftBound3d().front(), parallel_llt_back.leftBound3d().front(), parallel_llt_back.rightBound3d().front()); - ROS_DEBUG_STREAM("Created back_llt_diag diag id:" << back_llt_diag.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created back_llt_diag diag id:" << back_llt_diag.id()); for (auto regem : parallel_llt_back.regulatoryElements()) //copy existing regem into the new llts { back_llt_diag.addRegulatoryElement(regem); @@ -456,7 +459,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m middle_llt.addRegulatoryElement(regem); //copy existing regem into the new llts } middle_llts.push_back(middle_llt); - ROS_DEBUG_STREAM("Created matching direction of middle_llt id:" << middle_llt.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created matching direction of middle_llt id:" << middle_llt.id()); } ////////////////////////////// @@ -479,7 +482,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->label_),tfl_parallel->id()}); parallel_llt_front.addRegulatoryElement(tfl_parallel); - ROS_DEBUG_STREAM("Created TF_LIGHT of Id: " << tfl_parallel->id() << ", to parallel_llt_front id:" << parallel_llt_front.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_parallel->id() << ", to parallel_llt_front id:" << parallel_llt_front.id()); ////////////////////////////// //ADD TF_LIGHT TO OPPOSITE LANELET @@ -505,13 +508,13 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m gf_ptr->traffic_light_id_lookup_.push_back({generate32BitId(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->label_), tfl_opposite->id()}); opposite_llts_with_stop_line->front().addRegulatoryElement(tfl_opposite); - ROS_DEBUG_STREAM("Created TF_LIGHT of Id: " << tfl_opposite->id() << ", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Created TF_LIGHT of Id: " << tfl_opposite->id() << ", to opposite_llts_with_stop_line->front() id:" << opposite_llts_with_stop_line->front().id()); ////////////////////////////// //ADD ALL NEWLY CREATED LANELETS INTO GEOFENCE //OBJECTS TO BE PROCESSED LATER BY SCHEDULER ////////////////////////////// - ROS_DEBUG_STREAM("Added parallel_llt_front id:" << parallel_llt_front.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_front id:" << parallel_llt_front.id()); gf_ptr->lanelet_additions_.push_back(parallel_llt_front); gf_ptr->lanelet_additions_.push_back(front_llt_diag); @@ -519,7 +522,7 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), middle_llts.begin(), middle_llts.end()); gf_ptr->lanelet_additions_.push_back(back_llt_diag); - ROS_DEBUG_STREAM("Added parallel_llt_back id:" << parallel_llt_back.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Added parallel_llt_back id:" << parallel_llt_back.id()); gf_ptr->lanelet_additions_.push_back(parallel_llt_back); gf_ptr->lanelet_additions_.insert(gf_ptr->lanelet_additions_.end(), opposite_llts_with_stop_line->begin(), opposite_llts_with_stop_line->end());; @@ -530,23 +533,23 @@ std::shared_ptr WMBroadcaster::createWorkzoneGeometry(std::unordered_m ////////////////////////////// // fill information for paricipants and reason for blocking - cav_msgs::TrafficControlMessageV01 participants_and_reason_only; + carma_v2x_msgs::msg::TrafficControlMessageV01 participants_and_reason_only; - j2735_msgs::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule + j2735_v2x_msgs::msg::TrafficControlVehClass participant; // sending all possible VEHICLE will be processed as they are not accessuble by regionAccessRule - participant.vehicle_class = j2735_msgs::TrafficControlVehClass::MICROMOBILE; + participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE; participants_and_reason_only.params.vclasses.push_back(participant); - participant.vehicle_class = j2735_msgs::TrafficControlVehClass::BUS; + participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS; participants_and_reason_only.params.vclasses.push_back(participant); - participant.vehicle_class = j2735_msgs::TrafficControlVehClass::PASSENGER_CAR; + participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR; participants_and_reason_only.params.vclasses.push_back(participant); - participant.vehicle_class = j2735_msgs::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK; + participant.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK; participants_and_reason_only.params.vclasses.push_back(participant); @@ -614,11 +617,11 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapprevious(work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get()); if (previous_lanelets.empty()) //error if bad match { - ROS_ERROR_STREAM("Workzone area starts from lanelet with no previous lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id() + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area starts from lanelet with no previous lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::TAPERRIGHT]->affected_parts_.front().lanelet().get().id() << ". This case is rare and not supported at the moment."); return; } @@ -630,7 +633,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapinsert(parallel_llts->end(), new_taper_right_llts.begin(), new_taper_right_llts.end()); - ROS_DEBUG_STREAM("Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished TAPERRIGHT processing of size: " << new_taper_right_llts.size()); ////////////////////////////////// /// PARALLEL BACK (OPENRIGHT side) @@ -646,11 +649,11 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapfollowing(work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get()); if (next_lanelets.empty()) //error if bad match { - ROS_ERROR_STREAM("Workzone area ends at lanelet with no following lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id() + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Workzone area ends at lanelet with no following lanelet (Id : " << work_zone_geofence_cache[WorkZoneSection::OPENRIGHT]->affected_parts_.back().lanelet().get().id() << ". This case is rare and not supported at the moment."); return; } @@ -662,7 +665,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapinsert(parallel_llts->end(), new_open_right_llts.begin(), new_open_right_llts.end()); - ROS_DEBUG_STREAM("Finished OPENRIGHT processing of size: " << new_open_right_llts.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished OPENRIGHT processing of size: " << new_open_right_llts.size()); //////////////////// /// HANDLE MID HERE @@ -684,7 +687,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapinsert(opposite_llts->end(), temp_llts.begin(), temp_llts.end()); - ROS_DEBUG_STREAM("Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size()); return; } else if (temp_llts.size() == 2) // determine which @@ -704,7 +707,7 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapinsert(opposite_llts->end(), temp_llts.begin() + 1, temp_llts.end()- 1); } - ROS_DEBUG_STREAM("Finished REVERSE processing of size: " << opposite_llts->size() << " from original of 1 REVERSE lanelet size"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of 1 REVERSE lanelet size"); } else //if there are two or more lanelets { @@ -743,10 +746,10 @@ void WMBroadcaster::preprocessWorkzoneGeometry(std::unordered_mapinsert(opposite_llts->end(), temp_opposite_back_llts.begin(), temp_opposite_back_llts.end()); } - ROS_DEBUG_STREAM("Finished REVERSE processing of size: " << opposite_llts->size() << " from original of more than one REVERSE lanelet size"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Finished REVERSE processing of size: " << opposite_llts->size() << " from original of more than one REVERSE lanelet size"); } - ROS_DEBUG_STREAM("Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Ended preprocessWorkzoneGeometry with opposite_llts.size()" << opposite_llts->size() << ", and parallel_llts.size()" << parallel_llts->size()); } std::vector WMBroadcaster::splitLaneletWithPoint(const std::vector& input_pts, const lanelet::Lanelet& input_llt, double error_distance) @@ -766,7 +769,7 @@ std::vector WMBroadcaster::splitLaneletWithPoint(const std::ve auto new_parallel_llts = splitLaneletWithRatio(ratios, input_llt, error_distance); parallel_llts.insert(parallel_llts.end(),new_parallel_llts.begin(), new_parallel_llts.end()); - ROS_DEBUG_STREAM("splitLaneletWithPoint returning lanelets size: " << parallel_llts.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithPoint returning lanelets size: " << parallel_llts.size()); return parallel_llts; } @@ -781,13 +784,13 @@ lanelet::Lanelets WMBroadcaster::splitOppositeLaneletWithPoint(std::shared_ptrinsert(opposite_llts->begin(),new_llts_opposite.begin(), new_llts_opposite.end()); - ROS_DEBUG_STREAM("splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitOppositeLaneletWithPoint returning lanelets size: " << opposite_llts->size()); return opposing_llts; } @@ -805,7 +808,7 @@ std::vector WMBroadcaster::splitLaneletWithRatio(std::vector created_llts; std::sort(ratios.begin(), ratios.end()); - ROS_DEBUG_STREAM("splitLaneletWithRatio evaluating input ratios of size: " << ratios.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "splitLaneletWithRatio evaluating input ratios of size: " << ratios.size()); ratios.push_back(1.0); //needed to complete the loop @@ -825,13 +828,13 @@ std::vector WMBroadcaster::splitLaneletWithRatio(std::vector WMBroadcaster::splitLaneletWithRatio(std::vector gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const +void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const { - cav_msgs::TrafficControlDetail msg_detail; + carma_v2x_msgs::msg::TrafficControlDetail msg_detail; msg_detail = msg_v01.params.detail; // Get affected bounds lanelet::LineStrings3d pcl_bounds; - if (msg_detail.lataffinity == cav_msgs::TrafficControlDetail::LEFT) + if (msg_detail.lataffinity == carma_v2x_msgs::msg::TrafficControlDetail::LEFT) { for (auto llt : affected_llts) pcl_bounds.push_back(llt.leftBound()); gf_ptr->pcl_affects_left_ = true; @@ -892,26 +895,26 @@ void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_pt } // Get specified participants - ros::V_string left_participants; - ros::V_string right_participants; - ros::V_string participants=participantsChecker(msg_v01); + std::vector left_participants; + std::vector right_participants; + std::vector participants=participantsChecker(msg_v01); // Create the pcl depending on the allowed passing control direction, left, right, or both - if (msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::PERMITTED || - msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::PASSINGONLY) + if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED || + msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY) { left_participants = participants; } - else if (msg_detail.latperm[0] == cav_msgs::TrafficControlDetail::EMERGENCYONLY) + else if (msg_detail.latperm[0] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY) { left_participants.push_back(lanelet::Participants::VehicleEmergency); } - if (msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::PERMITTED || - msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::PASSINGONLY) + if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED || + msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::PASSINGONLY) { right_participants = participants; } - else if (msg_detail.latperm[1] == cav_msgs::TrafficControlDetail::EMERGENCYONLY) + else if (msg_detail.latperm[1] == carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY) { right_participants.push_back(lanelet::Participants::VehicleEmergency); } @@ -920,7 +923,7 @@ void WMBroadcaster::addPassingControlLineFromMsg(std::shared_ptr gf_pt lanelet::utils::getId(), pcl_bounds, left_participants, right_participants)); } -void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const +void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const { const std::string& reason = msg_v01.package.label; gf_ptr->label_ = msg_v01.package.label; @@ -937,7 +940,7 @@ void WMBroadcaster::addRegionAccessRule(std::shared_ptr gf_ptr, const gf_ptr->regulatory_element_ = regulatory_element; } -void WMBroadcaster::addRegionMinimumGap(std::shared_ptr gf_ptr, const cav_msgs::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector& affected_llts, const std::vector& affected_areas) const +void WMBroadcaster::addRegionMinimumGap(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, double min_gap, const std::vector& affected_llts, const std::vector& affected_areas) const { auto regulatory_element = std::make_shared(lanelet::DigitalMinimumGap::buildData(lanelet::utils::getId(), min_gap, affected_llts, affected_areas,participantsChecker(msg_v01) )); @@ -945,36 +948,36 @@ void WMBroadcaster::addRegionMinimumGap(std::shared_ptr gf_ptr, const gf_ptr->regulatory_element_ = regulatory_element; } -ros::V_string WMBroadcaster::participantsChecker(const cav_msgs::TrafficControlMessageV01& msg_v01) const +std::vector WMBroadcaster::participantsChecker(const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01) const { - ros::V_string participants; - for (j2735_msgs::TrafficControlVehClass participant : msg_v01.params.vclasses) + std::vector participants; + for (j2735_v2x_msgs::msg::TrafficControlVehClass participant : msg_v01.params.vclasses) { - // Currently j2735_msgs::TrafficControlVehClass::RAIL is not supported - if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::ANY) + // Currently j2735_v2x_msgs::msg::TrafficControlVehClass::RAIL is not supported + if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::ANY) { participants = {lanelet::Participants::Vehicle, lanelet::Participants::Pedestrian, lanelet::Participants::Bicycle}; break; } - else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::PEDESTRIAN) + else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN) { participants.push_back(lanelet::Participants::Pedestrian); } - else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::BICYCLE) + else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE) { participants.push_back(lanelet::Participants::Bicycle); } - else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::MICROMOBILE || - participant.vehicle_class == j2735_msgs::TrafficControlVehClass::MOTORCYCLE) + else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MICROMOBILE || + participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE) { participants.push_back(lanelet::Participants::VehicleMotorcycle); } - else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::BUS) + else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::BUS) { participants.push_back(lanelet::Participants::VehicleBus); } - else if (participant.vehicle_class == j2735_msgs::TrafficControlVehClass::LIGHT_TRUCK_VAN || - participant.vehicle_class == j2735_msgs::TrafficControlVehClass::PASSENGER_CAR) + else if (participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN || + participant.vehicle_class == j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR) { participants.push_back(lanelet::Participants::VehicleCar); } @@ -987,9 +990,9 @@ ros::V_string WMBroadcaster::participantsChecker(const cav_msgs::TrafficControlM return combineParticipantsToVehicle(participants); } -ros::V_string WMBroadcaster::invertParticipants(const ros::V_string& input_participants) const +std::vector WMBroadcaster::invertParticipants(const std::vector& input_participants) const { - ros::V_string participants; + std::vector participants; if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Pedestrian ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Pedestrian); if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::Bicycle ) == input_participants.end()) participants.emplace_back(lanelet::Participants::Bicycle); @@ -1003,44 +1006,44 @@ ros::V_string WMBroadcaster::invertParticipants(const ros::V_string& input_parti return participants; } -ros::V_string WMBroadcaster::combineParticipantsToVehicle(const ros::V_string& input_participants) const +std::vector WMBroadcaster::combineParticipantsToVehicle(const std::vector& input_participants) const { - ros::V_string participants; + std::vector participants; if(std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleMotorcycle)!= input_participants.end() && std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleBus) != input_participants.end() && std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleCar) != input_participants.end() && std::find(input_participants.begin(),input_participants.end(),lanelet::Participants::VehicleTruck) != input_participants.end()) { - ROS_DEBUG_STREAM("Detected participants to cover all possible vehicle types"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Detected participants to cover all possible vehicle types"); participants.emplace_back(lanelet::Participants::Vehicle); } else { - ROS_DEBUG_STREAM("Not making any changes to the participants list"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Not making any changes to the participants list"); participants = input_participants; } return participants; } -void WMBroadcaster::externalMapMsgCallback(const cav_msgs::MapData& map_msg) +void WMBroadcaster::externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniquePtr map_msg) { auto gf_ptr = std::make_shared(); if (!current_map_ || current_map_->laneletLayer.size() == 0) { - ROS_INFO_STREAM("Map is not available yet. Skipping MAP msg"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Map is not available yet. Skipping MAP msg"); return; } // 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()) + 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) + for (auto intersection : map_msg->intersections) { if (sim_.intersection_id_to_regem_id_.find(intersection.id.id) == sim_.intersection_id_to_regem_id_.end()) { @@ -1055,13 +1058,13 @@ void WMBroadcaster::externalMapMsgCallback(const cav_msgs::MapData& map_msg) return; } - gf_ptr->map_msg_ = map_msg; + gf_ptr->map_msg_ = *map_msg; gf_ptr->msg_.package.label_exists = true; gf_ptr->msg_.package.label = "MAP_MSG"; gf_ptr->id_ = boost::uuids::random_generator()(); // create dummy traffic Control message to add instant activation schedule - cav_msgs::TrafficControlMessageV01 traffic_control_msg; + carma_v2x_msgs::msg::TrafficControlMessageV01 traffic_control_msg; // process schedule from message addScheduleFromMsg(gf_ptr, traffic_control_msg); @@ -1071,39 +1074,39 @@ void WMBroadcaster::externalMapMsgCallback(const cav_msgs::MapData& map_msg) } // currently only supports geofence message version 1: TrafficControlMessageV01 -void WMBroadcaster::geofenceCallback(const cav_msgs::TrafficControlMessage& geofence_msg) +void WMBroadcaster::geofenceCallback(carma_v2x_msgs::msg::TrafficControlMessage::UniquePtr geofence_msg) { std::lock_guard guard(map_mutex_); std::stringstream reason_ss; // quickly check if the id has been added - if (geofence_msg.choice != cav_msgs::TrafficControlMessage::TCMV01) { - reason_ss << "Dropping received geofence for unsupported TrafficControl version: " << geofence_msg.choice; - ROS_WARN_STREAM(reason_ss.str()); - pubTCMACK(geofence_msg.tcm_v01.reqid, geofence_msg.tcm_v01.msgnum, static_cast(AcknowledgementStatus::REJECTED), reason_ss.str()); + if (geofence_msg->choice != carma_v2x_msgs::msg::TrafficControlMessage::TCMV01) { + reason_ss << "Dropping received geofence for unsupported TrafficControl version: " << geofence_msg->choice; + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str()); + pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast(AcknowledgementStatus::REJECTED), reason_ss.str()); return; } boost::uuids::uuid id; - std::copy(geofence_msg.tcm_v01.id.id.begin(), geofence_msg.tcm_v01.id.id.end(), id.begin()); + std::copy(geofence_msg->tcm_v01.id.id.begin(), geofence_msg->tcm_v01.id.id.end(), id.begin()); if (checked_geofence_ids_.find(boost::uuids::to_string(id)) != checked_geofence_ids_.end()) { reason_ss.str(""); reason_ss << "Dropping received TrafficControl message with already handled id: " << boost::uuids::to_string(id); - ROS_DEBUG_STREAM(reason_ss.str()); - pubTCMACK(geofence_msg.tcm_v01.reqid, geofence_msg.tcm_v01.msgnum, static_cast(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), reason_ss.str()); + pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str()); return; } // convert reqid to string check if it has been seen before boost::array req_id; - for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg.tcm_v01.reqid.id[i]; + for (auto i = 0; i < 8; i ++) req_id[i] = geofence_msg->tcm_v01.reqid.id[i]; boost::uuids::uuid uuid_id; std::copy(req_id.begin(),req_id.end(), uuid_id.begin()); std::string reqid = boost::uuids::to_string(uuid_id).substr(0, 8); // drop if the req has never been sent if (generated_geofence_reqids_.find(reqid) == generated_geofence_reqids_.end() && reqid.compare("00000000") != 0) { - ROS_WARN_STREAM("CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "CARMA_WM_CTRL received a TrafficControlMessage with unknown TrafficControlRequest ID (reqid): " << reqid); return; } @@ -1111,36 +1114,36 @@ void WMBroadcaster::geofenceCallback(const cav_msgs::TrafficControlMessage& geof auto gf_ptr = std::make_shared(); - gf_ptr->msg_ = geofence_msg.tcm_v01; + gf_ptr->msg_ = geofence_msg->tcm_v01; try { // process schedule from message - addScheduleFromMsg(gf_ptr, geofence_msg.tcm_v01); + addScheduleFromMsg(gf_ptr, geofence_msg->tcm_v01); scheduleGeofence(gf_ptr); reason_ss.str(""); reason_ss << "Successfully processed TCM."; - pubTCMACK(geofence_msg.tcm_v01.reqid, geofence_msg.tcm_v01.msgnum, static_cast(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str()); + pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast(AcknowledgementStatus::ACKNOWLEDGED), reason_ss.str()); } catch(std::exception& ex) { reason_ss.str(""); reason_ss << "Failed to process TCM. " << ex.what(); - pubTCMACK(geofence_msg.tcm_v01.reqid, geofence_msg.tcm_v01.msgnum, static_cast(AcknowledgementStatus::REJECTED), reason_ss.str()); + pubTCMACK(geofence_msg->tcm_v01.reqid, geofence_msg->tcm_v01.msgnum, static_cast(AcknowledgementStatus::REJECTED), reason_ss.str()); throw; //rethrows the exception object } }; void WMBroadcaster::scheduleGeofence(std::shared_ptr gf_ptr) { - ROS_INFO_STREAM("Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Scheduling new geofence message received by WMBroadcaster with id: " << gf_ptr->id_); bool detected_workzone_signal = gf_ptr->msg_.package.label_exists && gf_ptr->msg_.package.label.find("SIG_WZ") != std::string::npos; - cav_msgs::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail; + carma_v2x_msgs::msg::TrafficControlDetail msg_detail = gf_ptr->msg_.params.detail; // create workzone specific extra speed geofence - if (detected_workzone_signal && msg_detail.choice == cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) + if (detected_workzone_signal && msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) { // duplicate the messages with inverted points to support new lanelets created from workzone // as carma-cloud currently does not support geofence points with direction opposite to that of the road @@ -1148,7 +1151,7 @@ void WMBroadcaster::scheduleGeofence(std::shared_ptr gf auto gf_ptr_speed = std::make_shared(); gf_ptr_speed->schedules = gf_ptr->schedules; - cav_msgs::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_; + carma_v2x_msgs::msg::TrafficControlMessageV01 duplicate_msg = gf_ptr->msg_; std::reverse(duplicate_msg.geometry.nodes.begin() + 1, duplicate_msg.geometry.nodes.end()); double first_x = 0; @@ -1168,22 +1171,22 @@ void WMBroadcaster::scheduleGeofence(std::shared_ptr gf gf_ptr_speed->msg_ = duplicate_msg; scheduler_.addGeofence(gf_ptr_speed); } - if (detected_workzone_signal && msg_detail.choice != cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later + if (detected_workzone_signal && msg_detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) // if workzone message detected, save to cache to process later { gf_ptr->label_ = gf_ptr->msg_.package.label; // to extract intersection, and signal group id - if (msg_detail.choice == cav_msgs::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == cav_msgs::TrafficControlDetail::CLOSED || - msg_detail.closed == cav_msgs::TrafficControlDetail::TAPERRIGHT || - msg_detail.closed == cav_msgs::TrafficControlDetail::OPENRIGHT)) + if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE && (msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::CLOSED || + msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::TAPERRIGHT || + msg_detail.closed == carma_v2x_msgs::msg::TrafficControlDetail::OPENRIGHT)) { work_zone_geofence_cache_[msg_detail.closed] = gf_ptr; } - else if (msg_detail.choice == cav_msgs::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == cav_msgs::TrafficControlDetail::REVERSE) + else if (msg_detail.choice == carma_v2x_msgs::msg::TrafficControlDetail::DIRECTION_CHOICE && msg_detail.direction == carma_v2x_msgs::msg::TrafficControlDetail::REVERSE) { work_zone_geofence_cache_[WorkZoneSection::REVERSE] = gf_ptr; } if (work_zone_geofence_cache_.size() < WORKZONE_TCM_REQUIRED_SIZE) { - ROS_INFO_STREAM("Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now..."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Received 'SIG_WZ' signal. Waiting for the rest of the messages, returning for now..."); return; } } @@ -1192,11 +1195,11 @@ void WMBroadcaster::scheduleGeofence(std::shared_ptr gf } -void WMBroadcaster::geoReferenceCallback(const std_msgs::String& geo_ref) +void WMBroadcaster::geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref) { std::lock_guard guard(map_mutex_); - sim_.setTargetFrame(geo_ref.data); - base_map_georef_ = geo_ref.data; + sim_.setTargetFrame(geo_ref->data); + base_map_georef_ = geo_ref->data; } void WMBroadcaster::setMaxLaneWidth(double max_lane_width) @@ -1205,6 +1208,20 @@ void WMBroadcaster::setMaxLaneWidth(double max_lane_width) sim_.setMaxLaneWidth(max_lane_width_); } +void WMBroadcaster::setIntersectionCoordCorrection(const std::vector& intersection_ids_for_correction, const std::vector& intersection_correction) +{ + if (intersection_correction.size() % 2 != 0 || intersection_ids_for_correction.size() != intersection_correction.size() / 2) + { + throw std::invalid_argument("Some of intersection coordinate correction parameters are not fully set!"); + } + + for (auto i = 0; i < intersection_correction.size(); i = i + 2) + { + sim_.intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first = intersection_correction[i]; //x + sim_.intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y + } +} + void WMBroadcaster::setConfigSpeedLimit(double cL) { /*Logic to change config_lim to Velocity value config_limit*/ @@ -1241,9 +1258,9 @@ uint32_t WMBroadcaster::generate32BitId(const std::string& label) } // currently only supports geofence message version 1: TrafficControlMessageV01 -lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficControlMessageV01& tcm_v01) +lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const carma_v2x_msgs::msg::TrafficControlMessageV01& tcm_v01) { - ROS_DEBUG_STREAM("Getting affected lanelets"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Getting affected lanelets"); if (!current_map_ || current_map_->laneletLayer.size() == 0) { throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster")); @@ -1259,23 +1276,23 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficCo std::string projection = tcm_v01.geometry.proj; std::string datum = tcm_v01.geometry.datum; if (datum.empty()) { - ROS_WARN_STREAM("Datum field not populated. Attempting to use WGS84"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field not populated. Attempting to use WGS84"); datum = "WGS84"; } - ROS_DEBUG_STREAM("Projection field: " << projection); - ROS_DEBUG_STREAM("Datum field: " << datum); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection field: " << projection); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Datum field: " << datum); std::string universal_frame = datum; //lat/long included in TCM is in this datum - ROS_DEBUG_STREAM("Traffic Control heading provided: " << tcm_v01.geometry.heading << " System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Traffic Control heading provided: " << tcm_v01.geometry.heading << " System understanding is that this value will not affect the projection and is only provided for supporting derivative calculations."); // Create the resulting projection transformation PJ* universal_to_target = proj_create_crs_to_crs(PJ_DEFAULT_CTX, universal_frame.c_str(), projection.c_str(), nullptr); if (universal_to_target == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - ROS_ERROR_STREAM("Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " universal_frame: " << universal_frame << " projection: " << projection); return {}; // Ignore geofence if it could not be projected from universal to TCM frame @@ -1285,7 +1302,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficCo if (target_to_map == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - ROS_ERROR_STREAM("Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between geofence and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " target_to_map: " << target_to_map << " base_map_georef_: " << base_map_georef_); return {}; // Ignore geofence if it could not be projected into the map frame @@ -1294,17 +1311,17 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficCo // convert all geofence points into our map's frame std::vector gf_pts; - cav_msgs::PathNode prev_pt; + carma_v2x_msgs::msg::PathNode prev_pt; PJ_COORD c_init_latlong{{tcm_v01.geometry.reflat, tcm_v01.geometry.reflon, tcm_v01.geometry.refelv}}; PJ_COORD c_init = proj_trans(universal_to_target, PJ_FWD, c_init_latlong); prev_pt.x = c_init.xyz.x; prev_pt.y = c_init.xyz.y; - ROS_DEBUG_STREAM("In TCM's frame, initial Point X "<< prev_pt.x<<" Before conversion: Point Y "<< prev_pt.y ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "In TCM's frame, initial Point X "<< prev_pt.x<<" Before conversion: Point Y "<< prev_pt.y ); for (auto pt : tcm_v01.geometry.nodes) { - ROS_DEBUG_STREAM("Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Before conversion in TCM frame: Point X "<< pt.x <<" Before conversion: Point Y "<< pt.y); PJ_COORD c {{prev_pt.x + pt.x, prev_pt.y + pt.y, 0, 0}}; // z is not currently used PJ_COORD c_out; @@ -1314,7 +1331,7 @@ lanelet::Points3d WMBroadcaster::getPointsInLocalFrame(const cav_msgs::TrafficCo prev_pt.x += pt.x; prev_pt.y += pt.y; - ROS_DEBUG_STREAM("After conversion in Map frame: Point X "<< gf_pts.back().x() <<" After conversion: Point Y "<< gf_pts.back().y()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "After conversion in Map frame: Point X "<< gf_pts.back().x() <<" After conversion: Point Y "<< gf_pts.back().y()); } // save the points converted to local map frame @@ -1390,7 +1407,7 @@ void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) con current_map_->update(current_map_->laneletLayer.get(el.id()), gf_ptr->regulatory_element_); gf_ptr->update_list_.push_back(std::pair(el.id(), gf_ptr->regulatory_element_)); } else { - ROS_WARN_STREAM("Regulatory element with invalid id in geofence cannot be added to the map"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Regulatory element with invalid id in geofence cannot be added to the map"); } } @@ -1433,7 +1450,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) { std::lock_guard guard(map_mutex_); - ROS_INFO_STREAM("Adding active geofence to the map with geofence id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "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 @@ -1441,7 +1458,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) 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) + if (detected_workzone_signal && gf_ptr->msg_.params.detail.choice != carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE) { for (auto gf_cache_ptr : work_zone_geofence_cache_) { @@ -1460,10 +1477,13 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) } for (auto update : updates_to_send) - { + { // add marker to rviz tcm_marker_array_.markers.push_back(composeTCMMarkerVisualizer(update->gf_pts)); // create visualizer in rviz + if (update->affected_parts_.empty()) + continue; + // Process the geofence object to populate update remove lists addGeofenceHelper(update); @@ -1472,28 +1492,28 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) for (auto pair : update->update_list_) active_geofence_llt_ids_.insert(pair.first); } - autoware_lanelet2_msgs::MapBin gf_msg; + autoware_lanelet2_msgs::msg::MapBin gf_msg; // 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"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "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); - ROS_INFO_STREAM("Done rebuilding routing graph after is was invalidated by geofence"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence"); // Populate routing graph structure - ROS_INFO_STREAM("Creating routing graph message"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message"); auto readable_graph = std::static_pointer_cast(current_routing_graph_); gf_msg.routing_graph = readable_graph->routingGraphToMsg(participant_); gf_msg.has_routing_graph = true; - ROS_INFO_STREAM("Done creating routing graph message"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message"); } @@ -1511,7 +1531,6 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) gf_msg.seq_id = 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); } @@ -1520,72 +1539,74 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) void WMBroadcaster::removeGeofence(std::shared_ptr gf_ptr) { std::lock_guard guard(map_mutex_); - ROS_INFO_STREAM("Removing inactive geofence from the map with geofence id: " << gf_ptr->id_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Removing inactive geofence from the map with geofence id: " << gf_ptr->id_); // Process the geofence object to populate update remove lists + if (gf_ptr->affected_parts_.empty()) + return; + removeGeofenceHelper(gf_ptr); for (auto pair : gf_ptr->remove_list_) active_geofence_llt_ids_.erase(pair.first); // publish - autoware_lanelet2_msgs::MapBin gf_msg_revert; + autoware_lanelet2_msgs::msg::MapBin gf_msg_revert; auto send_data = std::make_shared(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {})); if (gf_ptr->invalidate_route_) { // If a geofence initially invalidated the route it stands to reason its removal should as well - ROS_INFO_STREAM("Rebuilding routing graph after is was invalidated by geofence removal"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Rebuilding routing graph after is was invalidated by geofence removal"); 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 removal"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done rebuilding routing graph after is was invalidated by geofence removal"); // Populate routing graph structure - ROS_INFO_STREAM("Creating routing graph message for geofence removal"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Creating routing graph message for geofence removal"); auto readable_graph = std::static_pointer_cast(current_routing_graph_); gf_msg_revert.routing_graph = readable_graph->routingGraphToMsg(participant_); gf_msg_revert.has_routing_graph = true; - ROS_INFO_STREAM("Done creating routing graph message for geofence removal"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Done creating routing graph message for geofence removal"); } carma_wm::toBinMsg(send_data, &gf_msg_revert); update_count_++; // Update the sequence count for geofence messages gf_msg_revert.seq_id = update_count_; gf_msg_revert.map_version = current_map_version_; - map_update_message_queue_.push_back(gf_msg_revert); // Add diff to current map update queue + map_update_pub_(gf_msg_revert); } -cav_msgs::Route WMBroadcaster::getRoute() +carma_planning_msgs::msg::Route WMBroadcaster::getRoute() { return current_route; } -void WMBroadcaster::routeCallbackMessage(const cav_msgs::Route& route_msg) +void WMBroadcaster::routeCallbackMessage(carma_planning_msgs::msg::Route::UniquePtr route_msg) { - - current_route = route_msg; - cav_msgs::TrafficControlRequest cR; - cR = controlRequestFromRoute(route_msg); + current_route = *route_msg; + carma_v2x_msgs::msg::TrafficControlRequest cR; + cR = controlRequestFromRoute(*route_msg); control_msg_pub_(cR); } -cav_msgs::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const cav_msgs::Route& route_msg, std::shared_ptr req_id_for_testing) +carma_v2x_msgs::msg::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const carma_planning_msgs::msg::Route& route_msg, std::shared_ptr req_id_for_testing) { lanelet::ConstLanelets path; if (!current_map_ || current_map_->laneletLayer.size() == 0) { // Return / log warning etc. - ROS_INFO_STREAM("Value 'current_map_' does not exist."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'current_map_' does not exist."); throw lanelet::InvalidObjectStateError(std::string("Base lanelet map is not loaded to the WMBroadcaster")); } @@ -1642,7 +1663,7 @@ cav_msgs::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const cav if (target_frame.empty()) { // Return / log warning etc. - ROS_INFO_STREAM("Value 'target_frame' is empty."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Value 'target_frame' is empty."); throw lanelet::InvalidObjectStateError(std::string("Base georeference map may not be loaded to the WMBroadcaster")); } @@ -1664,17 +1685,17 @@ cav_msgs::TrafficControlRequest WMBroadcaster::controlRequestFromRoute(const cav if (tmerc_proj == nullptr) { // proj_create_crs_to_crs returns 0 when there is an error in the projection - ROS_ERROR_STREAM("Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Failed to generate projection between request bounds frame and map with error number: " << proj_context_errno(PJ_DEFAULT_CTX) << " MapProjection: " << target_frame << " Message Projection: " << local_tmerc_enu_proj); return {}; // Ignore geofence if it could not be projected into the map frame } - ROS_DEBUG_STREAM("Before conversion: Top Left: ("<< minX <<", "<& input) +visualization_msgs::msg::Marker WMBroadcaster::composeTCMMarkerVisualizer(const std::vector& input) { // create the marker msgs - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.type = visualization_msgs::Marker::SPHERE_LIST; - marker.action = visualization_msgs::Marker::ADD; + marker.header.stamp = rclcpp::Time(); + marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; marker.ns = "route_visualizer"; marker.scale.x = 0.65; @@ -1794,7 +1815,7 @@ visualization_msgs::Marker WMBroadcaster::composeTCMMarkerVisualizer(const std:: for (int i = 0; i < input.size(); i++) { - geometry_msgs::Point temp_point; + geometry_msgs::msg::Point temp_point; temp_point.x = input[i].x(); temp_point.y = input[i].y(); temp_point.z = 2; //to show up on top of the lanelet lines @@ -1821,7 +1842,6 @@ double WMBroadcaster::distToNearestActiveGeofence(const lanelet::BasicPoint2d& c if (active_geofence_llt_ids_.find(llt.id()) != active_geofence_llt_ids_.end()) active_geofence_on_route.push_back(llt.id()); } - // Get the lanelet of this point auto curr_lanelet = lanelet::geometry::findNearest(current_map_->laneletLayer, curr_pos, 1)[0].second; @@ -1884,16 +1904,16 @@ void WMBroadcaster::removeGeofenceHelper(std::shared_ptr gf_ptr) const gf_ptr->prev_regems_ = {}; } -void WMBroadcaster::currentLocationCallback(const geometry_msgs::PoseStamped& current_pos) +void WMBroadcaster::currentLocationCallback(geometry_msgs::msg::PoseStamped::UniquePtr current_pos) { if (current_map_ && current_map_->laneletLayer.size() != 0) { - cav_msgs::CheckActiveGeofence check = checkActiveGeofenceLogic(current_pos); + carma_perception_msgs::msg::CheckActiveGeofence check = checkActiveGeofenceLogic(*current_pos); active_pub_(check);//Publish } else { - ROS_DEBUG_STREAM("Could not check active geofence logic because map was not loaded"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Could not check active geofence logic because map was not loaded"); } - } + bool WMBroadcaster::convertLightIdToInterGroupId(unsigned& intersection_id, unsigned& group_id, const lanelet::Id& lanelet_id) { for (auto it = traffic_light_id_lookup_.begin(); it != traffic_light_id_lookup_.end(); ++it) @@ -1925,14 +1945,14 @@ void WMBroadcaster::publishLightId() if (!traffic_lights.empty()) { - ROS_DEBUG_STREAM("Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light Regulatory Element id: " << traffic_lights.front()->id()); convert_success = convertLightIdToInterGroupId(intersection_id,group_id, traffic_lights.front()->id()); } if (!convert_success) continue; - ROS_DEBUG_STREAM("Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Found Traffic Light with Intersection id: " << intersection_id << " Group id:" << group_id); bool id_exists = false; for (int idx = 0; idx < upcoming_intersection_ids_.data.size(); idx +2) { @@ -1950,7 +1970,7 @@ void WMBroadcaster::publishLightId() upcoming_intersection_ids_.data.push_back(static_cast(group_id)); } } -cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geometry_msgs::PoseStamped& current_pos) +carma_perception_msgs::msg::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geometry_msgs::msg::PoseStamped& current_pos) { if (!current_map_ || current_map_->laneletLayer.size() == 0) @@ -1969,12 +1989,12 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom curr_pos.x() = current_pos_x; curr_pos.y() = current_pos_y; - cav_msgs::CheckActiveGeofence outgoing_geof; //message to publish + carma_perception_msgs::msg::CheckActiveGeofence outgoing_geof; //message to publish double next_distance = 0 ; //Distance to next geofence if (active_geofence_llt_ids_.size() <= 0 ) { - ROS_INFO_STREAM("No active geofence llt ids are loaded to the WMBroadcaster"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "No active geofence llt ids are loaded to the WMBroadcaster"); return outgoing_geof; } @@ -1992,7 +2012,7 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom { if (id == current_llt.id()) { - ROS_DEBUG_STREAM("Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Vehicle is on Lanelet " << current_llt.id() << ", which has an active geofence"); outgoing_geof.is_on_active_geofence = true; for (auto regem: current_llt.regulatoryElements()) { @@ -2005,12 +2025,12 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom outgoing_geof.advisory_speed = speed->speed_limit_.value(); outgoing_geof.reason = speed->getReason(); - ROS_DEBUG_STREAM("Active geofence has a speed limit of " << speed->speed_limit_.value()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a speed limit of " << speed->speed_limit_.value()); // Cannot overrule outgoing_geof.type if it is already set to LANE_CLOSED - if(outgoing_geof.type != cav_msgs::CheckActiveGeofence::LANE_CLOSED) + if(outgoing_geof.type != carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED) { - outgoing_geof.type = cav_msgs::CheckActiveGeofence::SPEED_LIMIT; + outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::SPEED_LIMIT; } } @@ -2020,7 +2040,7 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom lanelet::DigitalMinimumGapPtr min_gap = std::dynamic_pointer_cast (current_map_->regulatoryElementLayer.get(regem->id())); outgoing_geof.minimum_gap = min_gap->getMinimumGap(); - ROS_DEBUG_STREAM("Active geofence has a minimum gap of " << min_gap->getMinimumGap()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence has a minimum gap of " << min_gap->getMinimumGap()); } // Assign active geofence fields based on whether the current lane is closed or is immediately adjacent to a closed lane @@ -2032,10 +2052,10 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom // Update the 'type' and 'reason' for this active geofence if the vehicle is in a closed lane if(!accessRuleReg->accessable(lanelet::Participants::VehicleCar) || !accessRuleReg->accessable(lanelet::Participants::VehicleTruck)) { - ROS_DEBUG_STREAM("Active geofence is a closed lane."); - ROS_DEBUG_STREAM("Closed lane reason: " << accessRuleReg->getReason()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Active geofence is a closed lane."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Closed lane reason: " << accessRuleReg->getReason()); outgoing_geof.reason = accessRuleReg->getReason(); - outgoing_geof.type = cav_msgs::CheckActiveGeofence::LANE_CLOSED; + outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED; } // Otherwise, update the 'type' and 'reason' for this active geofence if the vehicle is in a lane immediately adjacent to a closed lane with the same travel direction else @@ -2059,11 +2079,11 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom (current_map_->regulatoryElementLayer.get(rightRegem->id())); if(!rightAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !rightAccessRuleReg->accessable(lanelet::Participants::VehicleTruck)) { - ROS_DEBUG_STREAM("Right adjacent Lanelet " << lanelet.id() << " is CLOSED"); - ROS_DEBUG_STREAM("Assigning LANE_CLOSED type to active geofence"); - ROS_DEBUG_STREAM("Assigning reason " << rightAccessRuleReg->getReason()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Right adjacent Lanelet " << lanelet.id() << " is CLOSED"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << rightAccessRuleReg->getReason()); outgoing_geof.reason = rightAccessRuleReg->getReason(); - outgoing_geof.type = cav_msgs::CheckActiveGeofence::LANE_CLOSED; + outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED; } } } @@ -2088,11 +2108,11 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom (current_map_->regulatoryElementLayer.get(leftRegem->id())); if(!leftAccessRuleReg->accessable(lanelet::Participants::VehicleCar) || !leftAccessRuleReg->accessable(lanelet::Participants::VehicleTruck)) { - ROS_DEBUG_STREAM("Left adjacent Lanelet " << lanelet.id() << " is CLOSED"); - ROS_DEBUG_STREAM("Assigning LANE_CLOSED type to active geofence"); - ROS_DEBUG_STREAM("Assigning reason " << leftAccessRuleReg->getReason()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Left adjacent Lanelet " << lanelet.id() << " is CLOSED"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning LANE_CLOSED type to active geofence"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Assigning reason " << leftAccessRuleReg->getReason()); outgoing_geof.reason = leftAccessRuleReg->getReason(); - outgoing_geof.type = cav_msgs::CheckActiveGeofence::LANE_CLOSED; + outgoing_geof.type = carma_perception_msgs::msg::CheckActiveGeofence::LANE_CLOSED; } } } @@ -2108,13 +2128,6 @@ cav_msgs::CheckActiveGeofence WMBroadcaster::checkActiveGeofenceLogic(const geom return outgoing_geof; } -void WMBroadcaster::newUpdateSubscriber(const ros::SingleSubscriberPublisher& single_sub_pub) const { - - for (const auto& msg : map_update_message_queue_) { - single_sub_pub.publish(msg); // For each applied update for the current map version publish the update to the new subscriber - } -} - lanelet::LineString3d WMBroadcaster::createLinearInterpolatingLinestring(const lanelet::Point3d& front_pt, const lanelet::Point3d& back_pt, double increment_distance) { double dx = back_pt.x() - front_pt.x(); @@ -2149,6 +2162,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() lanelet::Lanelet route_lanelet; lanelet::Ids cur_route_lanelet_ids = current_route.route_path_lanelet_ids; bool isLightFound = false; + for(auto id : cur_route_lanelet_ids) { route_lanelet= current_map_->laneletLayer.get(id); @@ -2171,14 +2185,14 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() } } else{ - ROS_DEBUG_STREAM("NO matching Traffic lights along the route"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching Traffic lights along the route"); }//END Traffic signals auto intersections = route_lanelet.regulatoryElementsAs(); if (intersections.empty()) { // no match if any of the entry lanelet is not part of any intersection. - ROS_DEBUG_STREAM("NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "NO matching intersection for current lanelet. lanelet id = " << route_lanelet.id()); } else { @@ -2196,7 +2210,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() } } //END intersections - ROS_DEBUG_STREAM("MAP msg: Intersection ID = " << map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "MAP msg: Intersection ID = " << map_msg_intersection_id << ", Signal Group ID =" << cur_signal_group_id ); if(map_msg_intersection_id != 0 && cur_signal_group_id != 0) { upcoming_intersection_ids_.data.clear(); @@ -2205,10 +2219,10 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() } } -void WMBroadcaster::pubTCMACK(j2735_msgs::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason) +void WMBroadcaster::pubTCMACK(j2735_v2x_msgs::msg::Id64b tcm_req_id, uint16_t msgnum, int ack_status, const std::string& ack_reason) { - cav_msgs::MobilityOperation mom_msg; - mom_msg.m_header.timestamp = ros::Time::now().toNSec()/1000000; + carma_v2x_msgs::msg::MobilityOperation mom_msg; + mom_msg.m_header.timestamp = scheduler_.now().nanoseconds()/1000000; mom_msg.m_header.sender_id = vehicle_id_; mom_msg.strategy = geofence_ack_strategy_; std::stringstream ss; diff --git a/carma_wm_ctrl/src/WMBroadcasterNode.cpp b/carma_wm_ctrl/src/WMBroadcasterNode.cpp index 38d7342711..88f3823964 100644 --- a/carma_wm_ctrl/src/WMBroadcasterNode.cpp +++ b/carma_wm_ctrl/src/WMBroadcasterNode.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,121 +14,197 @@ * the License. */ -#include -#include -#include +#include +#include namespace carma_wm_ctrl { // @SONAR_STOP@ -using std::placeholders::_1; +namespace std_ph = std::placeholders; -void WMBroadcasterNode::publishMap(const autoware_lanelet2_msgs::MapBin& map_msg) + +void WMBroadcasterNode::publishMap(const autoware_lanelet2_msgs::msg::MapBin& map_msg) { - map_pub_.publish(map_msg); + map_pub_->publish(map_msg); } -void WMBroadcasterNode::publishMapUpdate(const autoware_lanelet2_msgs::MapBin& geofence_msg) const +void WMBroadcasterNode::publishMapUpdate(const autoware_lanelet2_msgs::msg::MapBin& geofence_msg) const { - map_update_pub_.publish(geofence_msg); + map_update_pub_->publish(geofence_msg); } - -void WMBroadcasterNode::publishCtrlReq(const cav_msgs::TrafficControlRequest& ctrlreq_msg) const +void WMBroadcasterNode::publishCtrlReq(const carma_v2x_msgs::msg::TrafficControlRequest& ctrlreq_msg) const { - control_msg_pub_.publish(ctrlreq_msg); + control_msg_pub_->publish(ctrlreq_msg); } -void WMBroadcasterNode::publishActiveGeofence(const cav_msgs::CheckActiveGeofence& active_geof_msg) +void WMBroadcasterNode::publishActiveGeofence(const carma_perception_msgs::msg::CheckActiveGeofence& active_geof_msg) { - active_pub_.publish(active_geof_msg); + active_pub_->publish(active_geof_msg); } -void WMBroadcasterNode::publishTCMACK(const cav_msgs::MobilityOperation& mom_msg) +void WMBroadcasterNode::publishTCMACK(const carma_v2x_msgs::msg::MobilityOperation& mom_msg) { - tcm_ack_pub_.publish(mom_msg); + tcm_ack_pub_->publish(mom_msg); } -WMBroadcasterNode::WMBroadcasterNode() - : wmb_(std::bind(&WMBroadcasterNode::publishMap, this, _1), std::bind(&WMBroadcasterNode::publishMapUpdate, this, _1), - std::bind(&WMBroadcasterNode::publishCtrlReq, this, _1), std::bind(&WMBroadcasterNode::publishActiveGeofence, this, _1), - std::make_unique(), std::bind(&WMBroadcasterNode::publishTCMACK, this, _1)){}; +WMBroadcasterNode::WMBroadcasterNode(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) +{ + // Create initial config + config_ = Config(); + + config_.ack_pub_times = declare_parameter("ack_pub_times", config_.ack_pub_times); + config_.max_lane_width = declare_parameter("max_lane_width", config_.max_lane_width); + config_.traffic_control_request_period = declare_parameter("traffic_control_request_period", config_.traffic_control_request_period); + config_.vehicle_id = declare_parameter("vehicle_id", config_.vehicle_id); + config_.participant = declare_parameter("vehicle_participant_type", config_.participant); + config_.participant = declare_parameter("config_speed_limit", config_.config_limit); + + declare_parameter("intersection_ids_for_correction"); + declare_parameter("intersection_coord_correction"); +}; + +void WMBroadcasterNode::initializeWorker(std::weak_ptr weak_node_pointer) +{ + wmb_ = std::make_unique(std::bind(&WMBroadcasterNode::publishMap, this, std_ph::_1), std::bind(&WMBroadcasterNode::publishMapUpdate, this, std_ph::_1), + std::bind(&WMBroadcasterNode::publishCtrlReq, this, std_ph::_1), std::bind(&WMBroadcasterNode::publishActiveGeofence, this, std_ph::_1), + std::make_unique(weak_node_pointer), + std::bind(&WMBroadcasterNode::publishTCMACK, this, std_ph::_1)); +} -int WMBroadcasterNode::run() +carma_ros2_utils::CallbackReturn WMBroadcasterNode::handle_on_configure(const rclcpp_lifecycle::State &) { - int ack_pub_times = 1; - pnh_.getParam("ack_pub_times", ack_pub_times); - wmb_.setConfigACKPubTimes(ack_pub_times); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Starting configuration!"); + + // Reset config + config_ = Config(); + + initializeWorker(shared_from_this()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Done initializing worker!"); + + get_parameter("ack_pub_times", config_.ack_pub_times); + get_parameter("max_lane_width", config_.max_lane_width); + get_parameter("traffic_control_request_period", config_.traffic_control_request_period); + get_parameter("vehicle_id", config_.vehicle_id); + get_parameter("vehicle_participant_type", config_.participant); + get_parameter("config_speed_limit", config_.config_limit); + + wmb_->setConfigACKPubTimes(config_.ack_pub_times); + wmb_->setMaxLaneWidth(config_.max_lane_width); + wmb_->setConfigSpeedLimit(config_.config_limit); + wmb_->setConfigVehicleId(config_.vehicle_id); + wmb_->setVehicleParticipationType(config_.participant); + + rclcpp::Parameter intersection_coord_correction_param = get_parameter("intersection_coord_correction"); + config_.intersection_coord_correction = intersection_coord_correction_param.as_double_array(); + + rclcpp::Parameter intersection_ids_for_correction_param = get_parameter("intersection_ids_for_correction"); + config_.intersection_ids_for_correction = intersection_ids_for_correction_param.as_integer_array(); + + wmb_->setIntersectionCoordCorrection(config_.intersection_ids_for_correction, config_.intersection_coord_correction); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_mw_ctrl"),"Done loading parameters: " << config_); + + ///////////// + // PUBLISHERS + ///////////// + + // NOTE: Currently, intra-process comms must be disabled for the following two publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic + pub_qos_transient_local.transient_local(); // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers + // NOTE: The subscriber's QoS must be set to transient_local() as well for earlier messages to be resent to the later-joiner. - // Map Publisher - map_pub_ = cnh_.advertise("semantic_map", 1, true); // Map Update Publisher - // When a new node connects to this topic that node should be provided with all previous updates for the current map version - map_update_pub_ = cnh_.advertise("map_update", 200, [this](auto& pub){ wmb_.newUpdateSubscriber(pub); }); + map_update_pub_ = create_publisher("map_update", pub_qos_transient_local, intra_proc_disabled); + + // Map Publisher + map_pub_ = create_publisher("semantic_map", pub_qos_transient_local, intra_proc_disabled); + //Route Message Publisher - control_msg_pub_= cnh_.advertise("outgoing_geofence_request", 1, true); + control_msg_pub_= create_publisher("outgoing_geofence_request", 1); + //Check Active Geofence Publisher - active_pub_ = cnh_.advertise("active_geofence", 200, true); + active_pub_ = create_publisher("active_geofence", 200); + //publish TCM acknowledgement after processing TCM - tcm_ack_pub_ = cnh_.advertise("outgoing_geofence_ack", 2 * ack_pub_times , true); + tcm_ack_pub_ = create_publisher("outgoing_geofence_ack", 2 * config_.ack_pub_times ); + + //TCM Visualizer pub + tcm_visualizer_pub_= create_publisher("tcm_visualizer",1); + + //TCR Visualizer pub (visualized on UI) + tcr_visualizer_pub_ = create_publisher("tcr_bounding_points",1); + + //Upcoming intersection and group id of traffic light + upcoming_intersection_ids_pub_ = create_publisher("intersection_signal_group_ids", 1); + + // Return success if everything initialized successfully + + return CallbackReturn::SUCCESS; +} + +carma_ros2_utils::CallbackReturn WMBroadcasterNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) +{ + // Timer setup + timer_ = create_timer(get_clock(), + std::chrono::milliseconds((int)(config_.traffic_control_request_period * 1000)), + std::bind(&WMBroadcasterNode::spin_callback, this)); + + ///////////// + //SUBSCRIBERS + ///////////// NOTE: subscriber declaration delayed until here so that when map is received, publisher is already activated to immediately publish back + // Base Map Sub - base_map_sub_ = cnh_.subscribe("base_map", 1, &WMBroadcaster::baseMapCallback, &wmb_); + base_map_sub_ = create_subscription("base_map", 1, std::bind(&WMBroadcaster::baseMapCallback, wmb_.get(), std_ph::_1)); + // Base Map Georeference Sub - georef_sub_ = cnh_.subscribe("georeference", 1, &WMBroadcaster::geoReferenceCallback, &wmb_); + georef_sub_ = create_subscription("georeference", 1, std::bind(&WMBroadcaster::geoReferenceCallback, wmb_.get(), std_ph::_1)); + // Geofence Sub - geofence_sub_ = cnh_.subscribe("geofence", 200, &WMBroadcaster::geofenceCallback, &wmb_); + geofence_sub_ = create_subscription("geofence", 200, std::bind(&WMBroadcaster::geofenceCallback, wmb_.get(), std_ph::_1)); + // External Map Msg Sub - incoming_map_sub_ = cnh_.subscribe("incoming_map", 20, &WMBroadcaster::externalMapMsgCallback, &wmb_); + incoming_map_sub_ = create_subscription("incoming_map", 20, std::bind(&WMBroadcaster::externalMapMsgCallback, wmb_.get(), std_ph::_1)); + //Route Message Sub - route_callmsg_sub_ = cnh_.subscribe("route", 1, &WMBroadcaster::routeCallbackMessage, &wmb_); - + route_callmsg_sub_ = create_subscription("route", 1, std::bind(&WMBroadcaster::routeCallbackMessage, wmb_.get(), std_ph::_1)); + //Current Location Sub - curr_location_sub_ = cnh_.subscribe("current_pose", 1,&WMBroadcaster::currentLocationCallback, &wmb_); - //TCM Visualizer pub - tcm_visualizer_pub_= cnh_.advertise("tcm_visualizer",1,true); - //TCR Visualizer pub (visualized on UI) - tcr_visualizer_pub_ = cnh_.advertise("tcr_bounding_points",1,true); - //Upcoming intersection and group id of traffic light - upcoming_intersection_ids_pub_ = cnh_.advertise("intersection_signal_group_ids", 1, true); - - double config_limit; - double lane_max_width; - std::string vehicle_id; - double traffic_control_request_period; - pnh_.getParam("max_lane_width", lane_max_width); - wmb_.setMaxLaneWidth(lane_max_width); - - pnh_.getParam("traffic_control_request_period", traffic_control_request_period); - - pnh2_.getParam("/config_speed_limit", config_limit); - wmb_.setConfigSpeedLimit(config_limit); - - pnh2_.getParam("/vehicle_id", vehicle_id); - wmb_.setConfigVehicleId(vehicle_id); - - std::string participant; - pnh2_.getParam("/vehicle_participant_type", participant); - wmb_.setVehicleParticipationType(participant); - - timer = cnh_.createTimer(ros::Duration(traffic_control_request_period), [this](auto){ - tcm_visualizer_pub_.publish(wmb_.tcm_marker_array_); - tcr_visualizer_pub_.publish(wmb_.tcr_polygon_); - wmb_.publishLightId(); - //updating upcoming traffic signal group id and intersection id - wmb_.updateUpcomingSGIntersectionIds(); - if (wmb_.upcoming_intersection_ids_.data.size() > 0) - upcoming_intersection_ids_pub_.publish(wmb_.upcoming_intersection_ids_); - if(wmb_.getRoute().route_path_lanelet_ids.size() > 0) - wmb_.routeCallbackMessage(wmb_.getRoute()); - }, false); - - // Spin - cnh_.spin(); - return 0; + curr_location_sub_ = create_subscription("current_pose", 1, std::bind(&WMBroadcaster::currentLocationCallback, wmb_.get(), std_ph::_1)); + + + return CallbackReturn::SUCCESS; +} + +bool WMBroadcasterNode::spin_callback() +{ + tcm_visualizer_pub_->publish(wmb_->tcm_marker_array_); + tcr_visualizer_pub_->publish(wmb_->tcr_polygon_); + wmb_->publishLightId(); + //updating upcoming traffic signal group id and intersection id + wmb_->updateUpcomingSGIntersectionIds(); + if (wmb_->upcoming_intersection_ids_.data.size() > 0) + upcoming_intersection_ids_pub_->publish(wmb_->upcoming_intersection_ids_); + if(wmb_->getRoute().route_path_lanelet_ids.size() > 0) + wmb_->routeCallbackMessage(std::make_unique(wmb_->getRoute())); + + return true; } + // @SONAR_START@ -} // namespace carma_wm_ctrl \ No newline at end of file +} // namespace carma_wm_ctrl + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(carma_wm_ctrl::WMBroadcasterNode) \ No newline at end of file diff --git a/carma_wm_ctrl/src/main.cpp b/carma_wm_ctrl/src/main.cpp index d779373d85..89b8f1ba2c 100644 --- a/carma_wm_ctrl/src/main.cpp +++ b/carma_wm_ctrl/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,16 +14,20 @@ * the License. */ -#include -#include +#include +#include + // Main execution int main(int argc, char** argv) { // Initialize node - ros::init(argc, argv, "wm_broadcaster"); - carma_wm_ctrl::WMBroadcasterNode node; + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); - // Start execution - node.run(); - return 0; + rclcpp::shutdown(); }; \ No newline at end of file diff --git a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp index 9bdbae7173..cf0aa598b7 100644 --- a/carma_wm_ctrl/test/GeofenceScheduleTest.cpp +++ b/carma_wm_ctrl/test/GeofenceScheduleTest.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,82 +14,75 @@ * the License. */ -#include -#include - -using ::testing::_; -using ::testing::A; -using ::testing::DoAll; -using ::testing::InSequence; -using ::testing::Return; -using ::testing::ReturnArg; +#include +#include namespace carma_wm_ctrl { TEST(GeofenceSchedule, scheduleStarted) { GeofenceSchedule sch; - sch.schedule_start_ = ros::Time(0); - sch.schedule_end_ = ros::Time(1); - sch.control_start_ = ros::Duration(0); - sch.control_duration_ = ros::Duration(1); - sch.control_offset_ = ros::Duration(0); - sch.control_span_ = ros::Duration(1); - sch.control_period_ = ros::Duration(2); + sch.schedule_start_ = rclcpp::Time(0); + sch.schedule_end_ = rclcpp::Time(1); + sch.control_start_ = rclcpp::Duration(0); + sch.control_duration_ = rclcpp::Duration(1); + sch.control_offset_ = rclcpp::Duration(0); + sch.control_span_ = rclcpp::Duration(1); + sch.control_period_ = rclcpp::Duration(2); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(0))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(0.9))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1.0))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1.1))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(0.9))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1.0))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1.1))); - sch.schedule_start_ = ros::Time(1579882740.000); // EST Mon Jan 24 1970 11:19:00 - sch.schedule_end_ = ros::Time(1579886340.000); // 1 hr total duration + sch.schedule_start_ = rclcpp::Time(1579882740.000); // EST Mon Jan 24 1970 11:19:00 + sch.schedule_end_ = rclcpp::Time(1579886340.000); // 1 hr total duration - ASSERT_FALSE(sch.scheduleStarted(ros::Time(1579882739.000))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1579882740.000))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1579882741.000))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1579886340.000))); - ASSERT_TRUE(sch.scheduleStarted(ros::Time(1579886341.000))); + ASSERT_FALSE(sch.scheduleStarted(rclcpp::Time(1579882739.000))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1579882740.000))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1579882741.000))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1579886340.000))); + ASSERT_TRUE(sch.scheduleStarted(rclcpp::Time(1579886341.000))); } TEST(GeofenceSchedule, getNextInterval) { // Test before start - GeofenceSchedule sch(ros::Time(1), ros::Time(6), ros::Duration(2), ros::Duration(1), ros::Duration(0), ros::Duration(1), - ros::Duration(2) // This means the next schedule is a 4 (2+2) + GeofenceSchedule sch(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 1), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), + rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test before control start - ASSERT_NEAR(2.0, sch.getNextInterval(ros::Time(0)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(0)).first); + ASSERT_NEAR(2, sch.getNextInterval(rclcpp::Time(1e9 * 0)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 0)).first); // Test after start but before control_start - ASSERT_NEAR(2.0, sch.getNextInterval(ros::Time(1.5)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(1.5)).first); + ASSERT_NEAR(2, sch.getNextInterval(rclcpp::Time(1e9 * 1.5)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 1.5)).first); // Test between first control_start and control_end - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(2.5)).second.toSec(), 0.00001); - ASSERT_TRUE(sch.getNextInterval(ros::Time(2.5)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 2.5)).second.seconds(), 0.00001); + ASSERT_TRUE(sch.getNextInterval(rclcpp::Time(1e9 * 2.5)).first); // Test after control ends - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(3.5)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(3.5)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).first); - sch = GeofenceSchedule(ros::Time(1), ros::Time(6), ros::Duration(2), ros::Duration(3), ros::Duration(0), ros::Duration(1), - ros::Duration(2) // This means the next schedule is a 4 (2+2) + sch = GeofenceSchedule(rclcpp::Time(1e9 * 1), rclcpp::Time(1e9 * 6), rclcpp::Duration(1e9 * 2), rclcpp::Duration(1e9 * 3), rclcpp::Duration(1e9 * 0), rclcpp::Duration(1e9 * 1), + rclcpp::Duration(1e9 * 2) // This means the next schedule is a 4 (2+2) ); // Test between end of first control and start of second - ASSERT_NEAR(4.0, sch.getNextInterval(ros::Time(3.5)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(3.5)).first); + ASSERT_NEAR(4, sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 3.5)).first); // Test between 2nd control start and control end - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(4.5)).second.toSec(), 0.00001); - ASSERT_TRUE(sch.getNextInterval(ros::Time(4.5)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 4.5)).second.seconds(), 0.00001); + ASSERT_TRUE(sch.getNextInterval(rclcpp::Time(1e9 * 4.5)).first); // Test after control_end - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(5.5)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(5.5)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 5.5)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 5.5)).first); // Test other day of the week - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(90000)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(90000)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 90000)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 90000)).first); // Test after schedule end - ASSERT_NEAR(0.0, sch.getNextInterval(ros::Time(7.0)).second.toSec(), 0.00001); - ASSERT_FALSE(sch.getNextInterval(ros::Time(7.0)).first); + ASSERT_NEAR(0.0, sch.getNextInterval(rclcpp::Time(1e9 * 7.0)).second.seconds(), 0.00001); + ASSERT_FALSE(sch.getNextInterval(rclcpp::Time(1e9 * 7.0)).first); } } // namespace carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp index c26c897a64..1afc2db1ce 100644 --- a/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp +++ b/carma_wm_ctrl/test/GeofenceSchedulerTest.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -14,38 +14,38 @@ * the License. */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include #include -#include -#include -#include +#include +#include +#include +#include #include #include -using ::testing::_; -using ::testing::A; -using ::testing::DoAll; -using ::testing::InSequence; -using ::testing::Return; -using ::testing::ReturnArg; -using carma_utils::timers::testing::TestTimer; -using carma_utils::timers::testing::TestTimerFactory; +using carma_ros2_utils::timers::testing::TestTimer; +using carma_ros2_utils::timers::testing::TestTimerFactory; +using namespace std::chrono_literals; namespace carma_wm_ctrl { + TEST(GeofenceScheduler, Constructor) { - GeofenceScheduler scheduler(std::make_unique()); // Create scheduler with test timers. Having this + auto timer = std::make_shared(); + + GeofenceScheduler scheduler(timer); // Create scheduler with test timers. Having this // check helps verify that the timers do not crash // on destruction + } TEST(GeofenceScheduler, addGeofence) @@ -53,26 +53,29 @@ TEST(GeofenceScheduler, addGeofence) // Test adding then evaulate if the calls to active and inactive are done correctly // Finally test cleaing the timers auto gf_ptr = std::make_shared(); - + boost::uuids::uuid first_id = boost::uuids::random_generator()(); std::size_t first_id_hashed = boost::hash()(first_id); gf_ptr->id_ = first_id; + auto timer = std::make_shared(); + + GeofenceScheduler scheduler(timer); // Create scheduler + gf_ptr->schedules.push_back( - GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(3.5), // Ends at by 5.5 - ros::Duration(0), // repetition start 0 offset, so still start at 2 - ros::Duration(1), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) - ros::Duration(2))); - ros::Time::setNow(ros::Time(0)); // Set current time - - GeofenceScheduler scheduler(std::make_unique()); // Create scheduler + GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(3.5e9), // Ends at by 5.5 + rclcpp::Duration(0), // repetition start 0 offset, so still start at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of 2 so active durations are (2-3 and 4-5) + rclcpp::Duration(2e9))); + std::atomic active_call_count(0); std::atomic inactive_call_count(0); std::atomic last_active_gf(0); std::atomic last_inactive_gf(0); + scheduler.onGeofenceActive([&](std::shared_ptr gf_ptr) { active_call_count.store(active_call_count.load() + 1); // atomic is not working for boost::uuids::uuid, so hash it @@ -85,6 +88,7 @@ TEST(GeofenceScheduler, addGeofence) last_inactive_gf.store(boost::hash()(gf_ptr->id_)); }); + ASSERT_EQ(0, active_call_count.load()); ASSERT_EQ(0, inactive_call_count.load()); ASSERT_EQ(0, last_active_gf.load()); @@ -92,47 +96,52 @@ TEST(GeofenceScheduler, addGeofence) scheduler.addGeofence(gf_ptr); - ros::Time::setNow(ros::Time(1.0)); // Set current time + + timer->setNow(rclcpp::Time(1.0e9)); // Set current time + ASSERT_EQ(0, active_call_count.load()); ASSERT_EQ(0, inactive_call_count.load()); ASSERT_EQ(0, last_active_gf.load()); ASSERT_EQ(0, last_inactive_gf.load()); - ros::Time::setNow(ros::Time(2.1)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, first_id_hashed, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10, first_id_hashed, last_active_gf)); ASSERT_EQ(1, active_call_count.load()); ASSERT_EQ(0, inactive_call_count.load()); ASSERT_EQ(0, last_inactive_gf.load()); - ros::Time::setNow(ros::Time(3.1)); // Set current time + timer->setNow(rclcpp::Time(3.1e9)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, first_id_hashed, last_inactive_gf)); + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10, first_id_hashed, last_inactive_gf)); ASSERT_EQ(1, active_call_count.load()); ASSERT_EQ(1, inactive_call_count.load()); ASSERT_EQ(first_id_hashed, last_active_gf.load()); - ros::Time::setNow(ros::Time(3.5)); // Set current time + timer->setNow(rclcpp::Time(3.5e9)); // Set current time ASSERT_EQ(1, active_call_count.load()); ASSERT_EQ(1, inactive_call_count.load()); ASSERT_EQ(first_id_hashed, last_active_gf.load()); ASSERT_EQ(first_id_hashed, last_inactive_gf.load()); - ros::Time::setNow(ros::Time(4.2)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, 2, active_call_count)); + timer->setNow(rclcpp::Time(4.2e9)); // Set current time + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, 2, active_call_count)); ASSERT_EQ(1, inactive_call_count.load()); ASSERT_EQ(first_id_hashed, last_active_gf.load()); - ros::Time::setNow(ros::Time(5.5)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, 2, inactive_call_count)); + timer->setNow(rclcpp::Time(5.5e9)); // Set current time + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, 2, inactive_call_count)); ASSERT_EQ(2, active_call_count.load()); ASSERT_EQ(first_id_hashed, last_active_gf.load()); - ros::Time::setNow(ros::Time(9.5)); // Set current time + timer->setNow(rclcpp::Time(9.5e9)); // Set current time ASSERT_EQ(2, inactive_call_count.load()); ASSERT_EQ(2, active_call_count.load()); @@ -144,13 +153,15 @@ TEST(GeofenceScheduler, addGeofence) gf_ptr->id_ = second_id; scheduler.addGeofence(gf_ptr); - ros::Time::setNow(ros::Time(11.0)); // Set current time - carma_utils::testing::waitForEqOrTimeout(3.0, 10, inactive_call_count); // Let some time pass just in case + timer->setNow(rclcpp::Time(11.0e9)); // Set current time + + carma_ros2_utils::testing::waitForEqOrTimeout(10.0, 2, inactive_call_count); // Let some time pass just in case ASSERT_EQ(2, inactive_call_count.load()); ASSERT_EQ(2, active_call_count.load()); ASSERT_EQ(first_id_hashed, last_active_gf.load()); ASSERT_EQ(first_id_hashed, last_inactive_gf.load()); + } } // namespace carma_wm_ctrl \ No newline at end of file diff --git a/carma_wm_ctrl/test/MapToolsTest.cpp b/carma_wm_ctrl/test/MapToolsTest.cpp index 58f61660aa..fab5cb6041 100644 --- a/carma_wm_ctrl/test/MapToolsTest.cpp +++ b/carma_wm_ctrl/test/MapToolsTest.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -18,29 +18,22 @@ * This file contains unit tests which can be used like scripts to convert lanelet2 map files. */ -#include -#include +#include +#include #include #include #include #include -#include -#include +#include +#include #include #include -#include +#include #include -#include "TestHelpers.h" +#include "TestHelpers.hpp" #include #include -using ::testing::_; -using ::testing::A; -using ::testing::DoAll; -using ::testing::InSequence; -using ::testing::Return; -using ::testing::ReturnArg; - namespace carma_wm_ctrl { /** diff --git a/carma_wm_ctrl/test/TCMTest.cpp b/carma_wm_ctrl/test/TCMTest.cpp index c3a455c4bf..80025349f7 100644 --- a/carma_wm_ctrl/test/TCMTest.cpp +++ b/carma_wm_ctrl/test/TCMTest.cpp @@ -15,36 +15,35 @@ */ #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 +#include #include #include -#include -#include +#include -#include "TestHelpers.h" +#include "TestHelpers.hpp" using ::testing::_; using ::testing::A; @@ -53,8 +52,8 @@ using ::testing::InSequence; using ::testing::Return; using ::testing::ReturnArg; -using carma_utils::timers::testing::TestTimer; -using carma_utils::timers::testing::TestTimerFactory; +using carma_ros2_utils::timers::testing::TestTimer; +using carma_ros2_utils::timers::testing::TestTimerFactory; namespace carma_wm_ctrl @@ -64,12 +63,12 @@ namespace carma_wm_ctrl */ TEST(WMBroadcaster, DISABLED_geofenceCallback) { // - WMBroadcaster wmb([](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [](const cav_msgs::TrafficControlRequest& control_msg_pub_) {}, - [](const cav_msgs::CheckActiveGeofence& active_pub_) {}, + WMBroadcaster wmb([](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_) {}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_) {}, std::make_unique(), - [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); // Create broadcaster with test timers. Having this check + [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Create broadcaster with test timers. Having this check // helps verify that the timers do not crash on destruction wmb.setConfigSpeedLimit(35.0); @@ -96,20 +95,20 @@ TEST(WMBroadcaster, DISABLED_geofenceCallback) FAIL() << "Input map does not contain any lanelets"; } - std_msgs::String str_msg; + std_msgs::msg::String str_msg; str_msg.data = target_frame; - ROS_WARN_STREAM("Projection: " << target_frame); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Projection: " << target_frame); wmb.geoReferenceCallback(str_msg); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::SharedPtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); wmb.baseMapCallback(map_msg_ptr); - cav_msgs::TrafficControlMessage tcm; + carma_v2x_msgs::msg::TrafficControlMessage tcm; tcm.choice = 1; tcm.tcm_v01.reqid.id = {0,0,0,0,0,0,0,0}; // Original id was {254, 16, 166, 86, 224, 213, 73, 249} changed to broadcast to support testing tcm.tcm_v01.reqseq = 0; @@ -120,11 +119,11 @@ TEST(WMBroadcaster, DISABLED_geofenceCallback) tcm.tcm_v01.updated.nsec = 0; tcm.tcm_v01.package.label = "weather"; tcm.tcm_v01.package.label_exists = true; - j2735_msgs::Id128b gid; + j2735_v2x_msgs::msg::Id128b gid; gid.id = { 245, 58, 198, 220, 227, 49, 222, 135, 14, 191, 107, 175, 134, 207, 23, 89 }; tcm.tcm_v01.package.tcids = { gid }; tcm.tcm_v01.package_exists = true; - j2735_msgs::TrafficControlVehClass a,b,c,d,e,f,g,h,i,j,k,l,m; + j2735_v2x_msgs::msg::TrafficControlVehClass a,b,c,d,e,f,g,h,i,j,k,l,m; a.vehicle_class = 4; b.vehicle_class = 5; c.vehicle_class = 6; @@ -156,7 +155,7 @@ TEST(WMBroadcaster, DISABLED_geofenceCallback) tcm.tcm_v01.geometry.refelv = -409.600006104; tcm.tcm_v01.geometry.heading = 331.299987793; - cav_msgs::PathNode aa,bb,cc; + carma_v2x_msgs::msg::PathNode aa,bb,cc; aa.x = 0.0; aa.y = 0.0; aa.width = 0.0; diff --git a/carma_wm_ctrl/test/TestHelpers.h b/carma_wm_ctrl/test/TestHelpers.hpp similarity index 99% rename from carma_wm_ctrl/test/TestHelpers.h rename to carma_wm_ctrl/test/TestHelpers.hpp index 00ad5b65af..61b2141f95 100644 --- a/carma_wm_ctrl/test/TestHelpers.h +++ b/carma_wm_ctrl/test/TestHelpers.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -15,7 +15,7 @@ * the License. */ -#include +#include #include #include #include diff --git a/carma_wm_ctrl/test/TestMain.cpp b/carma_wm_ctrl/test/TestMain.cpp index c8b1de4e68..c57216fa5e 100644 --- a/carma_wm_ctrl/test/TestMain.cpp +++ b/carma_wm_ctrl/test/TestMain.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2022 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 @@ -15,16 +15,23 @@ */ #include -#include +#include // Run all the tests -int main(int argc, char** argv) + +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) ) { - ros::console::notifyLoggerLevelsChanged(); - } - return RUN_ALL_TESTS(); -} \ No newline at end of file + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + auto ret = rcutils_logging_set_logger_level( + rclcpp::get_logger("carma_wm_ctrl").get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/carma_wm_ctrl/test/WMBroadcasterTest.cpp b/carma_wm_ctrl/test/WMBroadcasterTest.cpp index 9832e95e7f..fa77e87cdb 100644 --- a/carma_wm_ctrl/test/WMBroadcasterTest.cpp +++ b/carma_wm_ctrl/test/WMBroadcasterTest.cpp @@ -14,45 +14,41 @@ * 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 +#include +#include #include #include -#include +#include +#include -#include "TestHelpers.h" -using ::testing::_; -using ::testing::A; -using ::testing::DoAll; -using ::testing::InSequence; -using ::testing::Return; -using ::testing::ReturnArg; +#include "TestHelpers.hpp" -using carma_utils::timers::testing::TestTimer; -using carma_utils::timers::testing::TestTimerFactory; +using carma_ros2_utils::timers::testing::TestTimer; +using carma_ros2_utils::timers::testing::TestTimerFactory; namespace carma_wm_ctrl @@ -60,18 +56,15 @@ namespace carma_wm_ctrl TEST(WMBroadcaster, Constructor) { - WMBroadcaster([](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, [](const cav_msgs::CheckActiveGeofence& active_pub_){}, std::make_unique(),[](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); // Create broadcaster with test timers. Having this check helps + WMBroadcaster([](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, std::make_shared(),[](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Create broadcaster with test timers. Having this check helps // verify that the timers do not crash on destruction } TEST(WMBroadcaster, baseMapCallback) { - ros::Time::setNow(ros::Time(0)); // Set current time - - size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); @@ -79,20 +72,20 @@ TEST(WMBroadcaster, baseMapCallback) ASSERT_EQ(4, map->laneletLayer.size()); // Verify the map can be decoded base_map_call_count++; - }, [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Get and convert map to binary message auto map = carma_wm::getDisjointRouteMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); ASSERT_EQ(1, base_map_call_count); } @@ -103,7 +96,7 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasFromTransform) using namespace lanelet::units::literals; size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); @@ -111,36 +104,36 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasFromTransform) ASSERT_EQ(4, map->laneletLayer.size()); // Verify the map can be decoded base_map_call_count++; - }, [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); ////// // Get and convert map to binary message ///// auto map = carma_wm::getDisjointRouteMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); ASSERT_EQ(1, base_map_call_count); // Setting georeferences // geofence's origin (0,0) is at base_map's (10,10) std::string base_map_proj_string, geofence_proj_string; - std_msgs::String base_map_proj; + std_msgs::msg::String base_map_proj; base_map_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; geofence_proj_string = "+proj=tmerc +lat_0=39.46645851394806215 +lon_0=-76.16907903057393980 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; base_map_proj.data = base_map_proj_string; - wmb.geoReferenceCallback(base_map_proj); + wmb.geoReferenceCallback(std::make_unique(base_map_proj)); // create the geofence request - cav_msgs::TrafficControlMessageV01 gf_msg; + carma_v2x_msgs::msg::TrafficControlMessageV01 gf_msg; gf_msg.geometry.proj = geofence_proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets pt.x = -8.5; pt.y = -9.5; pt.z = 0; // straight geofence line across 2 lanelets gf_msg.geometry.nodes.push_back(pt); @@ -150,8 +143,8 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasFromTransform) auto gf_pts = wmb.getPointsInLocalFrame(gf_msg); lanelet::ConstLaneletOrAreas affected_parts = wmb.getAffectedLaneletOrAreas(gf_pts); ASSERT_EQ(affected_parts.size(), 2); - ASSERT_EQ(affected_parts[0].id(), 10002); - ASSERT_EQ(affected_parts[1].id(), 10001); + ASSERT_EQ(affected_parts[0].id(), 10001); + ASSERT_EQ(affected_parts[1].id(), 10002); // check points that are outside, on the edge, and on the point that makes up the lanelets pt.x = -11.5; pt.y = -1.5; pt.z = 0; // -20, -10 gf_msg.geometry.nodes.push_back(pt); @@ -172,49 +165,51 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasOnlyLogic) // Set the environment size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; - }, [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); ////// // Get and convert map to binary message ///// + auto map = carma_wm::getBroadcasterTestMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); - cav_msgs::TrafficControlMessageV01 gf_msg; + carma_v2x_msgs::msg::TrafficControlMessageV01 gf_msg; // Check if error are correctly being thrown EXPECT_THROW(wmb.getPointsInLocalFrame(gf_msg), lanelet::InvalidObjectStateError); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); ASSERT_EQ(1, base_map_call_count); EXPECT_THROW(wmb.getPointsInLocalFrame(gf_msg), lanelet::InvalidObjectStateError); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; gf_msg.geometry.datum = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // create the control message's relevant parts gf_msg.geometry.proj = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets pt.x = 1.75; pt.y = 0.5; pt.z = 0; gf_msg.geometry.nodes.push_back(pt); auto gf_pts = wmb.getPointsInLocalFrame(gf_msg); + lanelet::ConstLaneletOrAreas affected_parts = wmb.getAffectedLaneletOrAreas(gf_pts); ASSERT_EQ(affected_parts.size(), 0); // this is 0 because there will never be geofence with only 1 pt // if there is, it won't apply to the map as it doesn't have any direction information, @@ -225,7 +220,6 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasOnlyLogic) affected_parts = wmb.getAffectedLaneletOrAreas(gf_pts); ASSERT_EQ(affected_parts.size(), 0); // although there are two points in the same lanelet, // lanelet and the two points are not in the same direction - gf_msg.geometry.nodes.pop_back(); pt.x = 0.0; pt.y = 0.05; pt.z = 0; //1.75 0.55 gf_msg.geometry.nodes.push_back(pt); @@ -234,7 +228,6 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasOnlyLogic) ASSERT_EQ(affected_parts.size(), 1); // because two points are in one geofence, it will be recorded now gf_msg.geometry.nodes.pop_back(); gf_msg.geometry.nodes.pop_back(); - pt.x = 0.5; pt.y = 0.5; pt.z = 0; // first of series geofence points across multiple lanelets 0.5 0.5 gf_msg.geometry.nodes.push_back(pt); pt.x = 0.0; pt.y = 0.6; pt.z = 0; // adding point in the next lanelet 0.5 1.1 @@ -243,7 +236,6 @@ TEST(WMBroadcaster, getAffectedLaneletOrAreasOnlyLogic) affected_parts = wmb.getAffectedLaneletOrAreas(gf_pts); ASSERT_EQ(affected_parts.size(), 3); // although (0.5,1.1) is in another overlapping lanelet (llt_unreg) // that lanelet is disjoint/doesnt have same direction/not successor of the any lanelet - pt.x = 1.0; pt.y = 1.0; pt.z = 0; // adding further points in different lanelet narrowing down our direction 1.5 2.1 gf_msg.geometry.nodes.push_back(pt); gf_pts = wmb.getPointsInLocalFrame(gf_msg); @@ -281,37 +273,42 @@ TEST(WMBroadcaster, geofenceCallback) std::size_t curr_id_hashed = boost::hash()(curr_id); gf->id_ = curr_id; - gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(1.1), // Ends at by 3.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 2 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of 2 so active durations are (2-3) + rclcpp::Duration(2e9))); // convert to ros msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); msg_v01.params.schedule.start = gf->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; + + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf->schedules[0].control_start_; daily_schedule.duration = gf->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf->schedules[0].control_period_; - - ros::Time::setNow(ros::Time(0)); // Set current time + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // variables needed to test size_t base_map_call_count = 0; std::atomic active_call_count(0); std::atomic last_active_gf(0); - + auto timer = std::make_shared(); WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); @@ -319,7 +316,7 @@ TEST(WMBroadcaster, geofenceCallback) ASSERT_EQ(4, map->laneletLayer.size()); // Verify the map can be decoded base_map_call_count++; }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); ASSERT_EQ(data_received->id_, curr_id); @@ -328,76 +325,80 @@ TEST(WMBroadcaster, geofenceCallback) active_call_count.store(active_call_count.load() + 1); // atomic is not working for boost::uuids::uuid, so hash it last_active_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); + + // Verify that nothing is happening when reserved // Get and convert map to binary message auto map = carma_wm::getDisjointRouteMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); ASSERT_EQ(1, base_map_call_count); // Setting georeferences - // geofence's origin (0,0) is at base_map's (10,10) std::string base_map_proj_string, geofence_proj_string; - std_msgs::String base_map_proj; + std_msgs::msg::String base_map_proj; base_map_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; - geofence_proj_string = "+proj=tmerc +lat_0=39.46645851394806215 +lon_0=-76.16907903057393980 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + geofence_proj_string = "+proj=tmerc +lat_0=39.46645851394806215 +lon_0=-76.16907903057393980 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; //different proj base_map_proj.data = base_map_proj_string; - wmb.geoReferenceCallback(base_map_proj); + wmb.geoReferenceCallback(std::make_unique(base_map_proj)); - cav_msgs::TrafficControlMessage gf_msg; - gf_msg.tcm_v01.geometry.datum = "+proj=tmerc +lat_0=39.46645851394806215 +lon_0=-76.16907903057393980 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; - gf_msg.choice = cav_msgs::TrafficControlMessage::RESERVED; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; + gf_msg.tcm_v01.geometry.datum = geofence_proj_string; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::RESERVED; ASSERT_EQ(0, active_call_count.load()); ASSERT_EQ(0, last_active_gf.load()); - wmb.geofenceCallback(gf_msg); - - ros::Time::setNow(ros::Time(1.0)); // Set current time - + timer->setNow(rclcpp::Time(1.0e9)); // Set current time + wmb.geofenceCallback(std::make_unique(gf_msg)); + ASSERT_EQ(0, active_call_count.load()); ASSERT_EQ(0, last_active_gf.load()); - // Verify that nothing is happening when reserved - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1));// Geofence should have started by now + + + + timer->setNow(rclcpp::Time(2.1e9));// Geofence should have started by now + wmb.geofenceCallback(std::make_unique(gf_msg)); ASSERT_EQ(0, active_call_count.load()); ASSERT_EQ(0, last_active_gf.load()); - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; // create the geofence request msg_v01.geometry.proj = geofence_proj_string; msg_v01.geometry.datum = geofence_proj_string; gf_msg.tcm_v01 = msg_v01; // every control message needs associated control request id - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(10000); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); gf_msg.tcm_v01.reqid = *req_id; // check geofence with no applicable points - ros::Time::setNow(ros::Time(0)); - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time - ASSERT_FALSE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); - ASSERT_EQ(0, active_call_count.load()); + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(100.0e9)); // set it past control_end so that it does't trigger again + ASSERT_FALSE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + ASSERT_EQ(0, active_call_count.load()); + // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets pt.x = -8.5; pt.y = -9.5; pt.z = 0; // straight geofence line across 2 lanelets msg_v01.geometry.nodes.push_back(pt); - pt.x = -8.5; pt.y = -8.5; pt.z = 0; + pt.x = 0.0; pt.y = 1.0; pt.z = 0; // offset from last point msg_v01.geometry.nodes.push_back(pt); // update id to continue testing curr_id = boost::uuids::random_generator()(); @@ -406,12 +407,11 @@ TEST(WMBroadcaster, geofenceCallback) msg_v01.reqid = *req_id; gf_msg.tcm_v01 = msg_v01; - ros::Time::setNow(ros::Time(0)); - // check how many times map_update is called so far - // calling again with same id should not have an effect - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); // set active + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(100.0e9)); // set it past control_end so that it does't trigger again + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(1, active_call_count.load()); // update id to continue testing @@ -419,27 +419,28 @@ TEST(WMBroadcaster, geofenceCallback) curr_id_hashed = boost::hash()(curr_id); std::copy(curr_id.begin(), curr_id.end(), msg_v01.id.id.begin()); gf_msg.tcm_v01 = msg_v01; - wmb.geofenceCallback(gf_msg); - + timer->setNow(rclcpp::Time(2.1e9)); // set active + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(100.0e9)); // set it past control_end so that it does't trigger again + // check if different geofence id is working - ros::Time::setNow(ros::Time(3.0)); // right before finishing at 3.1 - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + timer->setNow(rclcpp::Time(3.0e9)); // right before finishing at 3.1 + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(2, active_call_count.load()); - } TEST(WMBroadcaster, routeCallbackMessage) { - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(1346); route_msg.route_path_lanelet_ids.push_back(1349); - ROS_INFO_STREAM("This is a test: "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "This is a test: "); size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); @@ -447,14 +448,14 @@ TEST(WMBroadcaster, routeCallbackMessage) ASSERT_EQ(2, map->laneletLayer.size()); // Verify the map can be decoded base_map_call_count++; - }, [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); //Test throw exceptions - ASSERT_THROW(wmb.routeCallbackMessage(route_msg), lanelet::InvalidObjectStateError); - ROS_INFO_STREAM("Throw Exceptions Test Passed."); + ASSERT_THROW(wmb.routeCallbackMessage(std::make_unique(route_msg)), lanelet::InvalidObjectStateError); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Throw Exceptions Test Passed."); // Load vector map from a file start std::string file = "resource/test_vector_map1.osm"; @@ -473,22 +474,22 @@ TEST(WMBroadcaster, routeCallbackMessage) FAIL() << "Input map does not contain any lanelets"; } // apply loaded map to WMBroadcaster - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); ASSERT_EQ(1, base_map_call_count); //Test target_frame value - std_msgs::String target; + std_msgs::msg::String target; target.data = target_frame; - wmb.geoReferenceCallback(target); + wmb.geoReferenceCallback(std::make_unique(target)); ASSERT_FALSE(target_frame.empty()); // loading end - cav_msgs::TrafficControlRequest coRe; + carma_v2x_msgs::msg::TrafficControlRequest coRe; ///// Test without user defined route callback coRe = wmb.controlRequestFromRoute(route_msg); @@ -504,18 +505,18 @@ TEST(WMBroadcaster, addAndRemoveGeofence) size_t base_map_call_count = 0; size_t map_update_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map update callback map_update_call_count++; - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); ////// // Set up the map (add relevant regulatory elements) @@ -536,23 +537,23 @@ TEST(WMBroadcaster, addAndRemoveGeofence) ASSERT_EQ(map->laneletLayer.find(10000)->regulatoryElements().front()->id(), old_speed_limit->id());//should be 10045 old speed limit's id - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // Create the geofence object auto gf_ptr = std::make_shared(carma_wm_ctrl::Geofence()); gf_ptr->id_ = boost::uuids::random_generator()(); - cav_msgs::TrafficControlMessageV01 gf_msg; + carma_v2x_msgs::msg::TrafficControlMessageV01 gf_msg; gf_msg.geometry.datum = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; lanelet::DigitalSpeedLimitPtr new_speed_limit = std::make_shared(lanelet::DigitalSpeedLimit::buildData(map->regulatoryElementLayer.uniqueId(), 10_mph, {}, {}, { lanelet::Participants::VehicleCar })); @@ -560,7 +561,7 @@ TEST(WMBroadcaster, addAndRemoveGeofence) // create the control message's relevant parts to fill the object gf_msg.geometry.proj = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets gf_msg.geometry.nodes = {}; pt.x = 0.5; pt.y = 0.5; pt.z = 0; @@ -569,19 +570,20 @@ TEST(WMBroadcaster, addAndRemoveGeofence) gf_msg.geometry.nodes.push_back(pt); gf_ptr->gf_pts = wmb.getPointsInLocalFrame(gf_msg); gf_ptr->affected_parts_ = wmb.getAffectedLaneletOrAreas(gf_ptr->gf_pts); + gf_ptr->msg_ = gf_msg; - ASSERT_EQ(gf_ptr->affected_parts_.size(), 2); - ASSERT_EQ(gf_ptr->affected_parts_[1].id(), 10000); - ASSERT_EQ(gf_ptr->affected_parts_[1].regulatoryElements()[0]->id(), old_speed_limit->id()); // old speed limit - ASSERT_EQ(gf_ptr->affected_parts_[1].regulatoryElements().size(), 4); // old speed limit and other map conforming regulations + ASSERT_EQ(gf_ptr->affected_parts_.size(), 3); + ASSERT_EQ(gf_ptr->affected_parts_[0].id(), 10000); + ASSERT_EQ(gf_ptr->affected_parts_[0].regulatoryElements()[0]->id(), old_speed_limit->id()); // old speed limit + ASSERT_EQ(gf_ptr->affected_parts_[0].regulatoryElements().size(), 4); // old speed limit and other map conforming regulations // process the geofence and change the map wmb.addGeofence(gf_ptr); ASSERT_EQ(map_update_call_count, 1); /*Analyze prev_regems_.size()*/ - ASSERT_EQ(gf_ptr->prev_regems_.size(), 2); - ASSERT_EQ(gf_ptr->prev_regems_[0].first, 10007); - ASSERT_EQ(gf_ptr->prev_regems_[1].second->id(), old_speed_limit->id()); + ASSERT_EQ(gf_ptr->prev_regems_.size(), 3); + ASSERT_EQ(gf_ptr->prev_regems_[0].first, 10000); + ASSERT_EQ(gf_ptr->prev_regems_[0].second->id(), old_speed_limit->id()); // now suppose the geofence is finished being used, we have to revert the changes wmb.removeGeofence(gf_ptr); @@ -591,9 +593,9 @@ TEST(WMBroadcaster, addAndRemoveGeofence) // we can check if the removeGeofence worked, by using addGeofence again and if the original is there again wmb.addGeofence(gf_ptr); ASSERT_EQ(map_update_call_count, 3); - ASSERT_EQ(gf_ptr->prev_regems_.size(), 2); - ASSERT_EQ(gf_ptr->prev_regems_[1].first, 10000); - ASSERT_EQ(gf_ptr->prev_regems_[1].second->id(), old_speed_limit->id()); + ASSERT_EQ(gf_ptr->prev_regems_.size(), 3); + ASSERT_EQ(gf_ptr->prev_regems_[0].first, 10000); + ASSERT_EQ(gf_ptr->prev_regems_[0].second->id(), old_speed_limit->id()); } @@ -603,15 +605,15 @@ TEST(WMBroadcaster, GeofenceBinMsgTest) // Set the environment size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); ///// // Set up the map (add relevant regulatory elements) @@ -631,22 +633,22 @@ TEST(WMBroadcaster, GeofenceBinMsgTest) ASSERT_EQ(map->laneletLayer.findUsages(old_speed_limit).size(), 1); ASSERT_EQ(map->laneletLayer.find(10000)->regulatoryElements().front()->id(), old_speed_limit->id());//should be 10045 old speed limit's id - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // Create the geofence object auto gf_ptr = std::make_shared(carma_wm_ctrl::Geofence()); gf_ptr->id_ = boost::uuids::random_generator()(); - cav_msgs::TrafficControlMessageV01 gf_msg; + carma_v2x_msgs::msg::TrafficControlMessageV01 gf_msg; gf_msg.geometry.datum = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; lanelet::DigitalSpeedLimitPtr new_speed_limit = std::make_shared(lanelet::DigitalSpeedLimit::buildData(map->regulatoryElementLayer.uniqueId(), 10_mph, {}, {}, { lanelet::Participants::VehicleCar })); @@ -654,26 +656,27 @@ TEST(WMBroadcaster, GeofenceBinMsgTest) // create the control message's relevant parts to fill the object gf_msg.geometry.proj = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets gf_msg.geometry.nodes = {}; pt.x = 0.5; pt.y = 0.5; pt.z = 0; gf_msg.geometry.nodes.push_back(pt); - pt.x = 0.5; pt.y = 1.5; pt.z = 0; + pt.x = 0.0; pt.y = 1.0; pt.z = 0; gf_msg.geometry.nodes.push_back(pt); gf_ptr->gf_pts = wmb.getPointsInLocalFrame(gf_msg); gf_ptr->affected_parts_ = wmb.getAffectedLaneletOrAreas(gf_ptr->gf_pts); + gf_ptr->msg_ = gf_msg; - ASSERT_EQ(gf_ptr->affected_parts_.size(), 2); - ASSERT_EQ(gf_ptr->affected_parts_[1].id(), 10000); - ASSERT_EQ(gf_ptr->affected_parts_[1].regulatoryElements()[0]->id(), old_speed_limit->id()); // old speed limit - ASSERT_EQ(gf_ptr->affected_parts_[1].regulatoryElements().size(), 4); // old speed limit and other map conforming regulations + ASSERT_EQ(gf_ptr->affected_parts_.size(), 3); + ASSERT_EQ(gf_ptr->affected_parts_[0].id(), 10000); + ASSERT_EQ(gf_ptr->affected_parts_[0].regulatoryElements()[0]->id(), old_speed_limit->id()); // old speed limit + ASSERT_EQ(gf_ptr->affected_parts_[0].regulatoryElements().size(), 4); // old speed limit and other map conforming regulations // process the geofence and change the map // flow for adding geofence to the map wmb.addGeofence(gf_ptr); // from broadcaster - autoware_lanelet2_msgs::MapBin gf_obj_msg; + autoware_lanelet2_msgs::msg::MapBin gf_obj_msg; auto send_data = std::make_shared(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {})); carma_wm::toBinMsg(send_data, &gf_obj_msg); @@ -681,22 +684,22 @@ TEST(WMBroadcaster, GeofenceBinMsgTest) auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(gf_obj_msg, data_received); ASSERT_EQ(data_received->id_, gf_ptr->id_); - ASSERT_EQ(gf_ptr->remove_list_.size(), 2); - ASSERT_EQ(data_received->remove_list_.size(), 2); // old_speed_limit + ASSERT_EQ(gf_ptr->remove_list_.size(), 3); + ASSERT_EQ(data_received->remove_list_.size(), 3); // old_speed_limit ASSERT_TRUE(data_received->remove_list_[0].second->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) ==0 ); - ASSERT_EQ(data_received->update_list_.size(), 2); // geofence tags 2 lanelets - ASSERT_EQ(data_received->update_list_[1].first, 10000); + ASSERT_EQ(data_received->update_list_.size(), 3); // geofence tags 2 lanelets + ASSERT_EQ(data_received->update_list_[0].first, 10000); // we can see that the gf_ptr->now would have the prev speed limit of 5_mph that affected llt 10000 - ASSERT_EQ(gf_ptr->prev_regems_.size(), 2); - ASSERT_EQ(gf_ptr->prev_regems_[1].first, 10000); - ASSERT_EQ(gf_ptr->prev_regems_[1].second->id(), old_speed_limit->id()); + ASSERT_EQ(gf_ptr->prev_regems_.size(), 3); + ASSERT_EQ(gf_ptr->prev_regems_[0].first, 10000); + ASSERT_EQ(gf_ptr->prev_regems_[0].second->id(), old_speed_limit->id()); // now suppose the geofence is finished being used, we have to revert the changes wmb.removeGeofence(gf_ptr); ASSERT_EQ(gf_ptr->prev_regems_.size(), 0); // should be reset // from broadcaster - autoware_lanelet2_msgs::MapBin gf_msg_revert; + autoware_lanelet2_msgs::msg::MapBin gf_msg_revert; auto send_data_revert = std::make_shared(carma_wm::TrafficControl(gf_ptr->id_, gf_ptr->update_list_, gf_ptr->remove_list_, {})); carma_wm::toBinMsg(send_data_revert, &gf_msg_revert); // at map users @@ -704,12 +707,12 @@ TEST(WMBroadcaster, GeofenceBinMsgTest) carma_wm::fromBinMsg(gf_msg_revert, rec_data_revert); // previously added update_list_ should be tagged for removal, vice versa - ASSERT_EQ(rec_data_revert->remove_list_.size(), 2); + ASSERT_EQ(rec_data_revert->remove_list_.size(), 3); ASSERT_EQ(rec_data_revert->remove_list_.size(), data_received->update_list_.size()); ASSERT_EQ(rec_data_revert->update_list_.size(), data_received->remove_list_.size()); - ASSERT_EQ(rec_data_revert->update_list_.size(), 2); - ASSERT_EQ(rec_data_revert->update_list_[1].first, 10000); - ASSERT_EQ(rec_data_revert->update_list_[1].second->id(), old_speed_limit->id()); + ASSERT_EQ(rec_data_revert->update_list_.size(), 3); + ASSERT_EQ(rec_data_revert->update_list_[0].first, 10000); + ASSERT_EQ(rec_data_revert->update_list_[0].second->id(), old_speed_limit->id()); } @@ -722,27 +725,28 @@ TEST(WMBroadcaster, RegulatoryPCLTest) std::size_t curr_id_hashed = boost::hash()(curr_id); gf_ptr->id_ = curr_id; - gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(1.1), // Ends at by 3.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 2 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration(2e9))); // convert to ros msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf_ptr->id_.begin(), gf_ptr->id_.end(), msg_v01.id.id.begin()); msg_v01.params.schedule.start = gf_ptr->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf_ptr->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf_ptr->schedules[0].control_start_; daily_schedule.duration = gf_ptr->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf_ptr->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf_ptr->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf_ptr->schedules[0].control_period_; - - ros::Time::setNow(ros::Time(0)); // Set current time + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // variables needed to test size_t base_map_call_count = 0; @@ -767,14 +771,15 @@ TEST(WMBroadcaster, RegulatoryPCLTest) ASSERT_FALSE(old_pcl->passableFromLeft(lanelet::Participants::VehicleCar)); ASSERT_TRUE(old_pcl->passableFromRight(lanelet::Participants::VehicleCar)); + auto timer = std::make_shared(); WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); @@ -799,46 +804,46 @@ TEST(WMBroadcaster, RegulatoryPCLTest) } if (testing_reverse_direction) { - ASSERT_FALSE(lanelet::PassingControlLine::boundPassable(map->laneletLayer.get(10005).leftBound(), control_lines, true, + ASSERT_FALSE(lanelet::PassingControlLine::boundPassable(map->laneletLayer.get(10000).leftBound(), control_lines, true, lanelet::Participants::VehicleCar)); - ASSERT_TRUE(lanelet::PassingControlLine::boundPassable(map->laneletLayer.get(10005).leftBound(), control_lines, false, + ASSERT_TRUE(lanelet::PassingControlLine::boundPassable(map->laneletLayer.get(10000).leftBound(), control_lines, false, lanelet::Participants::VehicleCar)); } active_call_count.store(active_call_count.load() + 1); // atomic is not working for boost::uuids::uuid, so hash it last_active_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // set the accessibility msg_v01.params_exists = true; msg_v01.geometry.datum = proj_string; - j2735_msgs::TrafficControlVehClass veh_class; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::PASSENGER_CAR; + j2735_v2x_msgs::msg::TrafficControlVehClass veh_class; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR; msg_v01.params.vclasses.push_back(veh_class); - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE; - msg_v01.params.detail.lataffinity = cav_msgs::TrafficControlDetail::LEFT; // applies to the left boundaries of the - msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::NONE; // not accessible from left - msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::PERMITTED; // accessible from right + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE; + msg_v01.params.detail.lataffinity = carma_v2x_msgs::msg::TrafficControlDetail::LEFT; // applies to the left boundaries of the + msg_v01.params.detail.latperm[0] = carma_v2x_msgs::msg::TrafficControlDetail::NONE; // not accessible from left + msg_v01.params.detail.latperm[1] = carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED; // accessible from right // create the control message's relevant parts to fill the object msg_v01.geometry.proj = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets, thauto gf_ptr = std::make_shared();ese correspond to id 10000, 10007 pt.x = 0.5; pt.y = 0.5; pt.z = 0; msg_v01.geometry.nodes.push_back(pt); @@ -846,30 +851,30 @@ TEST(WMBroadcaster, RegulatoryPCLTest) msg_v01.geometry.nodes.push_back(pt); // register the geofence - cav_msgs::TrafficControlMessage gf_msg; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; gf_msg.tcm_v01.geometry.datum = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; // every control message needs associated control request id - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(10000); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); msg_v01.reqid = *req_id; gf_msg.tcm_v01 = msg_v01; testing_forward_direction = true; - wmb.geofenceCallback(gf_msg); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(100.0e9)); // Set current time - ros::Time::setNow(ros::Time(2.1)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(1, active_call_count.load()); testing_forward_direction = false; testing_reverse_direction = true; - ros::Time::setNow(ros::Time(0)); // Reset time // update id to continue testing curr_id = boost::uuids::random_generator()(); curr_id_hashed = boost::hash()(curr_id); @@ -881,10 +886,11 @@ TEST(WMBroadcaster, RegulatoryPCLTest) msg_v01.geometry.nodes.push_back(pt); gf_msg.tcm_v01 = msg_v01; - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time + timer->setNow(rclcpp::Time(2.1e9)); // Set current time + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(100.0e9)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(2, active_call_count.load()); } @@ -892,18 +898,21 @@ TEST(WMBroadcaster, geofenceFromMsgTest) { using namespace lanelet::units::literals; // Start creating ROS msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; boost::uuids::uuid curr_id = boost::uuids::random_generator()(); std::copy(curr_id.begin(), curr_id.end(), msg_v01.id.id.begin()); - msg_v01.params.schedule.start = ros::Time(1); // Schedule between 1 ... - msg_v01.params.schedule.end = ros::Time(8); // and 8 - cav_msgs::DailySchedule daily_schedule; - daily_schedule.begin = ros::Duration(2); // Starts at 2 - daily_schedule.duration = ros::Duration(1.1); // Ends at by 3.1 + msg_v01.params.schedule.start = rclcpp::Time(1); // Schedule between 1 ... + msg_v01.params.schedule.end = rclcpp::Time(8); // and 8 + carma_v2x_msgs::msg::DailySchedule daily_schedule; + daily_schedule.begin = rclcpp::Duration(2); // Starts at 2 + daily_schedule.duration = rclcpp::Duration(1.1); // Ends at by 3.1 msg_v01.params.schedule.between.push_back(daily_schedule); - msg_v01.params.schedule.repeat.offset = ros::Duration(0); // 0 offset for repetition start, so still starts at 2 - msg_v01.params.schedule.repeat.span = ros::Duration(1); // Duration of 1 and interval of two so active durations are (2-3) - msg_v01.params.schedule.repeat.period = ros::Duration(2); + msg_v01.params.schedule.repeat.offset = rclcpp::Duration(0); // 0 offset for repetition start, so still starts at 2 + msg_v01.params.schedule.repeat.span = rclcpp::Duration(1); // Duration of 1 and interval of two so active durations are (2-3) + msg_v01.params.schedule.repeat.period = rclcpp::Duration(2); + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // Get map and convert map to binary message auto map = carma_wm::getBroadcasterTestMap(); @@ -911,33 +920,33 @@ TEST(WMBroadcaster, geofenceFromMsgTest) // Set a basic environment size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [](const autoware_lanelet2_msgs::MapBin& map_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // create rest of control message's relevant parts to fill the object msg_v01.geometry.proj = proj_string; msg_v01.geometry.datum = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets, these correspond to id 10000, 10007 pt.x = 0.5; pt.y = 0.5; pt.z = 0; msg_v01.geometry.nodes.push_back(pt); @@ -949,11 +958,11 @@ TEST(WMBroadcaster, geofenceFromMsgTest) auto gf_ptr = std::make_shared(); // test restricted lane (trucks and buses will not have access) - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::RESTRICTED_CHOICE; - j2735_msgs::TrafficControlVehClass restricted_veh_class1; - j2735_msgs::TrafficControlVehClass restricted_veh_class2; - restricted_veh_class1.vehicle_class = j2735_msgs::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK; - restricted_veh_class2.vehicle_class = j2735_msgs::TrafficControlVehClass::BUS; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::RESTRICTED_CHOICE; + j2735_v2x_msgs::msg::TrafficControlVehClass restricted_veh_class1; + j2735_v2x_msgs::msg::TrafficControlVehClass restricted_veh_class2; + restricted_veh_class1.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::TWO_AXLE_SIX_TIRE_SINGLE_UNIT_TRUCK; + restricted_veh_class2.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS; msg_v01.params.vclasses.push_back(restricted_veh_class1); msg_v01.params.vclasses.push_back(restricted_veh_class2); wmb.geofenceFromMsg(gf_ptr, msg_v01); @@ -965,7 +974,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) msg_v01.params.vclasses = {}; // Clear the set vclasses for msg_v01 // test maxspeed - config limit inactive - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; msg_v01.params.detail.maxspeed = 99; lanelet::Velocity limit = 80_mph; @@ -976,7 +985,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) ASSERT_NEAR(max_speed->speed_limit_.value(), limit.value(),0.0001) ;//Check that the maximum speed limit is not larger than 80_mph ASSERT_GE(max_speed->speed_limit_, 0_mph);//Check that the maximum speed limit is not smaller than 0_mph - ROS_WARN_STREAM("Maximum speed limit is valid (1)."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Maximum speed limit is valid (1)."); msg_v01.params.detail.maxspeed = -4; @@ -984,11 +993,11 @@ TEST(WMBroadcaster, geofenceFromMsgTest) ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); max_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_GE(max_speed->speed_limit_, 0_mph);//Check that the maximum speed limit is not smaller than 0_mph - ROS_WARN_STREAM("Maximum speed limit is valid(2)."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Maximum speed limit is valid(2)."); // test minspeed - config limit inactive - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MINSPEED_CHOICE; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE; msg_v01.params.detail.minspeed = -4.0; @@ -996,7 +1005,7 @@ TEST(WMBroadcaster, geofenceFromMsgTest) ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); lanelet::DigitalSpeedLimitPtr min_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_GE(min_speed->speed_limit_, 0_mph); - ROS_WARN_STREAM("Minimum speed limit is valid.(1)"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Minimum speed limit is valid.(1)"); msg_v01.params.detail.minspeed = 99.0; @@ -1007,14 +1016,14 @@ TEST(WMBroadcaster, geofenceFromMsgTest) min_speed = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_NEAR(min_speed->speed_limit_.value(), limit.value(), 0.0001) ;//Check that the minimum speed limit is not larger than 80_mph ASSERT_GE(min_speed->speed_limit_, 0_mph); - ROS_WARN_STREAM("Minimum speed limit is valid.(2)"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Minimum speed limit is valid.(2)"); // test maxspeed - config limit active wmb.setConfigSpeedLimit(55.0);//Set the config speed limit - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; - msg_v01.params.detail.maxspeed = 0; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; + msg_v01.params.detail.maxspeed = 100.0; auto gf_ptr2 = std::make_shared(); wmb.geofenceFromMsg(gf_ptr2, msg_v01); ASSERT_TRUE(gf_ptr2->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); @@ -1022,11 +1031,11 @@ TEST(WMBroadcaster, geofenceFromMsgTest) //ASSERT_NEAR(max_speed->speed_limit_.value(), 22.352, 0.00001); ASSERT_LE(max_speed_cL->speed_limit_, 80_mph);//Check that the maximum speed limit is not larger than 80_mph ASSERT_EQ(max_speed_cL->speed_limit_, 55_mph);//Check that the maximum speed limit is equal to the configured limit - ROS_WARN_STREAM("Maximum speed limit (config_limit enabled) is valid."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Maximum speed limit (config_limit enabled) is valid."); // test minspeed - config limit active wmb.setConfigSpeedLimit(55.0);//Set the config speed limit - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MINSPEED_CHOICE; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MINSPEED_CHOICE; msg_v01.params.detail.minspeed = 0; wmb.geofenceFromMsg(gf_ptr2,msg_v01); ASSERT_TRUE(gf_ptr2->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::DigitalSpeedLimit::RuleName) == 0); @@ -1035,86 +1044,86 @@ TEST(WMBroadcaster, geofenceFromMsgTest) ASSERT_LE(min_speed_cL->speed_limit_, 80_mph);//Check that the maximum speed limit is not larger than 80_mph ASSERT_GE(min_speed_cL->speed_limit_, 0_mph); ASSERT_EQ(min_speed_cL->speed_limit_, 55_mph); - ROS_WARN_STREAM("Minimum speed limit (config_limit enabled) is valid."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Minimum speed limit (config_limit enabled) is valid."); // TEST passing control line // Test lataffinity - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::LATAFFINITY_CHOICE; - msg_v01.params.detail.lataffinity = cav_msgs::TrafficControlDetail::LEFT; // applies to the left boundaries of the + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::LATAFFINITY_CHOICE; + msg_v01.params.detail.lataffinity = carma_v2x_msgs::msg::TrafficControlDetail::LEFT; // applies to the left boundaries of the wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::PassingControlLine::RuleName) == 0); ASSERT_TRUE(gf_ptr->pcl_affects_left_); - msg_v01.params.detail.lataffinity = cav_msgs::TrafficControlDetail::RIGHT; // applies to the right boundaries of the + msg_v01.params.detail.lataffinity = carma_v2x_msgs::msg::TrafficControlDetail::RIGHT; // applies to the right boundaries of the wmb.geofenceFromMsg(gf_ptr, msg_v01); ASSERT_TRUE(gf_ptr->pcl_affects_right_); - msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::NONE; // not accessible from left - msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::PERMITTED; // accessible from right + msg_v01.params.detail.latperm[0] = carma_v2x_msgs::msg::TrafficControlDetail::NONE; // not accessible from left + msg_v01.params.detail.latperm[1] = carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED; // accessible from right // Test Participants - j2735_msgs::TrafficControlVehClass veh_class; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::ANY; + j2735_v2x_msgs::msg::TrafficControlVehClass veh_class; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::ANY; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); lanelet::PassingControlLinePtr pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_EQ(pcl->right_participants_.size(), 3); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::PEDESTRIAN; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::Pedestrian) == 0); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::BICYCLE; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BICYCLE; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::Bicycle) == 0); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::MOTORCYCLE; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::MOTORCYCLE; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleMotorcycle) == 0); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::BUS; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::BUS; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleBus) == 0); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::LIGHT_TRUCK_VAN; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::LIGHT_TRUCK_VAN; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleCar) == 0); msg_v01.params.vclasses = {}; - veh_class.vehicle_class = j2735_msgs::TrafficControlVehClass::THREE_AXLE_SINGLE_UNIT_TRUCK; + veh_class.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::THREE_AXLE_SINGLE_UNIT_TRUCK; msg_v01.params.vclasses.push_back(veh_class); wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleTruck) == 0); ASSERT_EQ(pcl->left_participants_.size(), 0); - msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::PERMITTED; // accessible from left - msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::NONE; // not accessible from right + msg_v01.params.detail.latperm[0] = carma_v2x_msgs::msg::TrafficControlDetail::PERMITTED; // accessible from left + msg_v01.params.detail.latperm[1] = carma_v2x_msgs::msg::TrafficControlDetail::NONE; // not accessible from right wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->left_participants_.begin()->data(), lanelet::Participants::VehicleTruck) == 0); ASSERT_EQ(pcl->right_participants_.size(), 0); - msg_v01.params.detail.latperm[0] = cav_msgs::TrafficControlDetail::EMERGENCYONLY; - msg_v01.params.detail.latperm[1] = cav_msgs::TrafficControlDetail::EMERGENCYONLY; + msg_v01.params.detail.latperm[0] = carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY; + msg_v01.params.detail.latperm[1] = carma_v2x_msgs::msg::TrafficControlDetail::EMERGENCYONLY; wmb.geofenceFromMsg(gf_ptr, msg_v01); pcl = std::dynamic_pointer_cast(gf_ptr->regulatory_element_); ASSERT_TRUE(strcmp(pcl->right_participants_.begin()->data(), lanelet::Participants::VehicleEmergency) == 0); @@ -1131,27 +1140,28 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) std::size_t curr_id_hashed = boost::hash()(curr_id); gf->id_ = curr_id; - gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(1.1), // Ends at by 3.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 2 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration(2e9))); // convert to ros msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); msg_v01.params.schedule.start = gf->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf->schedules[0].control_start_; daily_schedule.duration = gf->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf->schedules[0].control_period_; - - ros::Time::setNow(ros::Time(0)); // Set current time + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // variables needed to test size_t base_map_call_count = 0; @@ -1160,13 +1170,15 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) std::atomic last_inactive_gf(0); bool activated = false; + auto timer = std::make_shared(); + WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); map_update_call_count.store(map_update_call_count.load() + 1); @@ -1175,47 +1187,47 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) last_active_gf.store(boost::hash()(data_received->id_)); if (!activated) last_inactive_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Get and convert map to binary message auto map = carma_wm::getDisjointRouteMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeferences // geofence's origin (0,0) is at base_map's (10,10) std::string base_map_proj_string, geofence_proj_string; - std_msgs::String base_map_proj; + std_msgs::msg::String base_map_proj; base_map_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; geofence_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; base_map_proj.data = base_map_proj_string; - wmb.geoReferenceCallback(base_map_proj); + wmb.geoReferenceCallback(std::make_unique(base_map_proj)); - cav_msgs::TrafficControlMessage gf_msg; - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; // create the geofence request msg_v01.geometry.proj = geofence_proj_string; msg_v01.geometry.datum = geofence_proj_string; // every control message needs associated control request id - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(10000); route_msg.route_path_lanelet_ids.push_back(10001); route_msg.route_path_lanelet_ids.push_back(10002); route_msg.route_path_lanelet_ids.push_back(10003); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); msg_v01.reqid = *req_id; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets pt.x = 1.5; pt.y = 1.5; pt.z = 0; // straight geofence line across 2 lanelets msg_v01.geometry.nodes.push_back(pt); @@ -1226,16 +1238,16 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) curr_id_hashed = boost::hash()(curr_id); std::copy(curr_id.begin(), curr_id.end(), msg_v01.id.id.begin()); - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; msg_v01.params.detail.maxspeed = 50; gf_msg.tcm_v01 = msg_v01; // Make sure the geofence is active now - ros::Time::setNow(ros::Time(0)); activated = true; - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time so that geofence is active - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time so that geofence is active + wmb.geofenceCallback(std::make_unique(gf_msg)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(1, map_update_call_count.load()); lanelet::BasicPoint2d curr_pos = {1.5,0.5}; @@ -1248,14 +1260,14 @@ TEST(WMBroadcaster, distToNearestActiveGeofence) curr_pos = {1.5,1.5}; // it is currently on an active geofence nearest_gf_dist = wmb.distToNearestActiveGeofence(curr_pos); - ASSERT_NEAR(nearest_gf_dist, 0.5, 0.0001); // it should point the next + ASSERT_NEAR(nearest_gf_dist, 0.0, 0.0001); // it should point the next curr_pos = {1.5,3.5}; // it is currently not on any lanelet EXPECT_THROW(wmb.distToNearestActiveGeofence(curr_pos), std::invalid_argument); activated = false; - ros::Time::setNow(ros::Time(3.2)); // Geofences deactivate now - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_inactive_gf)); + timer->setNow(rclcpp::Time(3.2e9)); // Geofences deactivate now + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_inactive_gf)); ASSERT_EQ(2, map_update_call_count.load()); curr_pos = {1.5,1.5}; // it is currently on an active geofence @@ -1271,23 +1283,23 @@ TEST(WMBroadcaster, addRegionAccessRule) std::vector affected_llts {map->laneletLayer.get(map->laneletLayer.begin()->id())}; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) {}, - [&](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [&](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); - - cav_msgs::TrafficControlMessageV01 msg_v01; - cav_msgs::TrafficControlMessageV01 msg_v02; - j2735_msgs::TrafficControlVehClass participant1,participant2; - participant1.vehicle_class = j2735_msgs::TrafficControlVehClass::PASSENGER_CAR; + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) {}, + [&](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [&](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); + + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v02; + j2735_v2x_msgs::msg::TrafficControlVehClass participant1,participant2; + participant1.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR; msg_v01.params.vclasses.push_back(participant1); wmb.addRegionAccessRule(gf_ptr,msg_v01,affected_llts); ASSERT_EQ(gf_ptr->invalidate_route_,true); - participant2.vehicle_class = j2735_msgs::TrafficControlVehClass::PEDESTRIAN; + participant2.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PEDESTRIAN; msg_v02.params.vclasses = {}; msg_v02.params.vclasses.push_back(participant2); msg_v02.package.label = "Move over law"; @@ -1310,16 +1322,16 @@ TEST(WMBroadcaster, addRegionMinimumGap) std::vector affected_llts {map->laneletLayer.get(map->laneletLayer.begin()->id())}; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) {}, - [&](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [&](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); - - cav_msgs::TrafficControlMessageV01 msg_v01; - cav_msgs::TrafficControlMessageV01 msg_v02; - j2735_msgs::TrafficControlVehClass participant1,participant2; - participant1.vehicle_class = j2735_msgs::TrafficControlVehClass::PASSENGER_CAR; + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) {}, + [&](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [&](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); + + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v02; + j2735_v2x_msgs::msg::TrafficControlVehClass participant1,participant2; + participant1.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR; msg_v01.params.vclasses.push_back(participant1); double min_gap = 12; wmb.addRegionMinimumGap(gf_ptr, msg_v01,min_gap,affected_llts, {}); @@ -1339,13 +1351,13 @@ TEST(WMBroadcaster, invertParticipants) std::vector affected_llts {map->laneletLayer.get(map->laneletLayer.begin()->id())}; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) {}, - [&](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [&](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) {}, + [&](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [&](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); - ros::V_string participants; + std::vector participants; auto result = wmb.invertParticipants(participants); ASSERT_EQ(result.size(), 6); } @@ -1360,31 +1372,29 @@ TEST(WMBroadcaster, currentLocationCallback) std::size_t curr_id_hashed = boost::hash()(curr_id); gf->id_ = curr_id; - gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(1.1), // Ends at by 3.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 2 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration(2e9))); // convert to ros msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf->id_.begin(), gf->id_.end(), msg_v01.id.id.begin()); + msg_v01.params.schedule.start = gf->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf->schedules[0].control_start_; daily_schedule.duration = gf->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf->schedules[0].control_period_; - - ros::Time::setNow(ros::Time(0)); // Set current time - - - - + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // variables needed to test std::atomic map_update_call_count(0); @@ -1393,23 +1403,24 @@ TEST(WMBroadcaster, currentLocationCallback) bool activated = false; //Create input message - geometry_msgs::PoseStamped input_msg; + geometry_msgs::msg::PoseStamped input_msg; //Input message coordinates input_msg.pose.position.x = 1.5; input_msg.pose.position.y = 1.5; input_msg.pose.position.z = 0.0; + size_t base_map_call_count = 0; + auto timer = std::make_shared(); - size_t base_map_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); map_update_call_count.store(map_update_call_count.load() + 1); @@ -1418,50 +1429,48 @@ TEST(WMBroadcaster, currentLocationCallback) last_active_gf.store(boost::hash()(data_received->id_)); if (!activated) last_inactive_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); - //Test throw exceptions - ASSERT_THROW(wmb.currentLocationCallback(input_msg), lanelet::InvalidObjectStateError); - ROS_INFO_STREAM("Throw Exceptions Test Passed."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Throw Exceptions Test Passed."); // Get and convert map to binary message auto map = carma_wm::getDisjointRouteMap(); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeferences // geofence's origin (0,0) is at base_map's (10,10) std::string base_map_proj_string, geofence_proj_string; - std_msgs::String base_map_proj; + std_msgs::msg::String base_map_proj; base_map_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; geofence_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; base_map_proj.data = base_map_proj_string; - wmb.geoReferenceCallback(base_map_proj); + wmb.geoReferenceCallback(std::make_unique(base_map_proj)); - cav_msgs::TrafficControlMessage gf_msg; - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; // create the geofence request msg_v01.geometry.proj = geofence_proj_string; msg_v01.geometry.datum = geofence_proj_string; // every control message needs associated control request id - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(10000); route_msg.route_path_lanelet_ids.push_back(10001); route_msg.route_path_lanelet_ids.push_back(10002); route_msg.route_path_lanelet_ids.push_back(10003); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); msg_v01.reqid = *req_id; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets pt.x = 1.5; pt.y = 1.5; pt.z = 0; // straight geofence line across 2 lanelets msg_v01.geometry.nodes.push_back(pt); @@ -1472,16 +1481,15 @@ TEST(WMBroadcaster, currentLocationCallback) curr_id_hashed = boost::hash()(curr_id); std::copy(curr_id.begin(), curr_id.end(), msg_v01.id.id.begin()); - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; msg_v01.params.detail.maxspeed = 50; gf_msg.tcm_v01 = msg_v01; // Make sure the geofence is active now - ros::Time::setNow(ros::Time(0)); activated = true; - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time so that geofence is active - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time so that geofence is active + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(1, map_update_call_count.load()); // wmb.addGeofence(gf); @@ -1489,14 +1497,14 @@ TEST(WMBroadcaster, currentLocationCallback) std::unordered_set active_geofence_llt_ids; // active_geofence_llt_ids.insert(gf->id_); - cav_msgs::CheckActiveGeofence check = wmb.checkActiveGeofenceLogic(input_msg); + carma_perception_msgs::msg::CheckActiveGeofence check = wmb.checkActiveGeofenceLogic(input_msg); ASSERT_GE(check.distance_to_next_geofence, 0); EXPECT_TRUE(check.type > 0); EXPECT_TRUE(check.is_on_active_geofence); activated = false; - ros::Time::setNow(ros::Time(3.2)); // Geofences deactivate now - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_inactive_gf)); + timer->setNow(rclcpp::Time(3.2e9)); // Geofences deactivate now + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_inactive_gf)); ASSERT_EQ(2, map_update_call_count.load()); } @@ -1504,25 +1512,28 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) { // Create geofence pointer auto gf = std::make_shared(); - gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(0), // Schedule between 0 and 8 - ros::Time(8), - ros::Duration(0), // Starts at 0 - ros::Duration(1.1), // Ends at 1.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 0 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration(2e9))); // Convert the geofence pointer into a TrafficControlMessageV01 message - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; msg_v01.params.schedule.start = gf->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf->schedules[0].control_start_; daily_schedule.duration = gf->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf->schedules[0].control_period_; + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // Initialize variables required for this test std::atomic map_update_call_count(0); @@ -1530,65 +1541,67 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) bool activated = false; // Create WMBroadcaster object + auto timer = std::make_shared(); + WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); map_update_call_count.store(map_update_call_count.load() + 1); // atomic is not working for boost::uuids::uuid, so hash it if (activated) last_active_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Get and convert map to binary message auto map = carma_wm::test::buildGuidanceTestMap(3.7, 25); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeferences, otherwise geofenceCallback() will throw exception // geofence's origin (0,0) is at base_map's (10,10) std::string base_map_proj_string, geofence_proj_string; - std_msgs::String base_map_proj; + std_msgs::msg::String base_map_proj; base_map_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; geofence_proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; base_map_proj.data = base_map_proj_string; - wmb.geoReferenceCallback(base_map_proj); + wmb.geoReferenceCallback(std::make_unique(base_map_proj)); - cav_msgs::TrafficControlMessage gf_msg; - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; msg_v01.geometry.proj = geofence_proj_string; msg_v01.geometry.datum = geofence_proj_string; // Obtain a control request ID for the TrafficControlMessage - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(1200); route_msg.route_path_lanelet_ids.push_back(1201); route_msg.route_path_lanelet_ids.push_back(1202); route_msg.route_path_lanelet_ids.push_back(1203); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); msg_v01.reqid = *req_id; // Set geofence 1's TrafficControlMessage points for lanelets 1200 and 1201 - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; pt.x = 1.5; pt.y = 15; pt.z = 0; // Point in lanelet 1200 msg_v01.geometry.nodes.push_back(pt); - pt.x = 1.5; pt.y = 45; pt.z = 0; // Point in lanelet 1201 + pt.x = 0.0; pt.y = 30; pt.z = 0; // Point in lanelet 1201 msg_v01.geometry.nodes.push_back(pt); // Set an advisory speed limit for geofence 1 (Lanelets 1200 and 1201) - msg_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; - msg_v01.params.detail.maxspeed = 50; + msg_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; + msg_v01.params.detail.maxspeed = 22.352; // 50 mph // Set the ID for geofence 1 boost::uuids::uuid curr_id = boost::uuids::random_generator()(); @@ -1598,7 +1611,7 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) // Create geofence 2 with a prescribed minimum gap (Lanelets 1200 and 1201) auto gf_msg2 = gf_msg; - gf_msg2.tcm_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MINHDWY_CHOICE; + gf_msg2.tcm_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE; gf_msg2.tcm_v01.params.detail.minhdwy = 5; // Set the ID for geofence 2 @@ -1608,17 +1621,17 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) // Create geofence 3 with a lane closure (Lanelets 1210 and 1211) auto gf_msg3 = gf_msg; - gf_msg3.tcm_v01.params.detail.choice = cav_msgs::TrafficControlDetail::CLOSED_CHOICE; - gf_msg3.tcm_v01.params.detail.closed = cav_msgs::TrafficControlDetail::CLOSED; + gf_msg3.tcm_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE; + gf_msg3.tcm_v01.params.detail.closed = carma_v2x_msgs::msg::TrafficControlDetail::CLOSED; gf_msg3.tcm_v01.package.label = "MOVE OVER LAW"; - j2735_msgs::TrafficControlVehClass veh_type; - veh_type.vehicle_class = j2735_msgs::TrafficControlVehClass::ANY; + j2735_v2x_msgs::msg::TrafficControlVehClass veh_type; + veh_type.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::ANY; gf_msg3.tcm_v01.params.vclasses.push_back(veh_type); // Set geofence 3's TrafficControlMessage points for lanelets 1210 and 1211 pt.x = 4.5; pt.y = 15; pt.z = 0; // Point in lanelet 1210 gf_msg3.tcm_v01.geometry.nodes[0] = pt; - pt.x = 4.5; pt.y = 45; pt.z = 0; // Point in lanelet 1211 + pt.x = 0.0; pt.y = 30; pt.z = 0; // Point in lanelet 1211 gf_msg3.tcm_v01.geometry.nodes[1] = pt; // Set the ID for geofence 3 @@ -1628,13 +1641,13 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) // Create geofence 4 with an advisory speed limit (Lanelets 1220 and 1221) auto gf_msg4 = gf_msg; - gf_msg4.tcm_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MAXSPEED_CHOICE; - gf_msg4.tcm_v01.params.detail.maxspeed = 50; + gf_msg4.tcm_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MAXSPEED_CHOICE; + gf_msg4.tcm_v01.params.detail.maxspeed = 22.352; // 50 mph // Set geofence 4's TrafficControlMessage points for lanelets 1220 and 1221 pt.x = 10.0; pt.y = 15; pt.z = 0; // Point in lanelet 1220 gf_msg4.tcm_v01.geometry.nodes[0] = pt; - pt.x = 10.0; pt.y = 45; pt.z = 0; // Point in lanelet 1221 + pt.x = 0.0; pt.y = 30; pt.z = 0; // Point in lanelet 1221 gf_msg4.tcm_v01.geometry.nodes[1] = pt; // Set the ID for geofence 4 @@ -1644,7 +1657,7 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) // Create geofence 5 with a prescribed minimum gap (Lanelets 1220 and 1221) auto gf_msg5 = gf_msg4; - gf_msg5.tcm_v01.params.detail.choice = cav_msgs::TrafficControlDetail::MINHDWY_CHOICE; + gf_msg5.tcm_v01.params.detail.choice = carma_v2x_msgs::msg::TrafficControlDetail::MINHDWY_CHOICE; gf_msg5.tcm_v01.params.detail.minhdwy = 5; // Set the ID for geofence 5 @@ -1653,49 +1666,58 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) std::copy(curr_id.begin(), curr_id.end(), gf_msg5.tcm_v01.id.id.begin()); // Make sure the geofences are active now - ros::Time::setNow(ros::Time(0.5)); + timer->setNow(rclcpp::Time(2.1e9)); activated = true; // Set callback for geofence 1 - wmb.geofenceCallback(gf_msg); - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf1, last_active_gf)); + wmb.geofenceCallback(std::make_unique(gf_msg)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf1, last_active_gf)); ASSERT_EQ(1, map_update_call_count.load()); // Set callback for geofence 2 - wmb.geofenceCallback(gf_msg2); - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf2, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); + wmb.geofenceCallback(std::make_unique(gf_msg2)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf2, last_active_gf)); ASSERT_EQ(2, map_update_call_count.load()); // Set callback for geofence 3 - wmb.geofenceCallback(gf_msg3); - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf3, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); + wmb.geofenceCallback(std::make_unique(gf_msg3)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf3, last_active_gf)); ASSERT_EQ(3, map_update_call_count.load()); // Set callback for geofence 4 - wmb.geofenceCallback(gf_msg4); - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf4, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); + wmb.geofenceCallback(std::make_unique(gf_msg4)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf4, last_active_gf)); ASSERT_EQ(4, map_update_call_count.load()); // Set callback for geofence 5 - wmb.geofenceCallback(gf_msg5); - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf5, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); + wmb.geofenceCallback(std::make_unique(gf_msg5)); + + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed_gf5, last_active_gf)); ASSERT_EQ(5, map_update_call_count.load()); // Create current vehicle pose message for Lanelet 1200 - geometry_msgs::PoseStamped current_vehicle_pose; + geometry_msgs::msg::PoseStamped current_vehicle_pose; current_vehicle_pose.pose.position.x = 1.5; current_vehicle_pose.pose.position.y = 10.5; current_vehicle_pose.pose.position.z = 0.0; // Check active geofence result for Lanelet 1200 - cav_msgs::CheckActiveGeofence check = wmb.checkActiveGeofenceLogic(current_vehicle_pose); + carma_perception_msgs::msg::CheckActiveGeofence check = wmb.checkActiveGeofenceLogic(current_vehicle_pose); ASSERT_GE(check.distance_to_next_geofence, 0); EXPECT_TRUE(check.is_on_active_geofence); ASSERT_NEAR(check.value, 22.352, 0.0001); // Advisory Speed Limit (50 mph in m/s) ASSERT_NEAR(check.advisory_speed, 22.352, 0.0001); // 50 mph in m/s ASSERT_EQ(check.minimum_gap, 5); ASSERT_EQ(check.type, 2); // Type 2 is "LANE_CLOSED" since adjacent right lane is closed - ASSERT_EQ(check.reason, "MOVE OVER LAW"); + ASSERT_EQ(check.reason, ""); // does not have reason for 1220 lanelet // Update current vehicle pose message for Lanelet 1210 current_vehicle_pose.pose.position.x = 4.5; @@ -1708,7 +1730,7 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) ASSERT_NEAR(check.advisory_speed, 35.7632, 0.00001); // Matches original map speed limit ASSERT_EQ(check.minimum_gap, 0); // Not populated ASSERT_EQ(check.type, 2); // Type 2 is "LANE_CLOSED" - ASSERT_EQ(check.reason, "MOVE OVER LAW"); + ASSERT_EQ(check.reason, "MOVE OVER LAW"); // set in gf_msg3 // Create current vehicle pose message for Lanelet 1220 current_vehicle_pose.pose.position.x = 10.0; @@ -1721,7 +1743,7 @@ TEST(WMBroadcaster, checkActiveGeofenceLogicTest) ASSERT_NEAR(check.advisory_speed, 22.352, 0.0001); // 50 mph in m/s ASSERT_EQ(check.minimum_gap, 5); ASSERT_EQ(check.type, 2); // Type 2 is "LANE_CLOSED" since adjacent left lane is closed - ASSERT_EQ(check.reason, "MOVE OVER LAW"); + ASSERT_EQ(check.reason, ""); // 1220 lanelet was not set reason as copied from gf_msg // Create current vehicle pose message for Lanelet 1203 current_vehicle_pose.pose.position.x = 1.5; @@ -1741,27 +1763,28 @@ TEST(WMBroadcaster, RegionAccessRuleTest) std::size_t curr_id_hashed = boost::hash()(curr_id); gf_ptr->id_ = curr_id; - gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(ros::Time(1), // Schedule between 1 and 8 - ros::Time(8), - ros::Duration(2), // Starts at 2 - ros::Duration(1.1), // Ends at by 3.1 - ros::Duration(0), // 0 offset for repetition start, so still starts at 2 - ros::Duration(1), // Duration of 1 and interval of two so active durations are (2-3) - ros::Duration(2))); + gf_ptr->schedules.push_back(carma_wm_ctrl::GeofenceSchedule(rclcpp::Time(1e9), // Schedule between 1 and 8 + rclcpp::Time(8e9), + rclcpp::Duration(2e9), // Starts at 2 + rclcpp::Duration(1.1e9), // Ends at by 3.1 + rclcpp::Duration(0), // 0 offset for repetition start, so still starts at 2 + rclcpp::Duration(1e9), // Duration of 1 and interval of two so active durations are (2-3) + rclcpp::Duration(2e9))); // convert to ros msg - cav_msgs::TrafficControlMessageV01 msg_v01; + carma_v2x_msgs::msg::TrafficControlMessageV01 msg_v01; std::copy(gf_ptr->id_.begin(), gf_ptr->id_.end(), msg_v01.id.id.begin()); msg_v01.params.schedule.start = gf_ptr->schedules[0].schedule_start_; msg_v01.params.schedule.end = gf_ptr->schedules[0].schedule_end_; - cav_msgs::DailySchedule daily_schedule; + carma_v2x_msgs::msg::DailySchedule daily_schedule; daily_schedule.begin = gf_ptr->schedules[0].control_start_; daily_schedule.duration = gf_ptr->schedules[0].control_duration_; msg_v01.params.schedule.between.push_back(daily_schedule); msg_v01.params.schedule.repeat.offset = gf_ptr->schedules[0].control_offset_; msg_v01.params.schedule.repeat.span = gf_ptr->schedules[0].control_span_; msg_v01.params.schedule.repeat.period = gf_ptr->schedules[0].control_period_; - - ros::Time::setNow(ros::Time(0)); // Set current time + msg_v01.params.schedule.end_exists = true; + msg_v01.params.schedule.between_exists = true; + msg_v01.params.schedule.repeat_exists = true; // variables needed to test size_t base_map_call_count = 0; @@ -1795,14 +1818,16 @@ TEST(WMBroadcaster, RegionAccessRuleTest) (map->regulatoryElementLayer.get(map->laneletLayer.find(10007)->regulatoryElements().front()->id())); ASSERT_EQ(accessRuleReg->getReason(),"Move over law"); + auto timer = std::make_shared(); + WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { auto data_received = std::make_shared(carma_wm::TrafficControl()); carma_wm::fromBinMsg(geofence_bin, data_received); @@ -1821,29 +1846,29 @@ TEST(WMBroadcaster, RegionAccessRuleTest) active_call_count.store(active_call_count.load() + 1); // atomic is not working for boost::uuids::uuid, so hash it last_active_gf.store(boost::hash()(data_received->id_)); - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + timer, [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); // set the accessibility msg_v01.geometry_exists=true; msg_v01.params_exists=true; - j2735_msgs::TrafficControlVehClass veh_type; - veh_type.vehicle_class = j2735_msgs::TrafficControlVehClass::PASSENGER_CAR; + j2735_v2x_msgs::msg::TrafficControlVehClass veh_type; + veh_type.vehicle_class = j2735_v2x_msgs::msg::TrafficControlVehClass::PASSENGER_CAR; msg_v01.params.vclasses.push_back(veh_type); - msg_v01.params.detail.choice=cav_msgs::TrafficControlDetail::CLOSED_CHOICE; - msg_v01.params.detail.closed=cav_msgs::TrafficControlDetail::CLOSED; + msg_v01.params.detail.choice=carma_v2x_msgs::msg::TrafficControlDetail::CLOSED_CHOICE; + msg_v01.params.detail.closed=carma_v2x_msgs::msg::TrafficControlDetail::CLOSED; msg_v01.params.detail.minhdwy=5; // create the control message's relevant parts to fill the object @@ -1851,7 +1876,7 @@ TEST(WMBroadcaster, RegionAccessRuleTest) msg_v01.geometry.datum = proj_string; // set the points - cav_msgs::PathNode pt; + carma_v2x_msgs::msg::PathNode pt; // check points that are inside lanelets, thauto gf_ptr = std::make_shared();ese correspond to id 10000, 10007 pt.x = 0.5; pt.y = 0.5; pt.z = 0; msg_v01.geometry.nodes.push_back(pt); @@ -1859,29 +1884,28 @@ TEST(WMBroadcaster, RegionAccessRuleTest) msg_v01.geometry.nodes.push_back(pt); // register the geofence - cav_msgs::TrafficControlMessage gf_msg; - gf_msg.choice = cav_msgs::TrafficControlMessage::TCMV01; + carma_v2x_msgs::msg::TrafficControlMessage gf_msg; + gf_msg.choice = carma_v2x_msgs::msg::TrafficControlMessage::TCMV01; // every control message needs associated control request id - cav_msgs::Route route_msg; + carma_planning_msgs::msg::Route route_msg; route_msg.route_path_lanelet_ids.push_back(10000); - std::shared_ptr req_id = std::make_shared(j2735_msgs::Id64b()); + std::shared_ptr req_id = std::make_shared(j2735_v2x_msgs::msg::Id64b()); wmb.controlRequestFromRoute(route_msg, req_id); msg_v01.reqid = *req_id; gf_msg.tcm_v01 = msg_v01; testing_forward_direction = true; - wmb.geofenceCallback(gf_msg); + wmb.geofenceCallback(std::make_unique(gf_msg)); - ros::Time::setNow(ros::Time(2.1)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(1, active_call_count.load()); testing_forward_direction = false; testing_reverse_direction = true; - ros::Time::setNow(ros::Time(0)); // Reset time // update id to continue testing curr_id = boost::uuids::random_generator()(); curr_id_hashed = boost::hash()(curr_id); @@ -1893,10 +1917,10 @@ TEST(WMBroadcaster, RegionAccessRuleTest) msg_v01.geometry.nodes.push_back(pt); gf_msg.tcm_v01 = msg_v01; - wmb.geofenceCallback(gf_msg); - ros::Time::setNow(ros::Time(2.1)); // Set current time + wmb.geofenceCallback(std::make_unique(gf_msg)); + timer->setNow(rclcpp::Time(2.1e9)); // Set current time - ASSERT_TRUE(carma_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); + ASSERT_TRUE(carma_ros2_utils::testing::waitForEqOrTimeout(10.0, curr_id_hashed, last_active_gf)); ASSERT_EQ(2, active_call_count.load()); } @@ -1904,10 +1928,10 @@ TEST(WMBroadcaster, generate32BitId) { WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) {}, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) {}, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); std::string label = "TYPE:SIG_WZ,INT_ID:0001,SG_ID:001"; auto bits = wmb.generate32BitId(label); @@ -1918,20 +1942,20 @@ TEST(WMBroadcaster, splitLaneletWithRatio) { // Create WMBroadcaster object WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Get and convert map to binary message with 26 points of 1 meter in-between distances auto map = carma_wm::test::buildGuidanceTestMap(5, 25, 25); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); - ROS_WARN_STREAM("Error messages below are expected..."); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Error messages below are expected..."); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); auto first_lanelet = map->laneletLayer.get(1200); EXPECT_THROW(wmb.splitLaneletWithRatio({}, first_lanelet, 0.5), lanelet::InvalidInputError); @@ -1994,22 +2018,22 @@ TEST(WMBroadcaster, splitLaneletWithPoint) { // Create WMBroadcaster object WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // Get and convert map to binary message with 26 points of 1 meter in-between distances auto map = carma_wm::test::buildGuidanceTestMap(5, 25, 25); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); auto first_lanelet = map->laneletLayer.get(1200); - ROS_WARN_STREAM("Error messages below are expected..."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Error messages below are expected..."); EXPECT_THROW(wmb.splitLaneletWithPoint({}, first_lanelet, 0.5), lanelet::InvalidInputError); // check front ratio TOO CLOSE (0.5 meter error for 25 meter lanelet) @@ -2047,11 +2071,11 @@ TEST(WMBroadcaster, preprocessWorkzoneGeometry) // TESTING WORLD IS IN wmb_; // Create WMBroadcaster object WMBroadcaster wmb_( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // create opposite direction road on the left lane auto map = carma_wm::test::buildGuidanceTestMap(5, 25, 25); @@ -2115,12 +2139,12 @@ TEST(WMBroadcaster, preprocessWorkzoneGeometry) * START_LINE */ - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb_.baseMapCallback(map_msg_ptr); + wmb_.baseMapCallback(std::move(map_msg_ptr)); wmb_.setErrorDistance(0.5); std::shared_ptr> parallel_llts = std::make_shared>(std::vector()); @@ -2465,11 +2489,11 @@ TEST(WMBroadcaster, createWorkzoneGeometry) // Create WMBroadcaster object WMBroadcaster wmb_( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) {}, - [&](const autoware_lanelet2_msgs::MapBin& geofence_bin) { - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) {}, + [&](const autoware_lanelet2_msgs::msg::MapBin& geofence_bin) { + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); // create opposite direction road on the left lane auto map = carma_wm::test::buildGuidanceTestMap(5, 25, 25); @@ -2533,12 +2557,12 @@ TEST(WMBroadcaster, createWorkzoneGeometry) * START_LINE */ - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Trigger basemap callback - wmb_.baseMapCallback(map_msg_ptr); + wmb_.baseMapCallback(std::move(map_msg_ptr)); wmb_.setErrorDistance(0.5); ///////////////// @@ -2651,7 +2675,7 @@ TEST(WMBroadcaster, createWorkzoneGeometry) // update the map with new lanelets (mapUpdateCallback should follow this pattern as well) for(auto llt : gf_ptr->lanelet_additions_) { - ROS_INFO_STREAM("Adding llt with id:" << llt.id()); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Adding llt with id:" << llt.id()); auto left = llt.leftBound3d(); //new lanelet coming in for (int i = 0; i < left.size(); i ++) { @@ -2720,23 +2744,22 @@ TEST(WMBroadcaster, WMBroadcaster_VehicleParticipation_Test) carma_wm::CARMAWorldModel wml; - // Set the environment size_t base_map_call_count = 0; size_t map_update_call_count = 0; WMBroadcaster wmb( - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map callback lanelet::LaneletMapPtr map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(map_bin, map); base_map_call_count++; }, - [&](const autoware_lanelet2_msgs::MapBin& map_bin) { + [&](const autoware_lanelet2_msgs::msg::MapBin& map_bin) { // Publish map update callback map_update_call_count++; - }, [](const cav_msgs::TrafficControlRequest& control_msg_pub_){}, - [](const cav_msgs::CheckActiveGeofence& active_pub_){}, - std::make_unique(), [](const cav_msgs::MobilityOperation& tcm_ack_pub_){}); + }, [](const carma_v2x_msgs::msg::TrafficControlRequest& control_msg_pub_){}, + [](const carma_perception_msgs::msg::CheckActiveGeofence& active_pub_){}, + std::make_shared(), [](const carma_v2x_msgs::msg::MobilityOperation& tcm_ack_pub_){}); /*Test that Vehicle Participation Type Value is added before baseMapCallback*/ @@ -2778,16 +2801,15 @@ ASSERT_EQ(value, p1); ASSERT_EQ(test_map_elem->speed_limit_.value(), old_speed_limit1->speed_limit_.value()); ASSERT_EQ(test_map_elem->participants_.begin()->data(), old_speed_limit1->participants_.begin()->data()); - autoware_lanelet2_msgs::MapBin msg; + autoware_lanelet2_msgs::msg::MapBin msg; lanelet::utils::conversion::toBinMsg(map, &msg); - autoware_lanelet2_msgs::MapBinConstPtr map_msg_ptr(new autoware_lanelet2_msgs::MapBin(msg)); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr(new autoware_lanelet2_msgs::msg::MapBin(msg)); // Set the map - wmb.baseMapCallback(map_msg_ptr); + wmb.baseMapCallback(std::move(map_msg_ptr)); // Setting georeference otherwise, geofenceCallback will throw exception - std_msgs::String sample_proj_string; + std_msgs::msg::String sample_proj_string; std::string proj_string = "+proj=tmerc +lat_0=39.46636844371259 +lon_0=-76.16919523566943 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; - /*ADD NEW REGELEM TO MAP WITH NEW SL and VPT*/ lanelet::DigitalSpeedLimitPtr new_speed_limit = std::make_shared(lanelet::DigitalSpeedLimit::buildData(map->regulatoryElementLayer.uniqueId(), 10_mph, {}, {}, @@ -2798,20 +2820,22 @@ ASSERT_EQ(value, p1); ASSERT_EQ(map->regulatoryElementLayer.size(), 2); ASSERT_EQ(map->laneletLayer.findUsages(new_speed_limit).size(), 1); ASSERT_EQ(map->laneletLayer.find(10000)->regulatoryElements()[1]->id(), new_speed_limit->id()); - test_map_elem = std::dynamic_pointer_cast(map->laneletLayer.find(10000)->regulatoryElements()[1]); - ASSERT_EQ(test_map_elem->speed_limit_.value(), new_speed_limit->speed_limit_.value()); ASSERT_EQ(test_map_elem->participants_.begin()->data(), new_speed_limit->participants_.begin()->data()); // Set the map - wmb.baseMapCallback(map_msg_ptr); + autoware_lanelet2_msgs::msg::MapBin msg1; + lanelet::utils::conversion::toBinMsg(map, &msg1); + autoware_lanelet2_msgs::msg::MapBin::UniquePtr map_msg_ptr1(new autoware_lanelet2_msgs::msg::MapBin(msg1)); + wmb.baseMapCallback(std::move(map_msg_ptr1)); sample_proj_string.data = proj_string; - wmb.geoReferenceCallback(sample_proj_string); + wmb.geoReferenceCallback(std::make_unique(sample_proj_string)); - ROS_INFO_STREAM("Map Vehicle Participation Type Test Complete."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ctrl"), "Map Vehicle Participation Type Test Complete."); + } diff --git a/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp b/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp index 4f30272419..d18f6b2e0b 100644 --- a/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp +++ b/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp @@ -28,7 +28,7 @@ #include #include "carma_wm_ros2/TrackPos.hpp" #include "carma_wm_ros2/WorldModelUtils.hpp" - +#include #include "boost/date_time/posix_time/posix_time.hpp" #include "carma_wm_ros2/SignalizedIntersectionManager.hpp" diff --git a/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp b/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp index e10730048c..f1a1c455c8 100644 --- a/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp +++ b/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp @@ -143,7 +143,10 @@ class SignalizedIntersectionManager * \return traffic signal corresponding to the signal group */ lanelet::Lanelets identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr& map); - + + // SignalizedIntersection's reference point correction pair of (x, y) for each intersection_id + std::unordered_map> intersection_coord_correction_; + // SignalizedIntersection quick id lookup std::unordered_map intersection_id_to_regem_id_; diff --git a/carma_wm_ros2/include/carma_wm_ros2/WMListener.hpp b/carma_wm_ros2/include/carma_wm_ros2/WMListener.hpp index d2903922a6..fb800e96fd 100644 --- a/carma_wm_ros2/include/carma_wm_ros2/WMListener.hpp +++ b/carma_wm_ros2/include/carma_wm_ros2/WMListener.hpp @@ -144,9 +144,6 @@ class WMListener const bool multi_threaded_; std::mutex mw_mutex_; - double config_speed_limit_ = 0.0; - std::string participant_ = ""; - }; } // namespace carma_wm \ No newline at end of file diff --git a/carma_wm_ros2/src/CARMAWorldModel.cpp b/carma_wm_ros2/src/CARMAWorldModel.cpp index cdc9e1be63..5d0d1544ea 100644 --- a/carma_wm_ros2/src/CARMAWorldModel.cpp +++ b/carma_wm_ros2/src/CARMAWorldModel.cpp @@ -469,8 +469,8 @@ namespace carma_wm double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis double theta = sigma + M_PI_2; // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point // to the target point - double delta_x = cos(theta) * crosstrack; - double delta_y = sin(theta) * crosstrack; + double delta_x = cos(theta) * -crosstrack; + double delta_y = sin(theta) * -crosstrack; return lanelet::BasicPoint2d(x + delta_x, y + delta_y); } @@ -503,8 +503,8 @@ namespace carma_wm double sigma = geometry::point_to_point_yaw(prior_point, next_point); // Angle between route segment and x-axis double theta = sigma + M_PI_2; // M_PI_2 is 90 deg. Theta is the angle to the vector from the route projected point // to the target point - double delta_x = cos(theta) * crosstrack; - double delta_y = sin(theta) * crosstrack; + double delta_x = cos(theta) * -crosstrack; + double delta_y = sin(theta) * -crosstrack; x += delta_x; // Adjust x and y of target point to account for crosstrack y += delta_y; @@ -549,7 +549,14 @@ namespace carma_wm RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Building routing graph"); - TrafficRulesConstPtr traffic_rules = *(getTrafficRules(participant_type_)); + auto tr = getTrafficRules(participant_type_); + + if (!tr) + { + throw std::invalid_argument("Could not construct traffic rules for participant"); + } + + TrafficRulesConstPtr traffic_rules = *tr; lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*semantic_map_, *traffic_rules); map_routing_graph_ = std::move(map_graph); diff --git a/carma_wm_ros2/src/MapUpdateLoggerNode.cpp b/carma_wm_ros2/src/MapUpdateLoggerNode.cpp index edb75c6eaf..344cad561a 100644 --- a/carma_wm_ros2/src/MapUpdateLoggerNode.cpp +++ b/carma_wm_ros2/src/MapUpdateLoggerNode.cpp @@ -79,10 +79,32 @@ class MapUpdateLogger : public rclcpp::Node MapUpdateLogger::MapUpdateLogger(const rclcpp::NodeOptions& options) : Node("map_update_logger") { - - readable_pub_ = this->create_publisher("map_update_debug", 100); - update_sub_ = this->create_subscription("/environment/map_update", 100, - std::bind(&MapUpdateLogger::raw_callback, this, std::placeholders::_1)); + // Setup publishers + + // NOTE: Currently, intra-process comms must be disabled for publishers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::PublisherOptions readable_pub_options; + readable_pub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the map update debug publisher + + auto readable_pub_qos = rclcpp::QoS(rclcpp::KeepAll()); // A publisher with this QoS will store all messages that it has sent on the topic + readable_pub_qos.transient_local(); // A publisher with this QoS will re-send all (when KeepAll is used) messages to all late-joining subscribers + // NOTE: The subscriber's QoS must be set to transisent_local() as well for earlier messages to be resent to the later-joiner. + + // Create map update debug publisher, which will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + readable_pub_ = this->create_publisher("map_update_debug", readable_pub_qos, readable_pub_options); + + // Setup subscribers + + // NOTE: Currently, intra-process comms must be disabled for subcribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + rclcpp::SubscriptionOptions update_sub_options; + update_sub_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this SubscriptionOptions object + + auto update_sub_qos = rclcpp::QoS(rclcpp::KeepLast(100)); // Set the queue size for a subscriber with this QoS + update_sub_qos.transient_local(); // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed. + // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner. + + // Create map update subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too + update_sub_ = this->create_subscription("/environment/map_update", update_sub_qos, + std::bind(&MapUpdateLogger::raw_callback, this, std::placeholders::_1), update_sub_options); } carma_debug_ros2_msgs::msg::LaneletIdRegulatoryElementPair MapUpdateLogger::pairToDebugMessage(const std::pair& id_reg_pair) { diff --git a/carma_wm_ros2/src/SignalizedIntersectionManager.cpp b/carma_wm_ros2/src/SignalizedIntersectionManager.cpp index d8fcca1e24..546edd0bf9 100644 --- a/carma_wm_ros2/src/SignalizedIntersectionManager.cpp +++ b/carma_wm_ros2/src/SignalizedIntersectionManager.cpp @@ -90,6 +90,14 @@ namespace carma_wm double curr_x = ref_node.x(); double curr_y = ref_node.y(); + if (intersection_coord_correction_.find(intersection.id.id) != intersection_coord_correction_.end()) + { + curr_x += intersection_coord_correction_[intersection.id.id].first; + curr_y += intersection_coord_correction_[intersection.id.id].second; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Applied reference point correction, delta_x: " << intersection_coord_correction_[intersection.id.id].first << + ", delta_y: " << intersection_coord_correction_[intersection.id.id].second << ", to intersection id: " << (int)lane.lane_id); + } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Processing Lane id: " << (int)lane.lane_id); size_t min_number_of_points = 2; // two points minimum are required @@ -124,7 +132,7 @@ namespace carma_wm for (auto node : node_list) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "x: " << node.x() << ", y: " << node.y()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), node.x() << ", " << node.y()); } // save which signal group connect to which exit lanes @@ -435,6 +443,7 @@ namespace carma_wm this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_; this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_; this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_; + this->intersection_coord_correction_ = other.intersection_coord_correction_; return *this; } @@ -445,5 +454,6 @@ namespace carma_wm this->signal_group_to_exit_lanelet_ids_ = other.signal_group_to_exit_lanelet_ids_; this->intersection_id_to_regem_id_ = other.intersection_id_to_regem_id_; this->signal_group_to_traffic_light_id_ = other.signal_group_to_traffic_light_id_; + this->intersection_coord_correction_ = other.intersection_coord_correction_; } } // namespace carma_wm \ No newline at end of file diff --git a/carma_wm_ros2/src/WMListener.cpp b/carma_wm_ros2/src/WMListener.cpp index a8c89bc84d..42fceefcbc 100644 --- a/carma_wm_ros2/src/WMListener.cpp +++ b/carma_wm_ros2/src/WMListener.cpp @@ -37,18 +37,24 @@ WMListener::WMListener( rclcpp::Parameter config_speed_limit_param("config_speed_limit"); if(!node_params_->get_parameter("config_speed_limit", config_speed_limit_param)){ rclcpp::ParameterValue config_speed_limit_param_value; - config_speed_limit_param_value = node_params_->declare_parameter("config_speed_limit", rclcpp::ParameterValue (config_speed_limit_)); + config_speed_limit_param_value = node_params_->declare_parameter("config_speed_limit", rclcpp::ParameterValue (0.0)); } rclcpp::Parameter participant_param("vehicle_participant_type"); if(!node_params_->get_parameter("vehicle_participant_type", participant_param)){ rclcpp::ParameterValue participant_param_value; - participant_param_value = node_params_->declare_parameter("vehicle_participant_type", rclcpp::ParameterValue(participant_)); + participant_param_value = node_params_->declare_parameter("vehicle_participant_type", rclcpp::ParameterValue("")); } // Get params - node_params_->get_parameter("config_speed_limit"); - node_params_->get_parameter("vehicle_participant_type"); + config_speed_limit_param = node_params_->get_parameter("config_speed_limit"); + participant_param = node_params_->get_parameter("vehicle_participant_type"); + + RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded config speed limit: " << config_speed_limit_param.as_double()); + RCLCPP_INFO_STREAM(node_logging->get_logger(), "Loaded vehicle participant type: " << participant_param.as_string()); + + setConfigSpeedLimit(config_speed_limit_param.as_double()); + worker_->setVehicleParticipationType(participant_param.as_string()); rclcpp::SubscriptionOptions map_update_options; rclcpp::SubscriptionOptions map_options; @@ -78,12 +84,6 @@ WMListener::WMListener( } // Setup subscribers - map_update_sub_ = rclcpp::create_subscription(node_topics_, "map_update", 10, - std::bind(&WMListener::mapUpdateCallback, this, std::placeholders::_1), map_update_options); - - map_sub_ = rclcpp::create_subscription(node_topics_, "semantic_map", 2, - std::bind(&WMListenerWorker::mapCallback, worker_.get(), std::placeholders::_1), map_options); - route_sub_ = rclcpp::create_subscription(node_topics_, "route", 1, std::bind(&WMListenerWorker::routeCallback, worker_.get(), std::placeholders::_1), route_options); @@ -92,7 +92,25 @@ WMListener::WMListener( traffic_spat_sub_ = rclcpp::create_subscription(node_topics_, "incoming_spat", 1, std::bind(&WMListenerWorker::incomingSpatCallback, worker_.get(), std::placeholders::_1), traffic_spat_options); - + + // NOTE: Currently, intra-process comms must be disabled for subscribers that are transient_local: https://github.com/ros2/rclcpp/issues/1753 + map_update_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the map update subscriber + auto map_update_sub_qos = rclcpp::QoS(rclcpp::KeepLast(10)); // Set the queue size for the map update subscriber + map_update_sub_qos.transient_local(); // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed. + // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner. + + // Create map update subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too + map_update_sub_ = rclcpp::create_subscription(node_topics_, "map_update", map_update_sub_qos, + std::bind(&WMListener::mapUpdateCallback, this, std::placeholders::_1), map_update_options); + + map_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for the semantic map subscriber + auto map_sub_qos = rclcpp::QoS(rclcpp::KeepLast(2)); // Set the queue size for the semantic map subscriber + map_sub_qos.transient_local(); // If it is possible that this node is a late-joiner to its topic, it must be set to transient_local to receive earlier messages that were missed. + // NOTE: The publisher's QoS must be set to transisent_local() as well for earlier messages to be resent to this later-joiner. + + // Create semantic mab subscriber that will receive earlier messages that were missed ONLY if the publisher is transient_local too + map_sub_ = rclcpp::create_subscription(node_topics_, "semantic_map", map_sub_qos, + std::bind(&WMListenerWorker::mapCallback, worker_.get(), std::placeholders::_1), map_options); } WMListener::~WMListener() {} diff --git a/carma_wm_ros2/src/WorldModelUtils.cpp b/carma_wm_ros2/src/WorldModelUtils.cpp index ef7cf7d0ea..7e80d87d8e 100644 --- a/carma_wm_ros2/src/WorldModelUtils.cpp +++ b/carma_wm_ros2/src/WorldModelUtils.cpp @@ -168,6 +168,26 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& std::unordered_set filtered = filterSuccessorLanelets(possible_lanelets, affected_lanelets, lanelet_map, routing_graph); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Got successor lanelets of size: " << filtered.size()); affected_lanelets.insert(filtered.begin(), filtered.end()); + + + if (affected_lanelets.empty() && !possible_lanelets.empty() && idx !=0 ) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Checking if it is the edge case where only last point falls on a valid (correct direction) lanelet"); + for (auto llt: possible_lanelets) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Evaluating lanelet: " << llt.id()); + lanelet::BasicLineString2d gf_dir_line({gf_pts[idx - 1].basicPoint2d(), gf_pts[idx].basicPoint2d()}); + lanelet::BasicLineString2d llt_boundary({(llt.leftBound2d().begin())->basicPoint2d(), (llt.rightBound2d().begin())->basicPoint2d()}); + + // record the llts that are on the same dir + if (boost::geometry::intersects(llt_boundary, gf_dir_line)) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "Overlaps starting line... Picking llt: " << llt.id()); + affected_lanelets.insert(llt); + } + } + } + break; } @@ -214,7 +234,8 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "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); + if (interior_angle < M_PI_2 && interior_angle >= 0) + affected_lanelets.insert(llt); } else { @@ -222,7 +243,6 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& } } - } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::query"), "affected_lanelets size: " << affected_lanelets.size()); @@ -231,6 +251,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& 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)); @@ -241,7 +262,7 @@ lanelet::ConstLaneletOrAreas getAffectedLaneletOrAreas(const lanelet::Points3d& { affected_parts.push_back(pair.second); } - + return affected_parts; } diff --git a/carma_wm_ros2/test/CARMAWorldModelTest.cpp b/carma_wm_ros2/test/CARMAWorldModelTest.cpp index 16ba7e5647..7c307f670a 100644 --- a/carma_wm_ros2/test/CARMAWorldModelTest.cpp +++ b/carma_wm_ros2/test/CARMAWorldModelTest.cpp @@ -1130,9 +1130,13 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) if (!point) { FAIL() << "No point returned"; } + auto alt_point = wm->routeTrackPos(*point); // Verify the output is equivalent to the inverse function lanelet = map->laneletLayer.get(1200); ASSERT_NEAR((*point).x(), lanelet.centerline().back().x(), 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); + + ASSERT_NEAR(alt_point.downtrack, pos.downtrack, 0.001); + ASSERT_NEAR(alt_point.crosstrack, pos.crosstrack, 0.001); pos.downtrack = 20.0; point = wm->pointFromRouteTrackPos(pos); // Test lanelet connection point @@ -1153,7 +1157,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().front().y(), 0.001); pos.downtrack = 40.0; @@ -1162,7 +1166,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1203); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 12.5; @@ -1171,7 +1175,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), 12.5, 0.001); pos.downtrack = 10.0; @@ -1180,7 +1184,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 20.0; @@ -1189,7 +1193,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1201); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); //////////// @@ -1202,7 +1206,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().front().y(), 0.001); pos.downtrack = 40.0; @@ -1211,7 +1215,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1203); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 12.5; @@ -1220,7 +1224,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().front().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), 12.5, 0.001); pos.downtrack = 10.0; @@ -1229,7 +1233,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1200); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); pos.downtrack = 20.0; @@ -1238,7 +1242,7 @@ TEST(CARMAWorldModelTest, pointFromRouteTrackPos) FAIL() << "No point returned"; } lanelet = map->laneletLayer.get(1201); - ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() + 1.0, 0.001); + ASSERT_NEAR((*point).x(), lanelet.centerline().back().x() - 1.0, 0.001); ASSERT_NEAR((*point).y(), lanelet.centerline().back().y(), 0.001); } diff --git a/docker/build-image.sh b/docker/build-image.sh index f281fc55b3..a4f5e4aeba 100755 --- a/docker/build-image.sh +++ b/docker/build-image.sh @@ -44,9 +44,59 @@ while [[ $# -gt 0 ]]; do COMPONENT_VERSION_STRING=develop shift ;; + --ros-1-packages|--ros1) + ROS1_PACKAGES="" + shift + ;; + --ros-2-packages|--ros2) + ROS2_PACKAGES="" + shift + ;; + *) + # Var test based on Stack Overflow question: https://stackoverflow.com/questions/5406858/difference-between-unset-and-empty-variables-in-bash + # Asker: green69 + # Answerer: geekosaur + if [ "${ROS2_PACKAGES+set}" = "set" ]; then + ROS2_PACKAGES="$ROS2_PACKAGES $arg" + elif [ "${ROS1_PACKAGES+set}" = "set" ]; then + ROS1_PACKAGES="$ROS1_PACKAGES $arg" + else + echo "Unknown argument $arg..." + exit -1 + fi + shift + ;; esac done +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Performing incremental build of image to rebuild packages: $ROS1_PACKAGES $ROS2_PACKAGES..." + + echo "Updating Dockerfile references to use most recent image as base image" + # Trim of docker image LS command sourced from + # https://stackoverflow.com/questions/50625619/why-doesnt-the-cut-command-work-for-a-docker-image-ls-command + # Question Asker: Chris F + # Question Answerer: Arount + MOST_RECENT_IMAGE_DATA=$(docker image ls | grep $IMAGE | tr -s ' ') + + if [[ -z "$MOST_RECENT_IMAGE_DATA" ]]; then + echo No prior image exists to use as base, an initial image must be built first before attempting incremental build. + exit -1 + fi + + MOST_RECENT_IMAGE_HASH=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 3) + MOST_RECENT_IMAGE_ORG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 1 | cut -d "/" -f 1) + MOST_RECENT_IMAGE_TAG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 2) + MOST_RECENT_IMAGE_DATE=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 4,5,6) + + echo Using $MOST_RECENT_IMAGE_TAG $MOST_RECENT_IMAGE_HASH $MOST_RECENT_IMAGE_DATE as base for partial build... + + sed -i "s|^FROM[[:space:]]*[^[:space:]]*|FROM $MOST_RECENT_IMAGE_HASH|I" ../Dockerfile + + COMPONENT_VERSION_STRING="SNAPSHOT" + USERNAME="local" +fi + if [[ -z "$COMPONENT_VERSION_STRING" ]]; then COMPONENT_VERSION_STRING=$("./get-component-version.sh") fi @@ -61,6 +111,13 @@ if [[ $COMPONENT_VERSION_STRING = "develop" ]]; then --build-arg VERSION="$COMPONENT_VERSION_STRING" \ --build-arg VCS_REF=`git rev-parse --short HEAD` \ --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . +elif [[ $COMPONENT_VERSION_STRING = "SNAPSHOT" ]]; then + docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \ + --build-arg ROS1_PACKAGES="$ROS1_PACKAGES" \ + --build-arg ROS2_PACKAGES="$ROS2_PACKAGES" \ + --build-arg VERSION="$COMPONENT_VERSION_STRING" \ + --build-arg VCS_REF=`git rev-parse --short HEAD` \ + --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` . else #The addition of --network=host was a fix for a DNS resolution error that occured #when running the platform inside an Ubuntu 20.04 virtual machine. The error and possible soliutions are diff --git a/docker/checkout.bash b/docker/checkout.bash index 96fcd2c271..2d5ffad43b 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -45,9 +45,9 @@ if [[ "$BRANCH" = "develop" ]]; then git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch $BRANCH git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch $BRANCH else - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.1.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.1.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch carma-system-4.1.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.2.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.2.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch carma-system-4.2.0 fi # add astuff messages diff --git a/docker/install.sh b/docker/install.sh index 64cab36bec..bc7c45ba9c 100755 --- a/docker/install.sh +++ b/docker/install.sh @@ -23,7 +23,14 @@ set -ex # ROS1 installation ### # Source the autoware installation -source /opt/autoware.ai/ros/install/setup.bash + +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Sourcing previous build for incremental build start point..." + source /opt/carma/install/setup.bash +else + echo "Sourcing base image for full build..." + source /opt/autoware.ai/ros/install/setup.bash +fi cd ~/carma_ws @@ -31,10 +38,17 @@ sudo mkdir -p /opt/carma # Create install directory sudo chown carma /opt/carma # Set owner to expose permissions for build sudo chgrp carma /opt/carma # Set group to expose permissions for build -echo "Building ROS1 CARMA Components" - -colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release - +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + if [[ ! -z "$ROS1_PACKAGES" ]]; then + echo "Incrementally building ROS1 packages: $ROS1_PACKAGES" + colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS1_PACKAGES --allow-overriding $ROS1_PACKAGES + else + echo "Build type is incremental but no ROS1 packages specified, skipping ROS1 build..." + fi +else + echo "Building all ROS1 CARMA Components" + colcon build --install-base /opt/carma/install --cmake-args -DCMAKE_BUILD_TYPE=Release +fi echo "Build of ROS1 CARMA Components Complete" ### @@ -42,12 +56,28 @@ echo "Build of ROS1 CARMA Components Complete" ### # Source the ROS2 autoware installation source /home/carma/catkin/setup.bash -source /opt/autoware.ai/ros/install_ros2/setup.bash +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + echo "Sourcing previous build for incremental build start point..." + source /opt/carma/install_ros2/setup.bash +else + echo "Sourcing base image for full build..." + source /opt/autoware.ai/ros/install_ros2/setup.bash +fi cd ~/carma_ws echo "Building ROS2 CARMA Components" -colcon build --install-base /opt/carma/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release +if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then + if [[ ! -z "$ROS2_PACKAGES" ]]; then + echo "Incrementally building ROS2 packages: $ROS2_PACKAGES" + colcon build --install-base /opt/carma/install_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-above $ROS2_PACKAGES --allow-overriding $ROS2_PACKAGES + else + echo "Build type is incremental but no ROS2 packages specified, skipping ROS2 build..." + fi +else + echo "Building all ROS2 components..." + colcon build --install-base /opt/carma/install_ros2 --build-base build_ros2 --cmake-args -DCMAKE_BUILD_TYPE=Release +fi echo "Build of ROS2 CARMA Components Complete" diff --git a/engineering_tools/mem_update_install.sh b/engineering_tools/mem_update_install.sh new file mode 100755 index 0000000000..7412835055 --- /dev/null +++ b/engineering_tools/mem_update_install.sh @@ -0,0 +1,53 @@ +#! /bin/bash + +# -------------------------------------------------------------- +# Installs the mem_update.sh script to run automatically by cron +# +# Ensure this script has execute permissions, e.g. +# chmod 755 mem_update_install.sh +# then run the script: +# ./mem_update_install.sh +# It will do the rest. Next time the computer boots, the +# rmem_max value will be set to the desired value. +# If you want to set it immediately, without a reboot, run the +# generated script: +# sudo /carma_mem_update.sh +# Results will appear in the /mem_update_log after +# each invocation. +# -------------------------------------------------------------- + +HOME=/home/dev +FILE=/tmp/carma-crontab-old +SCRIPT=$HOME/carma_mem_update.sh +LOG=$HOME/mem_update_log + +# append the old crontab with the new command +# if a carma command was already there, replace it +#must have a CR at end of previous line or cron will ignore it +sudo /usr/bin/crontab -u root -l | grep -v carma > $FILE 2> /dev/null +echo "@reboot $SCRIPT >> $LOG" >> $FILE +echo " " >> $FILE + +# replace the crontab with the new file +sudo /usr/bin/crontab -u root $FILE + +echo "Root crontab updated to run the memory update upon reboot." + +# Create the script that cron will run in user's home dir +echo "#! /bin/bash" > $SCRIPT +echo " " >> $SCRIPT +echo "# Configures memory to handle rosbridge load until Carma is completely migrated to ROS2" >> $SCRIPT +echo "# CAUTION: when removing this script, ensure that root's crontab has the line referring to it removed" >> $SCRIPT +echo "# by using sudo crontab -e" >> $SCRIPT +echo "#" >> $SCRIPT +echo "date >> $LOG" >> $SCRIPT +echo "/usr/sbin/sysctl -w net.core.rmem_max=2147483647 2>> $LOG" >> $SCRIPT +echo "/usr/sbin/sysctl -a | grep rmem_max >> $LOG" >> $SCRIPT +echo "echo '-----' >> $LOG" >> $SCRIPT +chmod 755 $SCRIPT + +# Ensure there is a log file that is writable +touch $LOG +chmod 664 $LOG + +echo "Script created as $SCRIPT for cron to run" diff --git a/lci_strategic_plugin/CMakeLists.txt b/lci_strategic_plugin/CMakeLists.txt index f76871f05e..ee179a38be 100755 --- a/lci_strategic_plugin/CMakeLists.txt +++ b/lci_strategic_plugin/CMakeLists.txt @@ -21,6 +21,7 @@ set(PKG_CATKIN_DEPS tf2 tf2_geometry_msgs std_msgs + bsm_helper ) find_package(catkin REQUIRED COMPONENTS diff --git a/lci_strategic_plugin/config/parameters.yaml b/lci_strategic_plugin/config/parameters.yaml index 80b2f1abf3..b9640b3e42 100755 --- a/lci_strategic_plugin/config/parameters.yaml +++ b/lci_strategic_plugin/config/parameters.yaml @@ -35,6 +35,20 @@ desired_distance_to_stop_buffer : 15.0 # Double: The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the whole plan min_maneuver_planning_period : 15.1 +# Double: Update time interval of carma streets. Units in s +carma_streets_update_interval : 1.0 + +# Double: Minimum vehicle reaction time. Units in s +reaction_time : 2.0 + +# Bool: If enable_carma_streets_connection is true when we want to allow carma streets functionality (UC3) and if its false that means we don't want to allow carma streets behaviour and will only use UC2 behaviour. +enable_carma_streets_connection : false + + +# Double: Mobility operation rate +mobility_rate : 10.0 + + # String: The name to use for this plugin during comminications with the arbitrator strategic_plugin_name : LCIStrategicPlugin diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h index aa511aea29..ba87152938 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h @@ -20,9 +20,12 @@ #include #include #include +#include +#include #include #include #include +#include #include #include #include @@ -42,6 +45,13 @@ namespace lci_strategic_plugin { + + enum class TurnDirection { + Straight, + Right, + Left + }; + /** * \brief Struct representing trajectory smoothing algorithm parameters using distance and acceleration * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing @@ -125,7 +135,79 @@ class LCIStrategicPlugin * \brief Lookup transfrom from front bumper to base link */ void lookupFrontBumperTransform(); +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** + * \brief callback function for mobility operation + * + * \param msg input mobility operation msg + */ + void mobilityOperationCb(const cav_msgs::MobilityOperationConstPtr& msg); + + /** + * \brief BSM callback function + */ + void BSMCb(const cav_msgs::BSMConstPtr& msg); + + /** + * \brief parse strategy parameters + * + * \param strategy_params input string of strategy params from mob op msg + */ + void parseStrategyParams(const std::string& strategy_params); + + /** + * \brief Generates Mobility Operation messages + * + * \return mobility operation msg for status and intent + */ + cav_msgs::MobilityOperation generateMobilityOperation(); + + /** + * \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); + + /** + * \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 mobilityPubSpin(); + + ros::Publisher mobility_operation_pub; + + + ////////// VARIABLES /////////// + + TurnDirection intersection_turn_direction_ = TurnDirection::Straight; + bool approaching_light_controlled_interction_ = false; + + // CARMA Streets Variakes + // timestamp for msg received from carma streets + unsigned long long street_msg_timestamp_ = 0; + // scheduled stop time + unsigned long long scheduled_stop_time_ = 0; + // scheduled enter time + unsigned long long scheduled_enter_time_ = 0; + // scheduled depart time + 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; + + //BSM + std::string bsm_id_ = "default_bsm_id"; + uint8_t bsm_msg_count_ = 0; + uint16_t bsm_sec_mark_ = 0; + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Useful metrics for LCI Plugin * \param case_num_ Current speed profile case generated @@ -643,6 +725,8 @@ class LCIStrategicPlugin TrajectoryParams get_ts_case(double t, double et, double v0, double v1, double v_max, double v_min, double a_max, double a_min, double x0, double x_end, double dx, BoundaryDistances boundary_distances, std::vector params); ////////// VARIABLES /////////// + + std::string previous_strategy_params_ = ""; double max_comfort_accel_ = 2.0; // acceleration rates after applying miltiplier double max_comfort_decel_ = -2.0; diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h index 20bfd98285..64ade7c3f0 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h @@ -65,6 +65,24 @@ struct LCIStrategicPluginConfig //! 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; + //! Double: Approximate update time interval of carma streets + double carma_streets_update_interval = 1.0; + + //! Double: Vehicle reaction time to a received schedule in seconds (approximate value, only used for communication with the schedule) + double reaction_time = 2.0; + + //! Double: Minimum inter-vehicle gap + double min_gap = 10.0; + + //! Bool: Enable carma streets connection + bool enable_carma_streets_connection = false; + + //! Double: Mobility operation rate + double mobility_rate = 10.0; + + //! License plate of the vehicle. + std::string vehicle_id = "default_id"; + //! The name to use for this plugin during comminications with the arbitrator std::string strategic_plugin_name = "LCIStrategicPlugin"; diff --git a/lci_strategic_plugin/package.xml b/lci_strategic_plugin/package.xml index e3517c4379..c6dc275368 100755 --- a/lci_strategic_plugin/package.xml +++ b/lci_strategic_plugin/package.xml @@ -33,4 +33,5 @@ tf2 tf2_geometry_msgs std_msgs + bsm_helper diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index a77bbf1b04..4c1e3e8fe8 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -559,6 +559,7 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques if (intersection_lanelet.id() != lanelet::InvalId) { intersection_speed_ = findSpeedLimit(intersection_lanelet); + intersection_turn_direction_ = getTurnDirectionAtIntersection({intersection_lanelet}); } else { @@ -596,10 +597,18 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques current_state_speed, intersection_speed_.get(), max_comfort_accel_, max_comfort_decel_); ROS_DEBUG_STREAM("earliest_entry_time: " << std::to_string(earliest_entry_time.toSec()) << ", with : " << earliest_entry_time - current_state.stamp << " left at: " << std::to_string(current_state.stamp.toSec())); + ros::Time nearest_green_entry_time; - ros::Time nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) - + ros::Duration(0.01); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - + if (config_.enable_carma_streets_connection ==false || scheduled_enter_time_ == 0 ) + { + nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) + + ros::Duration(0.01); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehcile Estimation + } + else if(config_.enable_carma_streets_connection ==true && scheduled_enter_time_ != 0 ) + { + nearest_green_entry_time = ros::Time(std::max(earliest_entry_time.toSec(), (scheduled_enter_time_)/1000.0)) + ros::Duration(0.01); //Carma Street + } + if (!nearest_green_entry_time_cached_) { ROS_DEBUG_STREAM("Applying green_light_buffer for the first time and caching! nearest_green_entry_time (without buffer):" << std::to_string(nearest_green_entry_time.toSec()) << ", and earliest_entry_time: " << std::to_string(earliest_entry_time.toSec())); @@ -851,6 +860,139 @@ void LCIStrategicPlugin::planWhenDEPARTING(const cav_srvs::PlanManeuversRequest& current_state.stamp, intersection_exit_time, crossed_lanelets.front().id(), crossed_lanelets.back().id())); } +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +void LCIStrategicPlugin::mobilityOperationCb(const cav_msgs::MobilityOperationConstPtr& msg) +{ + if (msg->strategy == light_controlled_intersection_strategy_) + { + ROS_DEBUG_STREAM("Received Schedule message with id: " << msg->m_header.plan_id); + approaching_light_controlled_interction_ = true; + ROS_DEBUG_STREAM("Approaching light Controlled Intersection: " << approaching_light_controlled_interction_); + + if (msg->m_header.recipient_id == config_.vehicle_id) + { + street_msg_timestamp_ = msg->m_header.timestamp; + ROS_DEBUG_STREAM("street_msg_timestamp_: " << street_msg_timestamp_); + parseStrategyParams(msg->strategy_params); + previous_strategy_params_ = msg->strategy_params; + } + + } + +} + +void LCIStrategicPlugin::BSMCb(const cav_msgs::BSMConstPtr& msg) +{ + 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 LCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) +{ + // 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_); + +} + +cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() +{ + cav_msgs::MobilityOperation mo_; + mo_.m_header.timestamp = ros::Time::now().toNSec()/1000000; + mo_.m_header.sender_id = config_.vehicle_id; + mo_.m_header.sender_bsm_id = bsm_id_; + mo_.strategy = light_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; + + 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"; + + + 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_; +} + +TurnDirection LCIStrategicPlugin::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; + } + else + { + // if there is no attribute, assumption is straight + turn_direction = TurnDirection::Straight; + } + + } + return turn_direction; +} + + +bool LCIStrategicPlugin::mobilityPubSpin() +{ + if (approaching_light_controlled_interction_) + { + cav_msgs::MobilityOperation status_msg = generateMobilityOperation(); + mobility_operation_pub.publish(status_msg); + } + return true; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp) { ROS_DEBUG("<<<<<<<<<<<<<<<<< STARTING LCI_STRATEGIC_PLAN!!!!!!!!! >>>>>>>>>>>>>>>>"); @@ -861,6 +1003,24 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav return true; } + if(config_.enable_carma_streets_connection ==true) + { + if (!approaching_light_controlled_interction_) + { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("Not approaching light-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("Finding car information"); // Extract vehicle data from request diff --git a/lci_strategic_plugin/src/main.cpp b/lci_strategic_plugin/src/main.cpp index b67ec844fd..3f854f1c5c 100755 --- a/lci_strategic_plugin/src/main.cpp +++ b/lci_strategic_plugin/src/main.cpp @@ -36,7 +36,8 @@ int main(int argc, char** argv) ros::Publisher tf_distance_pub = pnh.advertise("distance_remaining_to_tf", 1); ros::Publisher earliest_et_pub = pnh.advertise("earliest_entry_time", 1); ros::Publisher et_pub = pnh.advertise("scheduled_entry_time", 1); - + + // Initialize world model carma_wm::WMListener wml; @@ -62,11 +63,24 @@ int main(int argc, char** argv) pnh.param("lane_following_plugin_name", config.lane_following_plugin_name, config.lane_following_plugin_name); pnh.param("stop_and_wait_plugin_name", config.stop_and_wait_plugin_name, config.stop_and_wait_plugin_name); pnh.param("intersection_transit_plugin_name", config.intersection_transit_plugin_name, config.intersection_transit_plugin_name); + pnh.param("carma_streets_update_interval", config.carma_streets_update_interval, config.carma_streets_update_interval); + pnh.param("reaction_time", config.reaction_time, config.reaction_time); + pnh.param("enable_carma_streets_connection",config.enable_carma_streets_connection, config.enable_carma_streets_connection); + pnh.param("mobility_rate",config.mobility_rate, config.mobility_rate); + pnh.getParam("/vehicle_id", config.vehicle_id); // clang-format on // Construct plugin lci_strategic_plugin::LCIStrategicPlugin lcip(wml.getWorldModel(), config); + // Create subscribers + // Mobility Operation Subscriber + ros::Subscriber mob_operation_sub = nh.subscribe("incoming_mobility_operation", 1, &lci_strategic_plugin::LCIStrategicPlugin::mobilityOperationCb, &lcip); + ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &lci_strategic_plugin::LCIStrategicPlugin::BSMCb, &lcip); + + //Publisher + lcip.mobility_operation_pub = nh.advertise("outgoing_mobility_operation", 1); + // Setup callback connections ros::ServiceServer plan_maneuver_srv = nh.advertiseService("plugins/" + config.strategic_plugin_name + "/plan_maneuvers", &lci_strategic_plugin::LCIStrategicPlugin::planManeuverCb, &lcip); @@ -91,6 +105,8 @@ int main(int argc, char** argv) et_pub.publish(scheduled_et); }); + ros::Timer mobility_pub_timer = nh.createTimer(ros::Duration(ros::Rate(config.mobility_rate)), [&lcip](const auto&) {lcip.mobilityPubSpin(); }); + // Start ros::CARMANodeHandle::spin(); diff --git a/lci_strategic_plugin/test/test_strategic_plugin.cpp b/lci_strategic_plugin/test/test_strategic_plugin.cpp index 9b921adae0..1ebeb7f480 100755 --- a/lci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/lci_strategic_plugin/test/test_strategic_plugin.cpp @@ -379,11 +379,46 @@ TEST_F(LCIStrategicTestFixture, handleFailureCase) EXPECT_THROW(lcip.handleFailureCase(5 ,0, 15, 0), std::invalid_argument); +} +TEST(LCIStrategicPluginTest, moboperationcbtest) +{ + cav_msgs::MobilityOperation msg; + msg.strategy = "Carma/light_controlled_intersection"; + std::shared_ptr wm = std::make_shared(); + LCIStrategicPluginConfig config; + LCIStrategicPlugin lcip(wm, config); + ASSERT_EQ(lcip.approaching_light_controlled_interction_, false); + auto msg_ptr = boost::make_shared(msg); + lcip.mobilityOperationCb(msg_ptr); + + ASSERT_EQ(lcip.approaching_light_controlled_interction_, true); } +TEST(LCIStrategicPluginTest, parseStrategyParamstest) +{ + cav_msgs::MobilityOperation msg; + msg.strategy_params = "st:16000,et:32000,dt:48000,dp:1,access:0"; + + std::shared_ptr wm = std::make_shared(); + LCIStrategicPluginConfig config; + LCIStrategicPlugin lcip(wm, config); + + lcip.parseStrategyParams(msg.strategy_params); + + EXPECT_EQ(16000, lcip.scheduled_stop_time_); + EXPECT_EQ(32000, lcip.scheduled_enter_time_); + EXPECT_EQ(48000, lcip.scheduled_depart_time_); + EXPECT_EQ(1, lcip.scheduled_departure_position_); + EXPECT_EQ(false, lcip.is_allowed_int_); + + cav_msgs::MobilityOperation outgoing_msg = lcip.generateMobilityOperation(); + EXPECT_EQ(outgoing_msg.strategy, "Carma/light_controlled_intersection"); + EXPECT_EQ(outgoing_msg.m_header.sender_id, config.vehicle_id); + std::cout << "strategy_param: " << outgoing_msg.strategy_params << std::endl; +} } // namespace lci_strategic_plugin \ No newline at end of file diff --git a/mobilitypath_publisher/config/parameters.yaml b/mobilitypath_publisher/config/parameters.yaml index 2554c5c1a7..783c55ca56 100644 --- a/mobilitypath_publisher/config/parameters.yaml +++ b/mobilitypath_publisher/config/parameters.yaml @@ -1,3 +1,3 @@ # Double: Rate of mobility path broadcast # Units: Hz -path_pub_rate: 10.0 \ No newline at end of file +path_pub_rate: 20.0 \ No newline at end of file diff --git a/motion_computation/src/motion_computation_node.cpp b/motion_computation/src/motion_computation_node.cpp index db07f24590..d83ee74eb4 100644 --- a/motion_computation/src/motion_computation_node.cpp +++ b/motion_computation/src/motion_computation_node.cpp @@ -64,6 +64,22 @@ namespace motion_computation result.successful = !error && !error_2; + if (result.successful){ + // Set motion_worker_'s prediction parameters + motion_worker_.setPredictionTimeStep(config_.prediction_time_step); + motion_worker_.setPredictionPeriod(config_.prediction_period); + motion_worker_.setXAccelerationNoise(config_.cv_x_accel_noise); + motion_worker_.setYAccelerationNoise(config_.cv_y_accel_noise); + motion_worker_.setProcessNoiseMax(config_.prediction_process_noise_max); + motion_worker_.setConfidenceDropRate(config_.prediction_confidence_drop_rate); + motion_worker_.setDetectionInputFlags( + config_.enable_sensor_processing, + config_.enable_bsm_processing, + config_.enable_psm_processing, + config_.enable_mobility_path_processing + ); + } + return result; } diff --git a/plan_delegator/src/plan_delegator.cpp b/plan_delegator/src/plan_delegator.cpp index e24781a92a..dfb1897759 100644 --- a/plan_delegator/src/plan_delegator.cpp +++ b/plan_delegator/src/plan_delegator.cpp @@ -218,7 +218,13 @@ namespace plan_delegator bool PlanDelegator::isManeuverExpired(const cav_msgs::Maneuver& maneuver, ros::Time current_time) const { - return GET_MANEUVER_PROPERTY(maneuver, end_time) <= current_time; // TODO maneuver expiration should maybe be based off of distance not time? https://github.com/usdot-fhwa-stol/carma-platform/issues/1107 + ROS_DEBUG_STREAM("maneuver start time:" << std::to_string(GET_MANEUVER_PROPERTY(maneuver, start_time).toSec())); + ROS_DEBUG_STREAM("maneuver end time:" << std::to_string(GET_MANEUVER_PROPERTY(maneuver, end_time).toSec())); + ROS_DEBUG_STREAM("current time:" << std::to_string(ros::Time::now().toSec())); + bool isexpired = GET_MANEUVER_PROPERTY(maneuver, end_time) <= current_time; + ROS_DEBUG_STREAM("isexpired:" << isexpired); + // TODO: temporary disabling expiration check + return false;// TODO maneuver expiration should maybe be based off of distance not time? https://github.com/usdot-fhwa-stol/carma-platform/issues/1107 } cav_srvs::PlanTrajectory PlanDelegator::composePlanTrajectoryRequest(const cav_msgs::TrajectoryPlan& latest_trajectory_plan, const uint16_t& current_maneuver_index) const diff --git a/platooning_control/config/parameters.yaml b/platooning_control/config/parameters.yaml index 9b0ee94e3d..8a0f8f2dcb 100644 --- a/platooning_control/config/parameters.yaml +++ b/platooning_control/config/parameters.yaml @@ -2,16 +2,20 @@ timeHeadway: 3.5 standStillHeadway: 22.0 maxAccel: 1.5 Kp: 0.4 -Kd: 0.05 #0.1 -Ki: 0.0 #0.035 +Kd: 0.0 #0.1 +Ki: 0.03 #0.035 maxValue: 2.0 minValue: -10.0 cmdTmestamp: 100 -adjustmentCap: 11.0 +adjustmentCap: 15.0 dt: 0.1 -integratorMax: 100 -integratorMin: -100 -wheelBase: 3.09 -lowpassGain: 1.0 -lookaheadRatio: 1.6 +integratorMax: 30 +integratorMin: -30 +wheelBase: 3.39 +lowpassGain: 0.5 #1.0 +lookaheadRatio: 2.0 #1.7 minLookaheadDist: 15.0 +correctionAngle: 0.0 +integratorMax_pp: 0.1 +integratorMin_pp: -0.1 +Ki_pp: 0.012 \ No newline at end of file diff --git a/platooning_control/include/platoon_control.h b/platooning_control/include/platoon_control.h index 2246c88ced..6ef21c0f50 100644 --- a/platooning_control/include/platoon_control.h +++ b/platooning_control/include/platoon_control.h @@ -57,6 +57,10 @@ namespace platoon_control // find the point correspoding to the lookahead distance cav_msgs::TrajectoryPlanPoint getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan); + + // timer callback for control signal publishers + // returns true if control signals are correctly calculated. + bool controlTimerCb(); // local copy of pose geometry_msgs::PoseStamped pose_msg_; @@ -65,6 +69,8 @@ namespace platoon_control double current_speed_ = 0.0; double trajectory_speed_ = 0.0; + cav_msgs::TrajectoryPlan latest_trajectory_; + private: @@ -81,6 +87,8 @@ namespace platoon_control // Variables PlatoonLeaderInfo platoon_leader_; + long prev_input_time_ = 0; //timestamp of the previous trajectory plan input received + long consecutive_input_counter_ = 0; //num inputs seen without a timeout // callback function for pose void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); @@ -96,12 +104,9 @@ namespace platoon_control double getTrajectorySpeed(std::vector trajectory_points); - - // Plugin discovery message cav_msgs::Plugin plugin_discovery_msg_; - // ROS Subscriber ros::Subscriber trajectory_plan_sub; ros::Subscriber current_twist_sub_; @@ -113,12 +118,6 @@ namespace platoon_control ros::Publisher plugin_discovery_pub_; ros::Publisher platoon_info_pub_; ros::Timer discovery_pub_timer_; - - - - - - - + ros::Timer control_pub_timer_; }; } diff --git a/platooning_control/include/platoon_control_config.h b/platooning_control/include/platoon_control_config.h index 247304a98b..ab5fccbef4 100644 --- a/platooning_control/include/platoon_control_config.h +++ b/platooning_control/include/platoon_control_config.h @@ -36,17 +36,24 @@ struct PlatooningControlPluginConfig int cmdTmestamp = 100; double integratorMax = 100; double integratorMin = -100; - double Kdd = 4.5; //coeficient for smooth steering + double Kdd = 4.5; //coefficient for smooth steering double wheelBase = 3.09; double lowpassGain = 0.5; double lookaheadRatio = 2.0; double minLookaheadDist = 6.0; std::string vehicleID = "DEFAULT_VEHICLE_ID"; // Vehicle id is the license plate of the vehicle + int shutdownTimeout = 200; // ms + int ignoreInitialInputs = 0; // num inputs to throw away after startup + double correctionAngle = 0.0; + + double integratorMax_pp = 0.0; + double integratorMin_pp = 0.0; + double Ki_pp = 0.0; friend std::ostream& operator<<(std::ostream& output, const PlatooningControlPluginConfig& c) { - output << "InLaneCruisingPluginConfig { " << std::endl + output << "PlatooningControlPluginConfig { " << std::endl << "timeHeadway: " << c.timeHeadway << std::endl << "standStillHeadway: " << c.standStillHeadway << std::endl << "maxAccel: " << c.maxAccel << std::endl @@ -66,6 +73,12 @@ struct PlatooningControlPluginConfig << "lookaheadRatio: " << c.lookaheadRatio << std::endl << "minLookaheadDist: " << c.minLookaheadDist << std::endl << "vehicleID: " << c.vehicleID << std::endl + << "shutdownTimeout: " << c.shutdownTimeout << std::endl + << "ignoreInitialInputs: " << c.ignoreInitialInputs << std::endl + << "correctionAngle: " << c.correctionAngle << std::endl + << "integratorMax_pp: " << c.integratorMax_pp << std::endl + << "integratorMin_pp: " << c.integratorMin_pp << std::endl + << "Ki_pp: " << c.Ki_pp << std::endl << "}" << std::endl; return output; } diff --git a/platooning_control/include/pure_pursuit.h b/platooning_control/include/pure_pursuit.h index 6e87255b2a..97e29bc134 100644 --- a/platooning_control/include/pure_pursuit.h +++ b/platooning_control/include/pure_pursuit.h @@ -106,7 +106,7 @@ namespace platoon_control // previous trajectory point cav_msgs::TrajectoryPlanPoint tp0; - + double _integral = 0.0; // helper function (if needed) // inline double deg2rad(double deg) const // { diff --git a/platooning_control/src/platoon_control.cpp b/platooning_control/src/platoon_control.cpp index 3de4d6a972..959a292acd 100644 --- a/platooning_control/src/platoon_control.cpp +++ b/platooning_control/src/platoon_control.cpp @@ -49,8 +49,17 @@ namespace platoon_control pnh_->param("lowpassGain", config.lowpassGain, config.lowpassGain); pnh_->param("lookaheadRatio", config.lookaheadRatio, config.lookaheadRatio); pnh_->param("minLookaheadDist", config.minLookaheadDist, config.minLookaheadDist); + pnh_->param("wheelBase", config.wheelBase, config.wheelBase); + pnh_->param("correctionAngle", config.correctionAngle, config.correctionAngle); + pnh_->param("integratorMax_pp", config.integratorMax_pp, config.integratorMax_pp); + pnh_->param("integratorMin_pp", config.integratorMin_pp, config.integratorMin_pp); + pnh_->param("Ki_pp", config.Ki_pp, config.Ki_pp); + + // Global params (from vehicle config) pnh_->getParam("/vehicle_id", config.vehicleID); - pnh_->getParam("/vehicle_wheel_base", config.wheelBase); + // pnh_->getParam("/vehicle_wheel_base", config.wheelBase); + pnh_->getParam("/control_plugin_shutdown_timeout", config.shutdownTimeout); + pnh_->getParam("/control_plugin_ignore_initial_inputs", config.ignoreInitialInputs); pcw_.updateConfigParams(config); config_ = config; @@ -83,7 +92,17 @@ namespace platoon_control discovery_pub_timer_ = pnh_->createTimer( ros::Duration(ros::Rate(10.0)), - [this](const auto&) { plugin_discovery_pub_.publish(plugin_discovery_msg_); }); + [this](const auto&) { plugin_discovery_pub_.publish(plugin_discovery_msg_); + ROS_DEBUG_STREAM("10hz timer callback called");}); + + ROS_DEBUG_STREAM("discovery timer created"); + + control_pub_timer_ = pnh_->createTimer( + ros::Duration(ros::Rate(30.0)), + [this](const auto&) { ROS_DEBUG_STREAM("30hz timer callback called"); + controlTimerCb(); }); + + ROS_DEBUG_STREAM("control timer created "); } @@ -92,23 +111,51 @@ namespace platoon_control ros::CARMANodeHandle::spin(); } + bool PlatoonControlPlugin::controlTimerCb() + { + ROS_DEBUG_STREAM("In control timer callback "); + // If it has been a long time since input data has arrived then reset the input counter and return + // Note: this quiets the controller after its input stream stops, which is necessary to allow + // the replacement controller to publish on the same output topic after this one is done. + long current_time = ros::Time::now().toNSec() / 1000000; + ROS_DEBUG_STREAM("current_time = " << current_time << ", prev_input_time_ = " << prev_input_time_ << ", input counter = " << consecutive_input_counter_); + if (current_time - prev_input_time_ > config_.shutdownTimeout) + { + ROS_DEBUG_STREAM("returning due to timeout."); + consecutive_input_counter_ = 0; + return false; + } - void PlatoonControlPlugin::trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp){ - - if (tp->trajectory_points.size() < 2) { - ROS_WARN_STREAM("PlatoonControlPlugin cannot execute trajectory as only 1 point was provided"); - return; + // If there have not been enough consecutive timely inputs then return (waiting for + // previous control plugin to time out and stop publishing, since it uses same output topic) + if (consecutive_input_counter_ <= config_.ignoreInitialInputs) + { + ROS_DEBUG_STREAM("returning due to first data input"); + return false; } - cav_msgs::TrajectoryPlanPoint first_trajectory_point = tp->trajectory_points[1]; // TODO this variable appears to be misnamed. It is the second trajectory point - cav_msgs::TrajectoryPlanPoint lookahead_point = getLookaheadTrajectoryPoint(*tp); - trajectory_speed_ = getTrajectorySpeed(tp->trajectory_points); + cav_msgs::TrajectoryPlanPoint second_trajectory_point = latest_trajectory_.trajectory_points[1]; + cav_msgs::TrajectoryPlanPoint lookahead_point = getLookaheadTrajectoryPoint(latest_trajectory_); - + trajectory_speed_ = getTrajectorySpeed(latest_trajectory_.trajectory_points); - generateControlSignals(first_trajectory_point, lookahead_point); // TODO this should really be called on a timer against that last trajectory so 30Hz control loop can be achieved + generateControlSignals(second_trajectory_point, lookahead_point); + return true; + } + void PlatoonControlPlugin::trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp) + { + if (tp->trajectory_points.size() < 2) { + ROS_WARN_STREAM("PlatoonControlPlugin cannot execute trajectory as only 1 point was provided"); + return; + } + + latest_trajectory_ = *tp; + prev_input_time_ = ros::Time::now().toNSec() / 1000000; + ++consecutive_input_counter_; + ROS_DEBUG_STREAM("New trajectory plan #" << consecutive_input_counter_ << " at time " << prev_input_time_); + ROS_DEBUG_STREAM("tp header time = " << tp->header.stamp.toNSec() / 1000000); } cav_msgs::TrajectoryPlanPoint PlatoonControlPlugin::getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan) @@ -178,6 +225,15 @@ namespace platoon_control ROS_DEBUG_STREAM("Platoon leader leader cmd speed: " << platoon_leader_.commandSpeed); cav_msgs::PlatooningInfo platooing_info_msg = *msg; + + ROS_DEBUG_STREAM("platooing_info_msg.actual_gap: " << platooing_info_msg.actual_gap); + + if (platooing_info_msg.actual_gap > 5.0) + { + platooing_info_msg.actual_gap -= 5.0; // TODO: temporary: should be vehicle length + } + + ROS_DEBUG_STREAM("platooing_info_msg.actual_gap: " << platooing_info_msg.actual_gap); // platooing_info_msg.desired_gap = pcw_.desired_gap_; // platooing_info_msg.actual_gap = pcw_.actual_gap_; pcw_.actual_gap_ = platooing_info_msg.actual_gap; @@ -218,7 +274,7 @@ namespace platoon_control void PlatoonControlPlugin::generateControlSignals(const cav_msgs::TrajectoryPlanPoint& first_trajectory_point, const cav_msgs::TrajectoryPlanPoint& lookahead_point){ - pcw_.setCurrentSpeed(trajectory_speed_); + pcw_.setCurrentSpeed(trajectory_speed_); //TODO why this and not the actual vehicle speed? Method name suggests different use than this. // pcw_.setCurrentSpeed(current_speed_); pcw_.setLeader(platoon_leader_); pcw_.generateSpeed(first_trajectory_point); @@ -243,9 +299,11 @@ namespace platoon_control double t1 = (trajectory_points[trajectory_points.size()-1].target_time.toSec() - trajectory_points[0].target_time.toSec()); double avg_speed = d1/t1; + ROS_DEBUG_STREAM("trajectory_points size = " << trajectory_points.size() << ", d1 = " << d1 << ", t1 = " << t1 << ", avg_speed = " << avg_speed); for(size_t i = 0; i < trajectory_points.size() - 2; i++ ) - { double dx = trajectory_points[i + 1].x - trajectory_points[i].x; + { + double dx = trajectory_points[i + 1].x - trajectory_points[i].x; double dy = trajectory_points[i + 1].y - trajectory_points[i].y; double d = sqrt(dx*dx + dy*dy); double t = (trajectory_points[i + 1].target_time.toSec() - trajectory_points[i].target_time.toSec()); @@ -259,7 +317,7 @@ namespace platoon_control ROS_DEBUG_STREAM("trajectory speed: " << trajectory_speed); ROS_DEBUG_STREAM("avg trajectory speed: " << avg_speed); - return avg_speed; + return avg_speed; //TODO: why are 2 speeds being calculated? Which should be returned? } diff --git a/platooning_control/src/pure_pursuit.cpp b/platooning_control/src/pure_pursuit.cpp index 47298950a6..c08462fcdd 100644 --- a/platooning_control/src/pure_pursuit.cpp +++ b/platooning_control/src/pure_pursuit.cpp @@ -72,9 +72,32 @@ namespace platoon_control void PurePursuit::calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp) { - double kappa = calculateKappa(tp); - - double steering = atan(config_.wheelBase * kappa); + double kappa = calculateKappa(tp); + ROS_DEBUG_STREAM("kappa pp: " << kappa); + + double lookahead = getLookaheadDist(tp); + ROS_DEBUG_STREAM("lookahead pp: " << lookahead); + + + double error=kappa*lookahead*lookahead/2; + ROS_DEBUG_STREAM("error term pp: " << error); + + // Integral term + _integral += error * config_.dt; + ROS_DEBUG_STREAM("Integral term pp: " << _integral); + + if (_integral > config_.integratorMax_pp){ + _integral = config_.integratorMax_pp; + } + else if (_integral < config_.integratorMin_pp){ + _integral = config_.integratorMin_pp; + } + double Iout = config_.Ki_pp * _integral; + ROS_DEBUG_STREAM("Iout pp: " << Iout); + double steering = atan(config_.wheelBase * kappa)+Iout; + + + steering += config_.correctionAngle; ROS_DEBUG_STREAM("calculated steering angle: " << steering); double filtered_steering = lowPassfilter(config_.lowpassGain, prev_steering, steering); ROS_DEBUG_STREAM("filtered steering: " << filtered_steering); diff --git a/platooning_control_ihp/CMakeLists.txt b/platooning_control_ihp/CMakeLists.txt new file mode 100644 index 0000000000..374e2e1f56 --- /dev/null +++ b/platooning_control_ihp/CMakeLists.txt @@ -0,0 +1,127 @@ +# Copyright (C) 2018-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 2.8.3) +project(platoon_control_ihp) + +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(1) +carma_package() + +## Compile as C++14, supported in ROS Noetic and newer +#add_compile_options(-std=c++14) +#set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") +#set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + carma_utils + cav_msgs + roscpp + std_msgs + autoware_msgs +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + +################################### +## catkin specific configuration ## +################################### + + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs autoware_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +file(GLOB_RECURSE headers */*.hpp */*.h) + +add_executable( ${PROJECT_NAME} + ${headers} + src/platoon_control_ihp.cpp + src/main.cpp) + + +add_library(platoon_control_ihp_plugin_lib + src/platoon_control_ihp.cpp + src/platoon_control_ihp_worker.cpp + src/pid_controller.cpp + src/pure_pursuit.cpp +) + +add_dependencies(platoon_control_ihp_plugin_lib ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME} platoon_control_ihp_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +############# +## Install ## +############# + + +install(DIRECTORY include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Install C++ +install(TARGETS ${PROJECT_NAME} platoon_control_ihp_plugin_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + + +############# +## Testing ## +############# + +catkin_add_gmock(${PROJECT_NAME}-test + test/test_pid.cpp + test/test_pure.cpp + test/test_worker.cpp + test/test_control.cpp + test/test_main.cpp) + +if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test platoon_control_ihp_plugin_lib ${catkin_LIBRARIES}) +endif() + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_platoon_control_ihp test/mynode.test test/test_mynode.cpp) + target_link_libraries(test_platoon_control_ihp ${catkin_LIBRARIES}) +endif() + + diff --git a/platooning_control_ihp/config/parameters.yaml b/platooning_control_ihp/config/parameters.yaml new file mode 100644 index 0000000000..1663a40185 --- /dev/null +++ b/platooning_control_ihp/config/parameters.yaml @@ -0,0 +1,25 @@ +timeHeadway: 3.5 +standStillHeadway: 22.0 +maxAccel: 1.5 +Kp: 0.4 +Kd: 0.05 #0.1 +Ki: 0.0 #0.035 +maxValue: 2.0 +minValue: -10.0 +cmdTmestamp: 100 +adjustmentCap: 11.0 +dt: 0.1 +integratorMax: 100 +integratorMin: -100 +wheelBase: 3.09 +lowpassGain: 1.0 +lookaheadRatio: 1.6 +minLookaheadDist: 15.0 +vehicleLength: 5.0 +ss_theta: 4.0 +standstill: 2.0 +inter_tau: 1.5 +intra_tau: 0.6 +gap_weight: 0.9 +time_step: 5 +test_front_join: false diff --git a/platooning_control_ihp/include/pid_controller.h b/platooning_control_ihp/include/pid_controller.h new file mode 100644 index 0000000000..f94595a9df --- /dev/null +++ b/platooning_control_ihp/include/pid_controller.h @@ -0,0 +1,61 @@ +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include +#include "platoon_control_ihp_config.h" + + +namespace platoon_control_ihp +{ + + + class PIDController + { + public: + + PIDController(); + + PlatooningControlIHPPluginConfig config_; + + // ~PIDController(); + + // Kp - proportional gain + // Ki - Integral gain + // Kd - derivative gain + // dt - loop interval time + // max - maximum value of manipulated variable + // min - minimum value of manipulated variable + // PID( double dt, double max, double min, double Kp, double Kd, double Ki ); + + // Returns the manipulated variable given a setpoint and current process value + double calculate( double setpoint, double pv ); + // ~PID(); + + void reset(); + + + + + + + private: + + double _pre_error = 0.0; + double _integral = 0.0; + + }; +} diff --git a/platooning_control_ihp/include/platoon_control_ihp.h b/platooning_control_ihp/include/platoon_control_ihp.h new file mode 100644 index 0000000000..774c535d2c --- /dev/null +++ b/platooning_control_ihp/include/platoon_control_ihp.h @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2019-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 "platoon_control_ihp_worker.h" + + + + +namespace platoon_control_ihp +{ + class PlatoonControlIHPPlugin + { + public: + + // Default constructor for PlatoonControlPlugin class + PlatoonControlIHPPlugin(); + + void initialize(); + + // general starting point of this node + void run(); + + // Compose twist message by calculating speed and steering commands. + void generateControlSignals(const cav_msgs::TrajectoryPlanPoint& point0, const cav_msgs::TrajectoryPlanPoint& point_end); + + // Compose twist message by calculating speed and steering commands. + geometry_msgs::TwistStamped composeTwistCmd(double linear_vel, double angular_vel); + + // Compose control message by calculating speed and steering commands. + autoware_msgs::ControlCommandStamped composeCtrlCmd(double linear_vel, double steering_angle); + + // find the point correspoding to the lookahead distance + cav_msgs::TrajectoryPlanPoint getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan); + + // timer callback for control signal publishers + // returns true if control signals are correctly calculated. + bool controlTimerCb(); + + // local copy of pose + geometry_msgs::PoseStamped pose_msg_; + + // current speed (in m/s) + double current_speed_ = 0.0; + double trajectory_speed_ = 0.0; + + cav_msgs::TrajectoryPlan latest_trajectory_; + + + private: + + + // CARMA ROS node handles + std::shared_ptr nh_, pnh_; + + // platoon control worker object + PlatoonControlIHPWorker pcw_; + + // platooning config object + PlatooningControlIHPPluginConfig config_; + + + // Variables + PlatoonLeaderInfo platoon_leader_; + long prev_input_time_ = 0; //timestamp of the previous trajectory plan input received + long consecutive_input_counter_ = 0; //num inputs seen without a timeout + + // callback function for pose + void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); + + // callback function for platoon info + void platoonInfo_cb(const cav_msgs::PlatooningInfoConstPtr& msg); + + // callback function for trajectory plan + void trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp); + + // callback function for current twist + void currentTwist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist); + + double getTrajectorySpeed(std::vector trajectory_points); + + // Plugin discovery message + cav_msgs::Plugin plugin_discovery_msg_; + + // ROS Subscriber + ros::Subscriber trajectory_plan_sub; + ros::Subscriber current_twist_sub_; + ros::Subscriber pose_sub_; + ros::Subscriber platoon_info_sub_; + // ROS Publisher + ros::Publisher twist_pub_; + ros::Publisher ctrl_pub_; + ros::Publisher plugin_discovery_pub_; + ros::Publisher platoon_info_pub_; + ros::Timer discovery_pub_timer_; + ros::Timer control_pub_timer_; + }; +} diff --git a/platooning_control_ihp/include/platoon_control_ihp_config.h b/platooning_control_ihp/include/platoon_control_ihp_config.h new file mode 100644 index 0000000000..1abeee40b6 --- /dev/null +++ b/platooning_control_ihp/include/platoon_control_ihp_config.h @@ -0,0 +1,94 @@ +#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 PlatooningControlPlugin + */ +struct PlatooningControlIHPPluginConfig +{ + double timeHeadway = 6.0; + double standStillHeadway = 12.0; + double maxAccel = 2.5; + double Kp = 0.5; + double Kd = -0.5; + double Ki = 0.0; + double maxValue = 100; + double minValue = -100; + double dt = 0.1; + double adjustmentCap = 10; + int cmdTmestamp = 100; + double integratorMax = 100; + double integratorMin = -100; + double Kdd = 4.5; //coefficient for smooth steering + double wheelBase = 3.09; + double lowpassGain = 0.5; + double lookaheadRatio = 2.0; + double minLookaheadDist = 6.0; + std::string vehicleID = "DEFAULT_VEHICLE_ID"; // Vehicle id is the license plate of the vehicle + int shutdownTimeout = 200; // ms + int ignoreInitialInputs = 0; // num inputs to throw away after startup + + // added for gap regulation + double vehicleLength = 5.0; // m + // UCLA: Added for IHP control + // ---------------------- UCLA: parameters for IHP platoon trajectory regulation ---------------- + /** + * \brief Parameter sets for IHP platoon trajectory regulation algorithm. + * Please refer to the updated design doc for detailed parameter description. + */ + double ss_theta = 4.0; // Minimum speed to be considered as moving, in mph. + double standstill = 2.0; // Extra time needed to reacte to traffic sceanrios when vehicle is standstill (not moving), in s. + double inter_tau = 1.5; // Inter-platoon time gap, refer to bumper to bumper gap time, in s. + double intra_tau = 0.6; // Intra-platoon time gao, refer to bumper to bumper gap time, in s. + double gap_weight = 0.9; // Weighted ratio for time-gap based calculation, unitless. + double time_step = 5; // The time step ga regulation algorithm uses to calculate desired position, in s. + bool test_front_join = false; //Flag to enable/disable front join functionality with two vehicles. + // Flag can be set to true, to test front join functionality with two vehicles + // But in normal operating conditions it should be set to false + //------------------------------------------------------------------------------------------------ + + friend std::ostream& operator<<(std::ostream& output, const PlatooningControlIHPPluginConfig& c) + { + output << "PlatoonControlIHPPluginConfig { " << std::endl + << "timeHeadway: " << c.timeHeadway << std::endl + << "standStillHeadway: " << c.standStillHeadway << std::endl + << "maxAccel: " << c.maxAccel << std::endl + << "Kp: " << c.Kp << std::endl + << "Kd: " << c.Kd << std::endl + << "Ki: " << c.Ki << std::endl + << "maxValue: " << c.maxValue << std::endl + << "minValue: " << c.minValue << std::endl + << "dt: " << c.dt << std::endl + << "adjustmentCap: " << c.adjustmentCap << std::endl + << "cmdTmestamp: " << c.cmdTmestamp << std::endl + << "integratorMax: " << c.integratorMax << std::endl + << "integratorMin: " << c.integratorMin << std::endl + << "Kdd: " << c.Kdd << std::endl + << "wheelBase: " << c.wheelBase << std::endl + << "lowpassGain: " << c.lowpassGain << std::endl + << "lookaheadRatio: " << c.lookaheadRatio << std::endl + << "minLookaheadDist: " << c.minLookaheadDist << std::endl + << "vehicleID: " << c.vehicleID << std::endl + << "shutdownTimeout: " << c.shutdownTimeout << std::endl + << "ignoreInitialInputs: " << c.ignoreInitialInputs << std::endl + << "}" << std::endl; + return output; + } +}; \ No newline at end of file diff --git a/platooning_control_ihp/include/platoon_control_ihp_worker.h b/platooning_control_ihp/include/platoon_control_ihp_worker.h new file mode 100644 index 0000000000..64a68b3eea --- /dev/null +++ b/platooning_control_ihp/include/platoon_control_ihp_worker.h @@ -0,0 +1,164 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include "pid_controller.h" +#include "pure_pursuit.h" +#include "platoon_control_ihp_config.h" +#include + + + + + +namespace platoon_control_ihp +{ + /** + * \brief Platoon Leader Struct + */ + struct PlatoonLeaderInfo{ + // 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; + // leader index in the platoon + int leaderIndex; + // Number of vehicles in front + int NumberOfVehicleInFront; + + }; + + + // Leader info: platoonmember + leader index + number of vehicles in front + + + class PlatoonControlIHPWorker + { + public: + + /** + * \brief Default constructor for platooning control worker + */ + PlatoonControlIHPWorker(); + + /** + * \brief Update configurations + */ + void updateConfigParams(PlatooningControlIHPPluginConfig new_config); + + /** + * \brief Returns latest speed command + */ + double getLastSpeedCommand() const; + + /** + * \brief Generates speed commands based on the trajectory point + */ + void generateSpeed(const cav_msgs::TrajectoryPlanPoint& point); + + /** + * \brief Generates steering commands based on lookahead trajectory point + */ + void generateSteer(const cav_msgs::TrajectoryPlanPoint& point); + + /** + * \brief set platoon leader + */ + void setLeader(const PlatoonLeaderInfo& leader); + + /** + * \brief set current speed + */ + void setCurrentSpeed(double speed); + + /** + * \brief UCLA: HP gap regulation that calculate the deisred position (Dtd, in m) for the platoon members. + * + * \params leaderCurrentPosition: The current position (dtd) of the leader, in m. + * + */ + double getIHPTargetPositionFollower(double leaderCurrentPosition); + + // Member Variables + double speedCmd = 0; + double currentSpeed = 0; + double lastCmdSpeed = 0.0; + double speedCmd_ = 0; + double steerCmd_ = 0; + double angVelCmd_ = 0; + + // Note: the standstill headway is an initial value, a desired gap was extracted from platooing_info_msg + double desired_gap_ = ctrl_config_.standStillHeadway; + + double actual_gap_ = 0.0; + bool last_cmd_set_ = false; + // UCLA: Read host platoon position as member variable + int host_platoon_position_ = 0; + // UCLA: Read the time headway summation of all predecessors + double current_predecessor_time_headway_sum_ = 0.0; + // UCLA: Read predecessor speed m/s, and DtD position m. + double predecessor_speed_ = 0.0; + double predecessor_position_ = 0.0; + + // Platoon Leader + PlatoonLeaderInfo platoon_leader; + + + void setCurrentPose(const geometry_msgs::PoseStamped msg); + + // geometry pose + geometry_msgs::Pose current_pose_; + + + private: + // config parameters + PlatooningControlIHPPluginConfig ctrl_config_; + + // pid controller object + PIDController pid_ctrl_; + + // pure pursuit controller object + PurePursuit pp_; + + double dist_to_front_vehicle; + + bool leaderSpeedCapEnabled = true; + bool enableMaxAdjustmentFilter = true; + + bool speedLimitCapEnabled = true; + bool enableLocalSpeedLimitFilter = true; + + bool maxAccelCapEnabled = true; + bool enableMaxAccelFilter = true; + + + }; +} diff --git a/platooning_control_ihp/include/pure_pursuit.h b/platooning_control_ihp/include/pure_pursuit.h new file mode 100644 index 0000000000..eeb9a0a6d2 --- /dev/null +++ b/platooning_control_ihp/include/pure_pursuit.h @@ -0,0 +1,118 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + + +#include +#include +#include +#include +#include +#include +#include +#include "platoon_control_ihp_config.h" + + + + + + + +namespace platoon_control_ihp +{ + + + class PurePursuit + { + + + public: + + /** + * \brief Default constructor for pure pursuit + */ + PurePursuit(); + + /** + * \brief calculates steering angle based on lookahead trajectory point + */ + void calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp); + + /** + * \brief calculates curvature to the lookahead trajectory point + */ + double calculateKappa(const cav_msgs::TrajectoryPlanPoint& tp); + + // geometry pose + geometry_msgs::Pose current_pose_; + double velocity_ = 0.0; + double angular_velocity_ = 0; + double steering_angle_ = 0; + + PlatooningControlIHPPluginConfig config_; + + /** + * \brief calculates sin of the heading angle to the target point + */ + double getAlphaSin(cav_msgs::TrajectoryPlanPoint tp, geometry_msgs::Pose current_pose); + + /** + * \brief Lowpass filter to smoothen control signal + */ + double lowPassfilter(double gain, double prev_value, double value); + + /** + * \brief returns steering angle + */ + double getSteeringAngle(); + + /**angular velocity + */ + double getAngularVelocity(); + + private: + + // calculate the lookahead distance from next trajectory point + double getLookaheadDist(const cav_msgs::TrajectoryPlanPoint& tp) const; + + // calculate yaw angle of the vehicle + double getYaw(const cav_msgs::TrajectoryPlanPoint& tp) const; + + // calculate steering direction + int getSteeringDirection(std::vector v1, std::vector v2) const; + + + + + double prev_steering = 0.0; + double prev_ang_vel = 0.0; + + + + // previous trajectory point + cav_msgs::TrajectoryPlanPoint tp0; + + + // helper function (if needed) + // inline double deg2rad(double deg) const + // { + // return deg * M_PI / 180; + // } // convert degree to radian + + + }; +} diff --git a/trajectory_executor/test/trajectory_executor_1.test b/platooning_control_ihp/launch/platoon_control_ihp.launch similarity index 63% rename from trajectory_executor/test/trajectory_executor_1.test rename to platooning_control_ihp/launch/platoon_control_ihp.launch index 3a6ace771d..53fa23e3ab 100644 --- a/trajectory_executor/test/trajectory_executor_1.test +++ b/platooning_control_ihp/launch/platoon_control_ihp.launch @@ -1,5 +1,5 @@ - + - - - - - + + + diff --git a/platooning_control_ihp/package.xml b/platooning_control_ihp/package.xml new file mode 100644 index 0000000000..a2efd03893 --- /dev/null +++ b/platooning_control_ihp/package.xml @@ -0,0 +1,15 @@ + + + platoon_control_ihp + 3.3.0 + The node is to control the ihp platooning maneuver + carma + Apache 2.0 License + catkin + carma_utils + cav_msgs + roscpp + std_msgs + autoware_msgs + carma_cmake_common + diff --git a/platooning_control_ihp/src/main.cpp b/platooning_control_ihp/src/main.cpp new file mode 100644 index 0000000000..3731b4bf49 --- /dev/null +++ b/platooning_control_ihp/src/main.cpp @@ -0,0 +1,32 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + + +#include +#include "platoon_control_ihp.h" + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "platoon_control_ihp"); + platoon_control_ihp::PlatoonControlIHPPlugin node; + node.run(); + return 0; +}; + + diff --git a/platooning_control_ihp/src/pid_controller.cpp b/platooning_control_ihp/src/pid_controller.cpp new file mode 100644 index 0000000000..32dbc85cb9 --- /dev/null +++ b/platooning_control_ihp/src/pid_controller.cpp @@ -0,0 +1,78 @@ +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "pid_controller.h" + + +namespace platoon_control_ihp +{ + PIDController::PIDController(){} + + double PIDController::calculate( double setpoint, double pv ) + { + // Calculate error + double error = setpoint - pv; + ROS_DEBUG_STREAM("PID error: " << error); + + // Proportional term + double Pout = config_.Kp * error; + ROS_DEBUG_STREAM("Proportional term: " << Pout); + + // Integral term + _integral += error * config_.dt; + ROS_DEBUG_STREAM("Integral term: " << _integral); + + if (_integral > config_.integratorMax){ + _integral = config_.integratorMax; + } + else if (_integral < config_.integratorMin){ + _integral = config_.integratorMin; + } + double Iout = config_.Ki * _integral; + ROS_DEBUG_STREAM("Iout: " << Iout); + + // Derivative term + double derivative = (error - _pre_error) / config_.dt; + ROS_DEBUG_STREAM("derivative term: " << derivative); + double Dout = config_.Kd * derivative; + ROS_DEBUG_STREAM("Dout: " << Dout); + + // Calculate total output + double output = Pout + Iout + Dout; + ROS_DEBUG_STREAM("total controller output: " << output); + + // Restrict to max/min + if( output > config_.maxValue ) + output = config_.maxValue; + else if( output < config_.minValue ) + output = config_.minValue; + // Save error to previous error + _pre_error = error; + + return output; + + } + + + + void PIDController::reset() + { + _integral = 0.0; + _pre_error = 0.0; + } + +} diff --git a/platooning_control_ihp/src/platoon_control_ihp.cpp b/platooning_control_ihp/src/platoon_control_ihp.cpp new file mode 100644 index 0000000000..1d5ad7f434 --- /dev/null +++ b/platooning_control_ihp/src/platoon_control_ihp.cpp @@ -0,0 +1,323 @@ +/* + * Copyright (C) 2019-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 "platoon_control_ihp.h" + +namespace platoon_control_ihp +{ +// @SONAR_STOP@ + PlatoonControlIHPPlugin::PlatoonControlIHPPlugin() + { + pcw_ = PlatoonControlIHPWorker(); + } + + + void PlatoonControlIHPPlugin::initialize() + { + nh_.reset(new ros::CARMANodeHandle()); + pnh_.reset(new ros::CARMANodeHandle("~")); + + PlatooningControlIHPPluginConfig config; + + pnh_->param("timeHeadway", config.timeHeadway, config.timeHeadway); + pnh_->param("standStillHeadway", config.standStillHeadway, config.standStillHeadway); + pnh_->param("maxAccel", config.maxAccel, config.maxAccel); + pnh_->param("Kp", config.Kp, config.Kp); + pnh_->param("Kd", config.Kd, config.Kd); + pnh_->param("Ki", config.Ki, config.Ki); + pnh_->param("maxValue", config.maxValue, config.maxValue); + pnh_->param("minValue", config.minValue, config.minValue); + pnh_->param("dt", config.dt, config.dt); + pnh_->param("adjustmentCap", config.adjustmentCap, config.adjustmentCap); + pnh_->param("integratorMax", config.integratorMax, config.integratorMax); + pnh_->param("integratorMin", config.integratorMin, config.integratorMin); + pnh_->param("Kdd", config.Kdd, config.Kdd); + pnh_->param("cmdTmestamp", config.cmdTmestamp, config.cmdTmestamp); + pnh_->param("lowpassGain", config.lowpassGain, config.lowpassGain); + pnh_->param("lookaheadRatio", config.lookaheadRatio, config.lookaheadRatio); + pnh_->param("minLookaheadDist", config.minLookaheadDist, config.minLookaheadDist); + + // Global params (from vehicle config) + pnh_->getParam("/vehicle_id", config.vehicleID); + pnh_->getParam("/vehicle_wheel_base", config.wheelBase); + pnh_->getParam("/control_plugin_shutdown_timeout", config.shutdownTimeout); + pnh_->getParam("/control_plugin_ignore_initial_inputs", config.ignoreInitialInputs); + + pcw_.updateConfigParams(config); + config_ = config; + + // Trajectory Plan Subscriber + trajectory_plan_sub = nh_->subscribe("PlatoonControlIHPPlugin/plan_trajectory", 1, &PlatoonControlIHPPlugin::trajectoryPlan_cb, this); + + // Current Twist Subscriber + current_twist_sub_ = nh_->subscribe("current_velocity", 1, &PlatoonControlIHPPlugin::currentTwist_cb, this); + + // Platoon Info Subscriber + // topic name so it is specific to ihp plugins + platoon_info_sub_ = nh_->subscribe("platoon_info_ihp", 1, &PlatoonControlIHPPlugin::platoonInfo_cb, this); + + // Control Publisher + twist_pub_ = nh_->advertise("twist_raw", 5, true); + ctrl_pub_ = nh_->advertise("ctrl_raw", 5, true); + platoon_info_pub_ = nh_->advertise("platooning_info", 1, true); + + + pose_sub_ = nh_->subscribe("current_pose", 1, &PlatoonControlIHPPlugin::pose_cb, this); + + plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); + plugin_discovery_msg_.name = "PlatoonControlIHPPlugin"; + plugin_discovery_msg_.version_id = "v1.0"; + plugin_discovery_msg_.available = true; + plugin_discovery_msg_.activated = true; + plugin_discovery_msg_.type = cav_msgs::Plugin::CONTROL; + plugin_discovery_msg_.capability = "control/trajectory_control"; + + + discovery_pub_timer_ = pnh_->createTimer( + ros::Duration(ros::Rate(10.0)), + [this](const auto&) { plugin_discovery_pub_.publish(plugin_discovery_msg_); + ROS_DEBUG_STREAM("10hz timer callback called");}); + + ROS_DEBUG_STREAM("discovery timer created"); + + control_pub_timer_ = pnh_->createTimer( + ros::Duration(ros::Rate(30.0)), + [this](const auto&) { ROS_DEBUG_STREAM("30hz timer callback called"); + controlTimerCb(); }); + + ROS_DEBUG_STREAM("control timer created "); + } + + + void PlatoonControlIHPPlugin::run() + { + initialize(); + ros::CARMANodeHandle::spin(); + } + + bool PlatoonControlIHPPlugin::controlTimerCb() + { + ROS_DEBUG_STREAM("In control timer callback "); + // If it has been a long time since input data has arrived then reset the input counter and return + // Note: this quiets the controller after its input stream stops, which is necessary to allow + // the replacement controller to publish on the same output topic after this one is done. + long current_time = ros::Time::now().toNSec() / 1000000; + ROS_DEBUG_STREAM("current_time = " << current_time << ", prev_input_time_ = " << prev_input_time_ << ", input counter = " << consecutive_input_counter_); + if (current_time - prev_input_time_ > config_.shutdownTimeout) + { + ROS_DEBUG_STREAM("returning due to timeout."); + consecutive_input_counter_ = 0; + return false; + } + + // If there have not been enough consecutive timely inputs then return (waiting for + // previous control plugin to time out and stop publishing, since it uses same output topic) + if (consecutive_input_counter_ <= config_.ignoreInitialInputs) + { + ROS_DEBUG_STREAM("returning due to first data input"); + return false; + } + + cav_msgs::TrajectoryPlanPoint second_trajectory_point = latest_trajectory_.trajectory_points[1]; + cav_msgs::TrajectoryPlanPoint lookahead_point = getLookaheadTrajectoryPoint(latest_trajectory_); + + trajectory_speed_ = getTrajectorySpeed(latest_trajectory_.trajectory_points); + + generateControlSignals(second_trajectory_point, lookahead_point); + + return true; + } + + void PlatoonControlIHPPlugin::trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp) + { + if (tp->trajectory_points.size() < 2) { + ROS_WARN_STREAM("PlatoonControlIHPPlugin cannot execute trajectory as only 1 point was provided"); + return; + } + + latest_trajectory_ = *tp; + prev_input_time_ = ros::Time::now().toNSec() / 1000000; + ++consecutive_input_counter_; + ROS_DEBUG_STREAM("New trajectory plan #" << consecutive_input_counter_ << " at time " << prev_input_time_); + ROS_DEBUG_STREAM("tp header time = " << tp->header.stamp.toNSec() / 1000000); + } + + cav_msgs::TrajectoryPlanPoint PlatoonControlIHPPlugin::getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan) + { + cav_msgs::TrajectoryPlanPoint lookahead_point; + + double lookahead_dist = config_.lookaheadRatio * current_speed_; + ROS_DEBUG_STREAM("lookahead based on speed: " << lookahead_dist); + + lookahead_dist = std::max(config_.minLookaheadDist, lookahead_dist); + ROS_DEBUG_STREAM("final lookahead: " << lookahead_dist); + + double traveled_dist = 0.0; + bool found_point = false; + + for (size_t i = 1; ileader_id; + platoon_leader_.vehiclePosition = msg->leader_downtrack_distance; + platoon_leader_.commandSpeed = msg->leader_cmd_speed; + // TODO: index is 0 temp to test the leader state + platoon_leader_.NumberOfVehicleInFront = msg->host_platoon_position; + platoon_leader_.leaderIndex = 0; + + ROS_DEBUG_STREAM("Platoon leader leader id: " << platoon_leader_.staticId); + ROS_DEBUG_STREAM("Platoon leader leader pose: " << platoon_leader_.vehiclePosition); + ROS_DEBUG_STREAM("Platoon leader leader cmd speed: " << platoon_leader_.commandSpeed); + + cav_msgs::PlatooningInfo platooing_info_msg = *msg; + // platooing_info_msg.desired_gap = pcw_.desired_gap_; + // platooing_info_msg.actual_gap = pcw_.actual_gap_; + pcw_.actual_gap_ = platooing_info_msg.actual_gap; + pcw_.desired_gap_ = platooing_info_msg.desired_gap; + + // UCLA: Read host platoon position + pcw_.host_platoon_position_ = platooing_info_msg.host_platoon_position; + // UCLA: Read the time headway summation of all predecessors + pcw_.current_predecessor_time_headway_sum_ = platooing_info_msg.current_predecessor_time_headway_sum; + // UCLA: Read predecessor speed m/s, and DtD position m. + pcw_.predecessor_speed_ = platooing_info_msg.predecessor_speed; + pcw_.predecessor_position_ = platooing_info_msg.predecessor_position; + + platooing_info_msg.host_cmd_speed = pcw_.speedCmd_; + platoon_info_pub_.publish(platooing_info_msg); + } + + + void PlatoonControlIHPPlugin::currentTwist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist) + { + current_speed_ = twist->twist.linear.x; + } + + +// @SONAR_START@ + + geometry_msgs::TwistStamped PlatoonControlIHPPlugin::composeTwistCmd(double linear_vel, double angular_vel) + { + geometry_msgs::TwistStamped cmd_twist; + cmd_twist.twist.linear.x = linear_vel; + cmd_twist.twist.angular.z = angular_vel; + cmd_twist.header.stamp = ros::Time::now(); + return cmd_twist; + } + + autoware_msgs::ControlCommandStamped PlatoonControlIHPPlugin::composeCtrlCmd(double linear_vel, double steering_angle) + { + autoware_msgs::ControlCommandStamped cmd_ctrl; + cmd_ctrl.header.stamp = ros::Time::now(); + cmd_ctrl.cmd.linear_velocity = linear_vel; + ROS_DEBUG_STREAM("ctrl command speed " << cmd_ctrl.cmd.linear_velocity); + cmd_ctrl.cmd.steering_angle = steering_angle; + ROS_DEBUG_STREAM("ctrl command steering " << cmd_ctrl.cmd.steering_angle); + + return cmd_ctrl; + } + + void PlatoonControlIHPPlugin::generateControlSignals(const cav_msgs::TrajectoryPlanPoint& first_trajectory_point, const cav_msgs::TrajectoryPlanPoint& lookahead_point) + { + + // setting a speed baseline according to the trajectory speed profile. PID will calculate additional speed changes in addition to this value. + pcw_.setCurrentSpeed(trajectory_speed_); //TODO why this and not the actual vehicle speed? Method name suggests different use than this. + // pcw_.setCurrentSpeed(current_speed_); + pcw_.setLeader(platoon_leader_); + pcw_.generateSpeed(first_trajectory_point); + pcw_.generateSteer(lookahead_point); + + + geometry_msgs::TwistStamped twist_msg = composeTwistCmd(pcw_.speedCmd_, pcw_.angVelCmd_); + twist_pub_.publish(twist_msg); + + autoware_msgs::ControlCommandStamped ctrl_msg = composeCtrlCmd(pcw_.speedCmd_, pcw_.steerCmd_); + ctrl_pub_.publish(ctrl_msg); + } + + // extract maximum speed of trajectory + double PlatoonControlIHPPlugin::getTrajectorySpeed(std::vector trajectory_points) + { + double trajectory_speed = 0; + + double dx1 = trajectory_points[trajectory_points.size()-1].x - trajectory_points[0].x; + double dy1 = trajectory_points[trajectory_points.size()-1].y - trajectory_points[0].y; + double d1 = sqrt(dx1*dx1 + dy1*dy1); + double t1 = (trajectory_points[trajectory_points.size()-1].target_time.toSec() - trajectory_points[0].target_time.toSec()); + + double avg_speed = d1/t1; + ROS_DEBUG_STREAM("trajectory_points size = " << trajectory_points.size() << ", d1 = " << d1 << ", t1 = " << t1 << ", avg_speed = " << avg_speed); + + for(size_t i = 0; i < trajectory_points.size() - 2; i++ ) + { + double dx = trajectory_points[i + 1].x - trajectory_points[i].x; + double dy = trajectory_points[i + 1].y - trajectory_points[i].y; + double d = sqrt(dx*dx + dy*dy); + double t = (trajectory_points[i + 1].target_time.toSec() - trajectory_points[i].target_time.toSec()); + double v = d/t; + if(v > trajectory_speed) + { + trajectory_speed = v; + } + } + + ROS_DEBUG_STREAM("trajectory speed: " << trajectory_speed); + ROS_DEBUG_STREAM("avg trajectory speed: " << avg_speed); + + return avg_speed; //TODO: why are 2 speeds being calculated? Which should be returned? + + } + +} diff --git a/platooning_control_ihp/src/platoon_control_ihp_worker.cpp b/platooning_control_ihp/src/platoon_control_ihp_worker.cpp new file mode 100644 index 0000000000..594503d07f --- /dev/null +++ b/platooning_control_ihp/src/platoon_control_ihp_worker.cpp @@ -0,0 +1,239 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "platoon_control_ihp_worker.h" + + +namespace platoon_control_ihp +{ + + PlatoonControlIHPWorker::PlatoonControlIHPWorker() + { + pid_ctrl_ = PIDController(); + pp_ = PurePursuit(); + } + + void PlatoonControlIHPWorker::updateConfigParams(PlatooningControlIHPPluginConfig new_config) + { + ctrl_config_ = new_config; + pid_ctrl_.config_ = new_config; + pp_.config_ = new_config; + } + + double PlatoonControlIHPWorker::getLastSpeedCommand() const + { + return speedCmd_; + } + + void PlatoonControlIHPWorker::setCurrentPose(const geometry_msgs::PoseStamped msg) + { + current_pose_ = msg.pose; + } + + double PlatoonControlIHPWorker::getIHPTargetPositionFollower(double leaderCurrentPosition) + { + /** + * Calculate desired position based on previous vehicle's trajectory for followers. + * + * TODO: The platoon trajectory regulation is derived with the assumption that all vehicle + * have identical length (i.e., 5m). Future development is needed to include variable + * vehicle length into consideration. + */ + + + // 1. find the summation of "veh_len/veh_speed" for all predecessors + double tmp_time_hdw = current_predecessor_time_headway_sum_; + + // 2. read host veh and front veh info + // Predecessor vehicle data. + double pred_spd = predecessor_speed_; // m/s + double pred_pos = predecessor_position_; // m + + // host data. + double ego_spd = currentSpeed; // m/s + double ego_pos = leaderCurrentPosition - actual_gap_; // m + + // platoon position index + int pos_idx = host_platoon_position_; + + double desirePlatoonGap = ctrl_config_.intra_tau; //s + + // IHP desired position calculation methods + double pos_g; // desired downtrack position calculated with time-gap, in m. + double pos_h; // desired downtrack position calculated with distance headaway, in m. + + // 3. IHP gap regualtion + // intermediate variables + double timeGapAndStepRatio = desirePlatoonGap/ctrl_config_.time_step; // The ratio between desired platoon time gap and the current time_step. + double totalTimeGap = desirePlatoonGap*pos_idx; // The overall time gap from host vehicle to the platoon leader, in s. + + // calcilate pos_gap and pos_headway + if ((pred_spd <= ego_spd) && (ego_spd <= ctrl_config_.ss_theta)) + { + // pos_g + pos_g = (pred_pos - ctrl_config_.vehicleLength - ctrl_config_.standstill + ego_pos*timeGapAndStepRatio) / (1 + timeGapAndStepRatio); + // pos_h + double pos_h_nom = (pred_pos - ctrl_config_.standstill + ego_pos*(totalTimeGap + tmp_time_hdw)/ctrl_config_.time_step); + double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/ctrl_config_.time_step)); + pos_h = pos_h_nom/pos_h_denom; + + } + else + { + // pos_g + pos_g = (pred_pos - ctrl_config_.vehicleLength + ego_pos*(timeGapAndStepRatio)) / (1 + timeGapAndStepRatio); + // pos_h + double pos_h_nom = (pred_pos + ego_pos*(totalTimeGap + tmp_time_hdw)/ctrl_config_.time_step); + double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/ctrl_config_.time_step)); + pos_h = pos_h_nom/pos_h_denom; + } + + // desire speed and desire location + double pos_des = ctrl_config_.gap_weight*pos_g + (1.0 - ctrl_config_.gap_weight)*pos_h; + // double des_spd = (pos_des - ego_pos) / ctrl_config_.time_step; + + // return IHP calculated desired location + return pos_des; + } + + + void PlatoonControlIHPWorker::generateSpeed(const cav_msgs::TrajectoryPlanPoint& point) + { + double speed_cmd = 0; + + if (!last_cmd_set_) + { + // last speed command for smooth speed transition + lastCmdSpeed = currentSpeed; + last_cmd_set_ = true; + } + + PlatoonLeaderInfo leader = platoon_leader; + if(leader.staticId != "" && leader.staticId != ctrl_config_.vehicleID) + { + double controllerOutput = 0.0; + + // --------- Calculate desired gap --------- + double leaderCurrentPosition = leader.vehiclePosition; + ROS_DEBUG_STREAM("The current leader position is " << leaderCurrentPosition); + + double desiredHostPosition = leaderCurrentPosition - desired_gap_; + ROS_DEBUG_STREAM("desiredHostPosition = " << desiredHostPosition); + + // --------- conisder use IHP here instead to regualte gap ---------- + // Call IHP gap regulation function to re-calculate desired Host position based on entire platoon's info + double desiredHostPosition_IHP = getIHPTargetPositionFollower(leaderCurrentPosition); + double hostVehiclePosition = leaderCurrentPosition - actual_gap_; + ROS_DEBUG_STREAM("hostVehiclePosition = " << hostVehiclePosition); + + // UCLA: Replace desiredPosition with desiredPosition_IHP here. + controllerOutput = pid_ctrl_.calculate(desiredHostPosition_IHP, hostVehiclePosition); + + double adjSpeedCmd = controllerOutput + leader.commandSpeed; + ROS_DEBUG_STREAM("Adjusted Speed Cmd = " << adjSpeedCmd << "; Controller Output = " << controllerOutput + << "; Leader CmdSpeed= " << leader.commandSpeed << "; Adjustment Cap " << ctrl_config_.adjustmentCap); + // After we get a adjSpeedCmd, we apply three filters on it if the filter is enabled + // First: we do not allow the difference between command speed of the host vehicle and the leader's commandSpeed higher than adjustmentCap + + speed_cmd = adjSpeedCmd; + ROS_DEBUG_STREAM("A speed command is generated from command generator: " << speed_cmd << " m/s"); + + if(enableMaxAdjustmentFilter) + { + if(speed_cmd > leader.commandSpeed + ctrl_config_.adjustmentCap) { + speed_cmd = leader.commandSpeed + ctrl_config_.adjustmentCap; + } else if(speed_cmd < leader.commandSpeed - ctrl_config_.adjustmentCap) { + speed_cmd = leader.commandSpeed - ctrl_config_.adjustmentCap; + } + ROS_DEBUG_STREAM("The adjusted cmd speed after max adjustment cap is " << speed_cmd << " m/s"); + } + + } + + else if (leader.staticId == ctrl_config_.vehicleID) + { + ROS_DEBUG_STREAM("Host vehicle is the leader"); + speed_cmd = currentSpeed; + + if(enableMaxAdjustmentFilter) + { + if(speed_cmd > ctrl_config_.adjustmentCap) + { + speed_cmd = ctrl_config_.adjustmentCap; + } + + ROS_DEBUG_STREAM("The adjusted leader cmd speed after max adjustment cap is " << speed_cmd << " m/s"); + } + + pid_ctrl_.reset(); + } + + else + { + // If there is no leader available, the vehicle will stop. This means there is a mis-communication between platooning strategic and control plugins. + ROS_DEBUG_STREAM("There is no leader available"); + speed_cmd = 0.0; + pid_ctrl_.reset(); + } + + + + // Third: we allow do not a large gap between two consecutive speed commands + if(enableMaxAccelFilter) + { + double max = lastCmdSpeed + (ctrl_config_.maxAccel * (ctrl_config_.cmdTmestamp / 1000.0)); + double min = lastCmdSpeed - (ctrl_config_.maxAccel * (ctrl_config_.cmdTmestamp / 1000.0)); + if(speed_cmd > max) { + speed_cmd = max; + } else if (speed_cmd < min) { + speed_cmd = min; + } + lastCmdSpeed = speed_cmd; + ROS_DEBUG_STREAM("The speed command after max accel cap is: " << speed_cmd << " m/s"); + } + + speedCmd_ = speed_cmd; + + lastCmdSpeed = speedCmd_; + + } + + void PlatoonControlIHPWorker::generateSteer(const cav_msgs::TrajectoryPlanPoint& point) + { + pp_.current_pose_ = current_pose_; + pp_.velocity_ = currentSpeed; + + pp_.calculateSteer(point); + steerCmd_ = pp_.getSteeringAngle(); + angVelCmd_ = pp_.getAngularVelocity(); + } + + // TODO get the actual leader from strategic plugin + void PlatoonControlIHPWorker::setLeader(const PlatoonLeaderInfo& leader){ + platoon_leader = leader; + } + + void PlatoonControlIHPWorker::setCurrentSpeed(double speed){ + currentSpeed = speed; + + } + + + + +} diff --git a/platooning_control_ihp/src/pure_pursuit.cpp b/platooning_control_ihp/src/pure_pursuit.cpp new file mode 100644 index 0000000000..423f6c022d --- /dev/null +++ b/platooning_control_ihp/src/pure_pursuit.cpp @@ -0,0 +1,140 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "pure_pursuit.h" +#include +#include + + + +namespace platoon_control_ihp +{ + PurePursuit::PurePursuit(){} + + double PurePursuit::getLookaheadDist(const cav_msgs::TrajectoryPlanPoint& tp) const + { + double x_diff = (tp.x - current_pose_.position.x); + double y_diff = (tp.y - current_pose_.position.y); + double dist = std::sqrt(x_diff * x_diff + y_diff * y_diff); + ROS_DEBUG_STREAM("calculated lookahead: " << dist); + return dist; + } + + + double PurePursuit::getYaw(const cav_msgs::TrajectoryPlanPoint& tp) const + { + double yaw = atan2(tp.y - current_pose_.position.y, tp.x - current_pose_.position.x); + return yaw; + } + + int PurePursuit::getSteeringDirection(std::vector v1, std::vector v2) const + { + double corss_prod = v1[0]*v2[1] - v1[1]*v2[0]; + if (corss_prod >= 0.0){ + return -1; + } + return 1; + } + + double PurePursuit::getSteeringAngle() + { + return steering_angle_; + } + + double PurePursuit::getAngularVelocity() + { + return angular_velocity_; + } + + double PurePursuit::calculateKappa(const cav_msgs::TrajectoryPlanPoint& tp) + { + double lookahead = getLookaheadDist(tp); + ROS_DEBUG_STREAM("used lookahead: " << lookahead); + double alpha = getAlphaSin(tp, current_pose_); + ROS_DEBUG_STREAM("calculated alpha: " << alpha); + double kappa = 2 * (alpha)/(lookahead); + ROS_DEBUG_STREAM("calculated kappa: " << kappa); + return kappa; + } + + void PurePursuit::calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp) + { + + double kappa = calculateKappa(tp); + + double steering = atan(config_.wheelBase * kappa); + ROS_DEBUG_STREAM("calculated steering angle: " << steering); + double filtered_steering = lowPassfilter(config_.lowpassGain, prev_steering, steering); + ROS_DEBUG_STREAM("filtered steering: " << filtered_steering); + if (std::isnan(filtered_steering)) filtered_steering = prev_steering; + prev_steering = filtered_steering; + steering_angle_ = filtered_steering; + + double ang_vel = velocity_ * kappa; + ROS_DEBUG_STREAM("calculated angular velocity: " << ang_vel); + double filtered_ang_vel = lowPassfilter(config_.lowpassGain, prev_ang_vel, ang_vel); + ROS_DEBUG_STREAM("filtered angular velocity: " << filtered_ang_vel); + prev_ang_vel = filtered_ang_vel; + if (std::isnan(filtered_ang_vel)) filtered_ang_vel = prev_ang_vel; + angular_velocity_ = filtered_ang_vel; + } + + double PurePursuit::getAlphaSin(cav_msgs::TrajectoryPlanPoint tp, geometry_msgs::Pose current_pose) + { + tf::Transform inverse; + tf::poseMsgToTF(current_pose, inverse); + tf::Transform transform = inverse.inverse(); + + geometry_msgs::Point point_msg; + point_msg.x = tp.x; + point_msg.y = tp.y; + point_msg.z = current_pose.position.z; + tf::Point p; + pointMsgToTF(point_msg, p); + tf::Point tf_p = transform * p; + geometry_msgs::Point tf_point_msg; + pointTFToMsg(tf_p, tf_point_msg); + ROS_DEBUG_STREAM("relative latitude: " << tf_point_msg.y); + ROS_DEBUG_STREAM("relative longitude: " << tf_point_msg.x); + ROS_DEBUG_STREAM("relative z: " << tf_point_msg.z); + double vec_mag = std::sqrt(tf_point_msg.y*tf_point_msg.y + tf_point_msg.x*tf_point_msg.x + tf_point_msg.z*tf_point_msg.z); + ROS_DEBUG_STREAM("relative vector mag: " << vec_mag); + double sin_alpha = tf_point_msg.y/vec_mag; + ROS_DEBUG_STREAM("alpha sin from transform: " << sin_alpha); + + double angle_tp_map = atan2(tp.y, tp.x); // angle of vector to tp point in map frame + ROS_DEBUG_STREAM("angle_tp_map: " << angle_tp_map); + tf::Quaternion quat; + tf::quaternionMsgToTF(current_pose.orientation, quat); + double roll, pitch, yaw; + tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); + ROS_DEBUG_STREAM("yaw: " << yaw); + double alpha = angle_tp_map - yaw; + double sin_alpha2 = sin(alpha); + ROS_DEBUG_STREAM("alpha from orientation: " << alpha); + ROS_DEBUG_STREAM("alpha sin from orientation: " << sin_alpha2); + + return sin_alpha; + } + + double PurePursuit::lowPassfilter(double gain, double prev_value, double value) + { + value = prev_value + gain*(value - prev_value); + return value; + } +} diff --git a/route/launch/route.launch b/platooning_control_ihp/test/mynode.test old mode 100755 new mode 100644 similarity index 64% rename from route/launch/route.launch rename to platooning_control_ihp/test/mynode.test index 91eca328c4..fe0d1979e1 --- a/route/launch/route.launch +++ b/platooning_control_ihp/test/mynode.test @@ -1,4 +1,3 @@ - - - - - - - + + + diff --git a/platooning_control_ihp/test/test_control.cpp b/platooning_control_ihp/test/test_control.cpp new file mode 100644 index 0000000000..ba95cda4ee --- /dev/null +++ b/platooning_control_ihp/test/test_control.cpp @@ -0,0 +1,52 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "platoon_control_ihp.h" +#include +#include + + + +TEST(PlatoonControlIHPPluginTest, test2) +{ + cav_msgs::TrajectoryPlan tp; + cav_msgs::TrajectoryPlanPoint point1; + point1.x = 1.0; + point1.y = 1.0; + + + cav_msgs::TrajectoryPlanPoint point2; + point2.x = 10.0; + point2.y = 10.0; + + cav_msgs::TrajectoryPlanPoint point3; + point3.x = 20.0; + point3.y = 20.0; + + tp.trajectory_points = {point1, point2, point3}; + + + + platoon_control_ihp::PlatoonControlIHPPlugin pc; + pc.current_speed_ = 5; + cav_msgs::TrajectoryPlanPoint out = pc.getLookaheadTrajectoryPoint(tp); + EXPECT_EQ(out.x, 10.0); +} + + + diff --git a/platooning_control_ihp/test/test_main.cpp b/platooning_control_ihp/test/test_main.cpp new file mode 100644 index 0000000000..3bbdd41035 --- /dev/null +++ b/platooning_control_ihp/test/test_main.cpp @@ -0,0 +1,29 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "pid_controller.h" +#include +#include + + +// Run all the tests +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/platooning_control_ihp/test/test_mynode.cpp b/platooning_control_ihp/test/test_mynode.cpp new file mode 100644 index 0000000000..289a48d074 --- /dev/null +++ b/platooning_control_ihp/test/test_mynode.cpp @@ -0,0 +1,71 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include "platoon_control_ihp.h" + + + + +// Declare a test +TEST(TestSuite, testCase1) +{ + ros::NodeHandle nh = ros::NodeHandle(); + ros::Publisher traj_pub_ = nh.advertise("PlatooningControlPlugin/plan_trajectory", 5); + cav_msgs::TrajectoryPlan tp; + traj_pub_.publish(tp); + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + auto num = traj_pub_.getNumSubscribers(); + EXPECT_EQ(1, num); + +} + +TEST(TestSuite, testCase2) +{ + ros::NodeHandle nh = ros::NodeHandle(); + ros::Publisher twist_pub_ = nh.advertise("current_velocity", 5); + geometry_msgs::TwistStamped twist1; + twist1.twist.linear.x = 10.0; + twist_pub_.publish(twist1); + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + auto num = twist_pub_.getNumSubscribers(); + EXPECT_EQ(1, num); + +} + + + + +int main (int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_platoon_control"); + + // std::thread spinner([] {while (ros::ok()) ros::spin();}); + + auto res = RUN_ALL_TESTS(); + + // ros::shutdown(); + + return res; +} \ No newline at end of file diff --git a/platooning_control_ihp/test/test_pid.cpp b/platooning_control_ihp/test/test_pid.cpp new file mode 100644 index 0000000000..b46685f315 --- /dev/null +++ b/platooning_control_ihp/test/test_pid.cpp @@ -0,0 +1,55 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "pid_controller.h" +#include +#include + + +TEST(PIDControllerTest, test1) +{ + platoon_control_ihp::PIDController pid; + double res = pid.calculate(40, 38); + EXPECT_EQ(-9, res); +} + + +TEST(PIDControllerTest, test2) +{ + platoon_control_ihp::PIDController pid; + double res = pid.calculate(20, 300); + EXPECT_EQ(100, res); +} + +TEST(PIDControllerTest, test3) +{ + platoon_control_ihp::PIDController pid; + double res = pid.calculate(300, 20); + EXPECT_EQ(-100, res); +} + +TEST(PIDControllerTest, test4) +{ + platoon_control_ihp::PIDController pid; + pid.reset(); + double res = pid.calculate(200, 20); + double res2 = pid.calculate(500,25); + EXPECT_EQ(-100, res2); + double res3 = pid.calculate(25,500); + EXPECT_EQ(100, res3); +} diff --git a/platooning_control_ihp/test/test_pure.cpp b/platooning_control_ihp/test/test_pure.cpp new file mode 100644 index 0000000000..6e38e1623e --- /dev/null +++ b/platooning_control_ihp/test/test_pure.cpp @@ -0,0 +1,47 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "pure_pursuit.h" +#include +#include + +TEST(PurePursuitTest, test_filter) +{ + PlatooningControlIHPPluginConfig config; + config.lowpassGain = 0.5; + + platoon_control_ihp::PurePursuit pp; + pp.config_ = config; + double new_angle = pp.lowPassfilter(3.0, 0, config.lowpassGain); + EXPECT_EQ(1.5, new_angle); +} + +TEST(PurePursuitTest, test1) +{ + + cav_msgs::TrajectoryPlanPoint point; + point.x = 1.0; + point.y = 1.0; + point.target_time = ros::Time(1.0); + platoon_control_ihp::PurePursuit pp; + pp.calculateSteer(point); + EXPECT_EQ(0, pp.steering_angle_); + + +} + diff --git a/platooning_control_ihp/test/test_worker.cpp b/platooning_control_ihp/test/test_worker.cpp new file mode 100644 index 0000000000..15f173e5af --- /dev/null +++ b/platooning_control_ihp/test/test_worker.cpp @@ -0,0 +1,132 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "platoon_control_ihp_worker.h" +#include +#include + +TEST(PlatoonControlIHPWorkerTest, test1) +{ + platoon_control_ihp::PlatoonControlIHPWorker pcw; + cav_msgs::TrajectoryPlanPoint point; + point.x = 1.0; + point.y = 2.0; + pcw.generateSpeed(point); + EXPECT_NEAR(0, pcw.speedCmd_, 0.1); +} + +TEST(PlatoonControlIHPWorkerTest, test11) +{ + platoon_control_ihp::PlatoonLeaderInfo leader; + platoon_control_ihp::PlatoonControlIHPWorker pcw; + leader.staticId = ""; + leader.leaderIndex = 0; + leader.NumberOfVehicleInFront = 1; + pcw.setLeader(leader); + cav_msgs::TrajectoryPlanPoint point; + point.x = 30.0; + point.y = 20.0; + pcw.currentSpeed = 10.0; + pcw.lastCmdSpeed = 10; + pcw.generateSpeed(point); + EXPECT_NEAR(10.0, pcw.getLastSpeedCommand(), 0.5); +} + +TEST(PlatoonControlIHPWorkerTest, test2) +{ + + platoon_control_ihp::PlatoonControlIHPWorker pcw; + platoon_control_ihp::PlatoonLeaderInfo leader; + leader.commandSpeed = 10; + leader.vehicleSpeed = 10; + leader.vehiclePosition = 50; + leader.staticId = "id"; + leader.leaderIndex = 0; + leader.NumberOfVehicleInFront = 1; + pcw.setLeader(leader); + + cav_msgs::TrajectoryPlanPoint point; + point.x = 30.0; + point.y = 20.0; + pcw.currentSpeed = 10.0; + pcw.lastCmdSpeed = 10; + pcw.generateSpeed(point); + EXPECT_NEAR(9.75, pcw.getLastSpeedCommand(), 0.5); + + + cav_msgs::TrajectoryPlanPoint point2; + point2.x = 30.0; + point2.y = 40.0; + pcw.generateSpeed(point2); + EXPECT_NEAR(10, pcw.getLastSpeedCommand(), 0.5); + + cav_msgs::TrajectoryPlanPoint point3; + point3.x = 50.0; + point3.y = 60.0; + pcw.generateSpeed(point3); + EXPECT_NEAR(10.25, pcw.getLastSpeedCommand(), 0.5); + +} + +TEST(PlatoonControlIHPWorkerTest, test3) +{ + + platoon_control_ihp::PlatoonControlIHPWorker pcw; + platoon_control_ihp::PlatoonLeaderInfo leader; + leader.commandSpeed = 10; + leader.vehicleSpeed = 10; + leader.vehiclePosition = 50; + leader.staticId = "id"; + leader.leaderIndex = 0; + leader.NumberOfVehicleInFront = 2; + pcw.setLeader(leader); + + cav_msgs::TrajectoryPlanPoint point; + point.x = 30.0; + point.y = 15.0; + pcw.currentSpeed = 10.0; + pcw.lastCmdSpeed = 10; + pcw.generateSpeed(point); + EXPECT_NEAR(10.25, pcw.getLastSpeedCommand(), 0.5); + + + cav_msgs::TrajectoryPlanPoint point2; + point2.x = 50.0; + point2.y = 60.0; + pcw.platoon_leader.vehiclePosition = 51; + pcw.generateSpeed(point2); + EXPECT_NEAR(10.5, pcw.getLastSpeedCommand(), 0.5); + + cav_msgs::TrajectoryPlanPoint point3; + point3.x = 50.0; + point3.y = 60.0; + pcw.platoon_leader.vehiclePosition = 49; + pcw.generateSpeed(point3); + EXPECT_NEAR(10.25, pcw.getLastSpeedCommand(), 0.5); + + } + +TEST(PlatoonControlIHPWorkerTest, test_steer) +{ + platoon_control_ihp::PlatoonControlIHPWorker pcw; + cav_msgs::TrajectoryPlanPoint point; + point.x = 1.0; + point.y = 2.0; + pcw.generateSteer(point); + EXPECT_NEAR(0, pcw.steerCmd_, 0.1); +} diff --git a/platooning_strategic_IHP/CMakeLists.txt b/platooning_strategic_IHP/CMakeLists.txt new file mode 100644 index 0000000000..253a3a10a9 --- /dev/null +++ b/platooning_strategic_IHP/CMakeLists.txt @@ -0,0 +1,119 @@ +# Copyright (C) 2018-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 2.8.3) +project(platoon_strategic_ihp) + +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(1) +carma_package() + +## 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") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + carma_utils + cav_msgs + cav_srvs + roscpp + std_msgs + carma_wm + lanelet2_extension + tf + tf2 + tf2_ros + tf2_geometry_msgs +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED) + +################################### +## catkin specific configuration ## +################################### + + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs carma_wm cav_srvs lanelet2_extension +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +#file(GLOB_RECURSE headers */*.hpp */*.h) + +add_library(platoon_strategic_ihp_plugin_lib + src/platoon_strategic_ihp.cpp + src/platoon_manager_ihp.cpp) + +add_dependencies(platoon_strategic_ihp_plugin_lib ${catkin_EXPORTED_TARGETS}) +target_link_libraries(platoon_strategic_ihp_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_executable( ${PROJECT_NAME} + src/main_ihp.cpp) + +target_link_libraries(${PROJECT_NAME} platoon_strategic_ihp_plugin_lib) + + +############# +## Install ## +############# + + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Install C++ +install(TARGETS ${PROJECT_NAME} platoon_strategic_ihp_plugin_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + + +############# +## Testing ## +############# + +# catkin_add_gmock(${PROJECT_NAME}-test +# test/test_platoon_manager_front_join.cpp +# test/test_platoon_manager.cpp +# test/mobility_messages.cpp +# test/main_test.cpp) + +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test platoon_strategic_ihp_plugin_lib ${catkin_LIBRARIES}) +# endif() diff --git a/platooning_strategic_IHP/config/parameters.yaml b/platooning_strategic_IHP/config/parameters.yaml new file mode 100644 index 0000000000..e74ce5d41f --- /dev/null +++ b/platooning_strategic_IHP/config/parameters.yaml @@ -0,0 +1,34 @@ +maxPlatoonSize: 10 +algorithmType: 0 +mvr_duration: 15 +timeHeadway: 1.2 +standStillHeadway: 12.0 +maxAllowedJoinTimeGap: 15.0 +maxAllowedJoinGap: 200.0 +minAllowedJoinGap: 0.0 +desiredJoinTimeGap: 4.0 +desiredJoinGap: 300.0 +maxLeaderAbortingCalls: 2 +waitingStateTimeout: 25.0 +cmdSpeedMaxAdjustment: 10.0 +minAllowableHeadaway: 1.6 +headawayStableLowerBond: 1.7 +maxAllowableHeadaway: 4.0 +headawayStableUpperBond: 3.9 +minCutinGap: 18.0 +maxCutinGap: 150.0 +longitudinalCheckThresold: 100.0 +maxCrosstrackError: 1.5 +vehicleLength: 5.0 +ss_theta: 4.0 +standstill: 2.0 +inter_tau: 1.5 +intra_tau: 0.6 +gap_weight: 0.9 +test_front_join: false +test_cutin_join: false +slowDownAdjuster: 0.75 +createGapAdjuster: 0.3 +minPlatooningSpeed: 5.0 +allowCutinJoin: true +significantDTDchange: 0.2 diff --git a/platooning_strategic_IHP/include/platoon_config_ihp.h b/platooning_strategic_IHP/include/platoon_config_ihp.h new file mode 100644 index 0000000000..93cce96f1c --- /dev/null +++ b/platooning_strategic_IHP/include/platoon_config_ihp.h @@ -0,0 +1,121 @@ +#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 YieldPluginConfig + */ +struct PlatoonPluginConfig +{ + // following parameters are for general platooning plugin + double vehicleLength = 5.0; // m + int maxPlatoonSize = 10; // 1 + int algorithmType = 0; // N/A + int statusMessageInterval = 100; // ms + int infoMessageInterval = 200; // ms + double maneuver_plan_dt = 15; // s + // following parameters are for IHP gap regulation algorithm + double time_step = 15; // s + double epislon = 0.001; // m/s + + // following parameters are for platoon forming and operation + double timeHeadway = 2.0; // s + double standStillHeadway = 12.0; // m + double maxAllowedJoinTimeGap = 15.0; // s + double maxAllowedJoinGap = 90.0; // m + double minAllowedJoinGap = 5.0; // m + double longitudinalCheckThresold = 85.0; // m + double desiredJoinTimeGap = 4.0; // s + double desiredJoinGap = 30.0; // m + int maxLeaderAbortingCalls = 2; // counter + double waitingStateTimeout = 25.0; // s + double cmdSpeedMaxAdjustment = 10.0; // m/s + + // following parameters are mainly for APF leader selection + // UCLA: Rename the next four variables for better explainability. + double minAllowableHeadaway = 1.6; // s + double headawayStableLowerBond = 1.7; // s + double maxAllowableHeadaway = 4.0; // s + double headawayStableUpperBond = 3.9; // s + + double minCutinGap = 22.0; // m + double maxCutinGap = 22.0; // m + double maxCrosstrackError = 2.0; // m + + // Speed adjuster to slow down platoon memebr to create gap + double slowDownAdjuster = 0.75; // ratio + double createGapAdjuster = 0.3; // ratio + + std::string vehicleID = "default_id"; + + // min speed to start platooning negotiations + double minPlatooningSpeed = 7.0; // m/s + + // ---------------------- UCLA: parameters for IHP platoon trajectory regulation ---------------- + /** + * \brief Parameter sets for IHP platoon trajectory regulation algorithm. + * Please refer to the updated design doc for detailed parameter description. + */ + double ss_theta = 4.0; // Stanstill determining threshold, in m/s. + double standstill = 2.0; // Stanstill reaction time adjuster, in s. + double inter_tau = 1.5; // Inter-platoon time gap, refer to bumper to bumper gap time, in s. + double intra_tau = 0.6; // Intra-platoon time gao, refer to bumper to bumper gap time, in s. + double gap_weight = 0.9; // Weighted ratio for time-gap based calculation, unitless. + bool test_front_join = false; //Flag to enable/disable front join functionality with two vehicles. + // Flag can be set to true, to test front join functionality with two vehicles + // But in normal operating conditions it should be set to false + bool test_cutin_join = false; //Flag to enable/disable front/rear cutin join functionality with only 2 vehicles + //Normal operations it should be false, but true allows a cutin to a single-car platoon + //------------------------------------------------------------------------------------------------ + bool allowCutinJoin = true; //Flag to enable/disable cut-in joins + double significantDTDchange = 0.2; // Ratio of dtd that is considered a significant change and triggers a new sort of platoon vector + int join_index = -1; // target join index for cut-in join - used for testing purposes. -1: front cutin join, target_platoon.size()-1: cut-in rear + + + friend std::ostream& operator<<(std::ostream& output, const PlatoonPluginConfig& c) + { + output << "PlatoonPluginConfig { " << std::endl + << "maxPlatoonSize: " << c.maxPlatoonSize << std::endl + << "algorithmType: " << c.algorithmType << std::endl + << "statusMessageInterval: " << c.statusMessageInterval << std::endl + << "infoMessageInterval: " << c.infoMessageInterval << std::endl + << "timeHeadway: " << c.timeHeadway << std::endl + << "standStillHeadway: " << c.standStillHeadway << std::endl + << "maxAllowedJoinTimeGap: " << c.maxAllowedJoinTimeGap << std::endl + << "desiredJoinTimeGap: " << c.desiredJoinTimeGap << std::endl + << "desiredJoinGap: " << c.desiredJoinGap << std::endl + << "maxLeaderAbortingCalls: " << c.maxLeaderAbortingCalls << std::endl + << "waitingStateTimeout: " << c.waitingStateTimeout << std::endl + << "cmdSpeedMaxAdjustment: " << c.cmdSpeedMaxAdjustment << std::endl + << "minAllowableHeadaway: " << c.minAllowableHeadaway << std::endl + << "maxAllowableHeadaway: " << c.maxAllowableHeadaway << std::endl + << "headawayStableLowerBond: " << c.headawayStableLowerBond << std::endl + << "headawayStableUpperBond: " << c.headawayStableUpperBond << std::endl + << "minCutinGap: " << c.minCutinGap << std::endl + << "maxCutinGap: " << c.maxCutinGap << std::endl + << "maxCrosstrackError: " << c.maxCrosstrackError << std::endl + << "vehicleID: " << c.vehicleID << std::endl + << "minPlatooningSpeed: " << c.minPlatooningSpeed << std::endl + << "allowCutinJoin: " << c.allowCutinJoin << std::endl + << "significantDTDchange: " << c.significantDTDchange << std::endl + << "join_index: " << c.join_index << std::endl + << "}" << std::endl; + return output; + } +}; diff --git a/platooning_strategic_IHP/include/platoon_manager_ihp.h b/platooning_strategic_IHP/include/platoon_manager_ihp.h new file mode 100644 index 0000000000..cacb908892 --- /dev/null +++ b/platooning_strategic_IHP/include/platoon_manager_ihp.h @@ -0,0 +1,491 @@ +#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. + */ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "platoon_config_ihp.h" + +namespace platoon_strategic_ihp +{ + /** + * \brief Struct for an action plan, which describes a transient joining activity + */ + struct ActionPlan + { + bool valid; //is host currently negotiating/executing a join action? + unsigned long planStartTime; //time that plan was initiated + std::string planId; //ID of this action + std::string peerId; //vehicle ID of the candidtate joining vehicle or leader of platoon we are joining + + ActionPlan() : valid(false), planStartTime(0), planId(""), peerId("") {} + + ActionPlan(bool valid, unsigned long planStartTime, std::string planId, std::string peerId): + valid(valid), planStartTime(planStartTime), planId(planId), peerId(peerId) {} + }; + + /** + * \brief 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 + }; + + /** + * \brief Platoon States. UCLA: Added additional states + * (i.e., LEADERABORTING and CANDIDATELEADER) for same-lane front join. + * (i.e., LEADWITHOPERATION and PREPARETOJOIN) for cut-in front join. + */ + enum PlatoonState + { + STANDBY, // 0; + LEADERWAITING, // 1; + LEADER, // 2; + CANDIDATEFOLLOWER, // 3; + FOLLOWER, // 4; + //UCLA: FRONTAL JOIN STATE + LEADERABORTING, // 5; + //UCLA: FRONTAL JOIN STATE + CANDIDATELEADER, // 6; + //UCLA: CUT-IN JOIN STATE + LEADWITHOPERATION, // 7; + //UCLA: CUT-IN JOIN STATE + PREPARETOJOIN // 8; + }; + + /** + * \brief Platoon member info + */ + struct PlatoonMember + { + // Static ID is permanent ID for each vehicle + std::string staticId; + // Vehicle real time command speed in m/s. + double commandSpeed; + // Actual vehicle speed in m/s. + double vehicleSpeed; + // Vehicle current downtrack distance on the current route in m. + double vehiclePosition; + // Vehicle current crosstrack distance on the current route in m. + double vehicleCrossTrack; + // The local time stamp when the host vehicle update any informations of this member. + long timestamp; + + PlatoonMember(): staticId(""), commandSpeed(0.0), vehicleSpeed(0.0), vehiclePosition(0.0), vehicleCrossTrack(0.0), timestamp(0) {} + + PlatoonMember(std::string staticId, double commandSpeed, double vehicleSpeed, double vehiclePosition, double vehicleCrossTrack, long timestamp): staticId(staticId), + commandSpeed(commandSpeed), vehicleSpeed(vehicleSpeed), vehiclePosition(vehiclePosition), vehicleCrossTrack(vehicleCrossTrack), timestamp(timestamp) {} + }; + + + class PlatoonManager + { + public: + + /** + * \brief Default constructor + */ + PlatoonManager(); + + /** + * \brief Stores the latest info on location of the host vehicle. + * + * \param downtrack distance downtrack from beginning of route, m + * \param crosstrack distance crosstrack from roadway centerline, m + */ + void updateHostPose(const double downtrack, const double crosstrack); + + /** + * \brief Stores the latest info on host vehicle's command & actual speeds + * + * \param cmdSpeed current commanded speed, m/s + * \param actualSpeed current measured speed, m/s + */ + void updateHostSpeeds(const double cmdSpeed, const double actualSpeed); + + /** + * \brief Update information for members of the host's platoon based on a mobility operation STATUS message + * + * \param senderId static id of the broadcasting vehicle + * \param platoonId platoon id + * \param params message strategy parameters + * \param DtD downtrack distance along host's route, m + * \param CtD crosstrack distance in roadway at host's location, m + */ + void hostMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params, + const double& DtD, const double& CtD); + + /** + * \brief Update information for members of a neighboring platoon based on a mobility operation STATUS message + * + * \param senderId static id of the broadcasting vehicle + * \param platoonId platoon id + * \param params message strategy parameters + * \param DtD downtrack distance along host's route, m + * \param CtD crosstrack distance in roadway at host's location, m + */ + void neighborMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params, + const double& DtD, const double& CtD); + + /** + * \brief Updates the list of vehicles in the specified platoon, based on info available from a + * mobility operation STATUS message from one of that platoon's vehicles. It ensures the list of vehicles + * is properly sorted in order of appearance from front to rear in the platoon. If the host is in the + * platoon, it will update host info as well. + * + * \param platoon the list of vehicles in the platoon in question + * \param senderId vehicle ID that sent the current info + * \param cmdSpeed the commanded speed of the sending vehicle + * \param dtDistance the downtrack location (from beginning of host's route) of the sending vehicle, m + * \param ctDistance the crosstrack location (from center of roadway at host's current route location) of sending vehicle, m + * \param curSpeed the current actual speed of the sending vehicle, m/s + **/ + void updatesOrAddMemberInfo(std::vector& platoon, std::string senderId, double cmdSpeed, + double dtDistance, double ctDistance, double curSpeed); + + /** + * \brief Returns total size of the platoon , in number of vehicles. + */ + int getHostPlatoonSize(); + + /** + * \brief Resets necessary variables to indicate that the current ActionPlan is dead. + */ + void clearActionPlan(); + + /** + * \brief Resets all variables that might indicate other members of the host's platoon; sets the host back to solo vehicle. + */ + void resetHostPlatoon(); + + /** + * \brief Resets all variables that describe a neighbor platoon, so that no neighbor is known. + */ + void resetNeighborPlatoon(); + + /** + * \brief Removes a single member from the internal record of platoon members + * + * \param mem index of the member to be removed (zero-based) + * + * \return true if removal was successful, false otherwise + */ + bool removeMember(const size_t mem); + + /** + * \brief Removes a single member from the internal record of platoon members + * + * \param id the vehicle ID of the member to be removed + * + * \return true if removal was successful, false otherwise + */ + bool removeMemberById(const std::string id); + + /** + * \brief Returns dynamic leader of the host vehicle. + * + * \return The current dynamic leader as a vehcile object. + */ + PlatoonMember getDynamicLeader(); + + /** + * \brief 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(); + + /** + * \brief Update status when state change from Follower to Leader + */ + void changeFromFollowerToLeader(); + + /** + * \brief Update status when state change from Leader to Follower + * + * \param newPlatoonId new ID of the platoon + * \param newLeaderId ID of the new lead vehicle + */ + void changeFromLeaderToFollower(std::string newPlatoonId, std::string newLeaderId); + + /** + * \brief Get number of vehicles in front of host vehicle inside platoon + */ + int getNumberOfVehicleInFront(); + + /** + * \brief Returns overall length of the platoon. in m. + */ + double getCurrentPlatoonLength(); + + /** + * \brief Returns downtrack distance of the rear vehicle in platoon, in m. + */ + double getPlatoonRearDowntrackDistance(); + + /** + * \brief Returns distance to the predessecor vehicle, in m. + */ + double getDistanceToPredVehicle(); + + /** + * \brief Returns current speed, in m/s. + */ + double getCurrentSpeed() const; + + /** + * \brief Returns command speed. in m/s. + */ + double getCommandSpeed() const; + + /** + * \brief Returns current downtrack distance, in m. + */ + double getCurrentDowntrackDistance() const; + + /** + * \brief Returns current crosstrack distance, in m. + */ + double getCurrentCrosstrackDistance() const; + + /** + * \brief Returns current host static ID as a string. + */ + std::string getHostStaticID() const; + + /** + * \brief UCLA: Return the platoon leader downtrack distance, in m. + */ + double getPlatoonFrontDowntrackDistance(); + + /** + * \brief UCLA: Return the time headway summation of all predecessors, in s. + */ + double getPredecessorTimeHeadwaySum(); + + /** + * \brief UCLA: Return the speed of the preceding vehicle, in m/s. + */ + double getPredecessorSpeed(); + + /** + * \brief UCLA: Return the position of the preceding vehicle, in m. + */ + double getPredecessorPosition(); + + /** + * \brief UCLA: Return follower's desired position (i.e., downtrack, in m) that maintains the desired + * intra-platoon time gap, based on IHP platoon trajectory regulation algorithm. + * + * \params dt: The planning time step, in s. + * + * \return: Host vehicle's desired position as downtrack distance, in m. + */ + double getIHPDesPosFollower(double dt); + + /** + * \brief UCLA: Return joiner's desired position in terms of target platoon index to cut into the platoon. + * CAUTION: ASSUMES that the neighbor platoon info is fully populated! + * + * \param joinerDtD: The current downtrack distance (with regards to host vehicle) of the joiner vehicle. + * + * \return: cut-in index: The index of the gap-leading vehicle within the platoon. If front join, return -1. + */ + int getClosestIndex(double joinerDtD); + + /** + * \brief UCLA: Return the current actual gap size in the target platoon for cut-in join, in m. + * Note: The origin of the vehicle (for downtrack distance calculation) is located at the rear axle. + * CAUTION: ASSUMES that the neighbor platoon info is fully populated! + * + * \param gap_leading_index: The platoon index of the gap-leading vehicle. + * \param joinerDtD: The current downtrack distance (with regards to host vehicle route) of the joiner vehicle. + * + * \return: cut-in gap: The desired gap size for cut-in join, in m. + */ + double getCutInGap(const int gap_leading_index, const double joinerDtD); + + const std::string dummyID = "00000000-0000-0000-0000-000000000000"; + + // List of members in the host's own platoon (host will always be represented, so size is never zero) + std::vector host_platoon_; + + // Platoon ID of the host's platoon + std::string currentPlatoonID = dummyID; //dummy indicates not part of a platoon + + // Vehicle ID of the host's platoon leader (host may be the leader) + std::string platoonLeaderID = dummyID; //dummy indicates not part of a platoon + + // Current platooning state of the host vehicle + PlatoonState current_platoon_state = PlatoonState::STANDBY; + + //index to the host_platoon_ vector that represents the host vehicle + size_t hostPosInPlatoon_ = 0; + + // Plan that represents a joining activity only, it is NOT the ID of the platoon itself + ActionPlan current_plan = ActionPlan(); + + // Is the host a follower in its platoon? + bool isFollower = false; + + // List of members in a detected neighbor platoon, which may be empty + // CAUTION: we can only represent one neighbor platoon in this version, so if multiple platoons are nearby, + // code will get very confused and results are unpredictable. + std::vector neighbor_platoon_; + + // Num vehicles in the neighbor platoon, as indicated by the size field in the INFO message + size_t neighbor_platoon_info_size_ = 0; + + // Platoon ID of the neighboring platoon + std::string targetPlatoonID = dummyID; //ID of a real platoon that we may be attempting to join + + std::string neighborPlatoonID = dummyID; //ID of the neighbor platoon that we may be attempting to join (dummy if neighbor is a solo vehicle) + + // Vehicle ID of the neighbor platoon's leader + std::string neighbor_platoon_leader_id_ = dummyID; //dummy indicates unknown + + // Is the record of neighbor platoon members complete (does it contain a record for every member)? + bool is_neighbor_record_complete_ = false; + + // Current vehicle pose in map + geometry_msgs::PoseStamped pose_msg_; + + // host vehicle's static ID + std::string HostMobilityId = "default_host_id"; + + // UCLA: add indicator of creating gap + bool isCreateGap = false; + + size_t dynamic_leader_index_ = 0; + + private: + + // local copy of configuration file + PlatoonPluginConfig config_; + + std::string OPERATION_INFO_TYPE = "INFO"; + std::string OPERATION_STATUS_TYPE = "STATUS"; + std::string JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"; + // UCLA: add params for frontal join + std::string JOIN_FROM_FRONT_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"; + std::string MOBILITY_STRATEGY = "Carma/Platooning"; + + double minCutinGap_ = 22.0; // m + double maxCutinGap_ = 32.0; // m + std::string previousFunctionalDynamicLeaderID_ = ""; + int previousFunctionalDynamicLeaderIndex_ = -1; + + // note: APF related parameters are in config.h. + + double vehicleLength_ = 5.0; // the length of the vehicle, in m. + double gapWithPred_ = 0.0; // time headway with predecessor, in s. + double downtrack_progress_ = 0; // current downtrack distance, in m. + + // Note: Parameters for IHP platoon trajectory regulation are in config.h. + + std::string algorithmType_ = "APF_ALGORITHM"; // a string that defines the current algorithm to determine the dynamic leader. + + /** + * \brief Check the gap with the predecessor vehicle. + * + * \param distanceToPredVehicle: The distance between the host to the predecessor vehicle. + * + * \return (bool) if the predecessor is to close. + */ + bool insufficientGapWithPredecessor(double distanceToPredVehicle); + + /** + * \brief Calculate the time headaway of each platoon member and save as a vector. + * + * \param downtrackDistance: a vector containing the downtrack distances of all members. + * \param speed: a vector containing the speeds of all members. + * + * \return A vector containing the time headaway of all platoon members, each headway is in s. + */ + std::vector calculateTimeHeadway(std::vector downtrackDistance, std::vector speed) const; + + /** + * \brief Determine the proper vehicle to follow based the time headway of each member. + * Note that the host will always choose the closest violator (i.e. time headaway too small or too large) to follow. + * + * \param timeHeadways: A vector containing the time headaway of all platoon members. + * + * \return An index indicating the proper vehicle to follow (i.e., leader). If choose to follow platoon leader, return 0. + */ + int determineDynamicLeaderBasedOnViolation(std::vector timeHeadways); + + /** + * \brief Find the closest vehicle to the host vehicle that violates the (time headaway) lower boundary condition. + * + * \param timeHeadways: A vector containing the time headaway of all platoon members. + * + * \return An index indicating the closest violating vehicle. If no violator, return -1. + */ + int findLowerBoundaryViolationClosestToTheHostVehicle(std::vector timeHeadways) const; + + /** + * \brief Find the closest vehicle to the host vehicle that violates the (time headaway) maximum spacing condition. + * + * \param timeHeadways: A vector containing the time headaway of all platoon members. + * + * \return An index indicating the closest violating vehicle. If no violator, return -1. + */ + int findMaximumSpacingViolationClosestToTheHostVehicle(std::vector timeHeadways) const; + + /** + * \brief Return a sub-vector of the platoon-wise time headaways vector that start with a given index. + * + * \param timeHeadways: A vector containing the time headaway of all platoon members. + * start: An integer indicates the starting position. + * + * \return An sub-vector start with given index. + */ + std::vector getTimeHeadwayFromIndex(std::vector timeHeadways, int start) const; + }; +} \ No newline at end of file diff --git a/platooning_strategic_IHP/include/platoon_strategic_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp.h new file mode 100644 index 0000000000..da4f45329f --- /dev/null +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp.h @@ -0,0 +1,869 @@ +/* + * Copyright (C) 2019-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. + */ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + +#pragma once + +#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 "platoon_config_ihp.h" +#include +#include +#include +#include +#include +#include +#include + +namespace platoon_strategic_ihp +{ + using PublishPluginDiscoveryCB = std::function; + using MobilityResponseCB = std::function; + using MobilityRequestCB = std::function; + using MobilityOperationCB = std::function; + using PlatooningInfoCB = std::function; + + class PlatoonStrategicIHPPlugin + { + public: + + /** + * \brief Default constructor for PlatoonStrategicIHPPlugin class + */ + PlatoonStrategicIHPPlugin(); + + /** + * \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 + */ + PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, + PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, + MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, + PlatooningInfoCB platooning_info_publisher); + + /** + * \brief Callback function for Mobility Operation Message + * + * \param msg Mobility Operation Message + */ + void mob_op_cb(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Callback function for Mobility Request Message + * + * \param msg Mobility Request Message + */ + void mob_req_cb(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Callback function for Mobility Response Message + * + * \param msg Mobility Response Message + */ + void mob_resp_cb(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to the process and respond to the mobility request + * + * \param msg Mobility Request Message + * + * \return Mobility response message + */ + MobilityRequestResponse handle_mob_req(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Callback function to the maneuver request + * + * \param req Maneuver service request + * \param resp Maneuver service response + * + * \return Mobility response message + */ + bool plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp); + + /** + * \brief Find lanelet index from path + * + * \param path path + * \param target_id target lanelet id + * + * \return lanelet index + */ + int findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path); + + /** + * \brief Find lanelet width from local position + * + * \return lanelet index + */ + double findLaneWidth(); + + /** + * \brief Find lanelet index from path + * + * \param current_dist: current downtrack distance (m) + * \param end_dist: ending downtrack distance (m) + * \param current_speed: current speed (m/s) + * \param target_speed: target speed (m/s) + * \param lane_id: lanelet id + * \param current_time: current time (s) + * + * \return Maneuver message + */ + cav_msgs::Maneuver composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time); + + /** + * \brief Find start(current) and target(end) lanelet index from path to generate lane change maneuver message. + * + * \param current_dist: current downtrack distance + * \param end_dist: ending downtrack distance + * \param current_speed: current speed + * \param target_speed: target speed + * \param starting_lane_id: current lanelet id which serves as the starting lanlet id + * \param ending_lane_id: target lanelet id which is also the ending lanelet id that is in another lane + * \param current_time: current time in seconds + * + * \return Maneuver message + */ + cav_msgs::Maneuver composeLaneChangeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int starting_lane_id, int ending_lane_id, ros::Time& current_time); + + /** + * \brief Update maneuver status based on prior plan + * + * \param maneuver maneuver + * \param speed speed + * \param current_progress current progress + * \param lane_id lanelet ud + */ + void updateCurrentStatus(cav_msgs::Maneuver maneuver, double& speed, double& current_progress, int& lane_id); + + /** + * \brief Callback function for current pose + * + * \param msg PoseStamped msg + */ + void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); + + /** + * \brief Compose Platoon information message + * + * \return PlatooningInfo msg + + * Note: There is a difference between the "platoon info status" versus the the "platoon strategic plugin states". + * + * [platooning info status] + * The "platooning info status" reflect the overall operating status. The status can + * include vehicles in different "platoon strategic plugin states" as long as the current state is relavent + * to the genral status. + * + * [platoon strategic plugin states] + * The "platoon strategic plugin states" manage the negotiation strategies and + * vehicle communication in a more refined manner that can achieve the "platooning info status'" objective. Hence, + * vehicles in different states will behave differently based on the corresponding roles and predefined rules. + * However, they can belong to the same "platooning info statu" for the same operation purpose. + * + * [Example] + * Multiple strategic states, such as "leader aborting" and "candidate follower", can both be mapped to + * "connecting to new leader" platooning info status, as both states are serving the same purpose of connecting + * to the leader. + * + */ + cav_msgs::PlatooningInfo composePlatoonInfoMsg(); + + /** + * \brief Callback for the twist subscriber, which will store latest twist locally + * \param msg Latest twist message + */ + void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); + + /** + * \brief Callback for the control command + * \param msg Latest twist cmd message + */ + void cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg); + + /** + * \brief Callback for the georeference + * \param msg Latest georeference + */ + void georeference_cb(const std_msgs::StringConstPtr& msg); + + /** + * \brief Spin callback function + */ + bool onSpin(); + + /** + * \brief Run Leader State + */ + void run_leader(); + + /** + * \brief Run Leader Waiting State + */ + void run_leader_waiting(); + + /** + * \brief Run Candidate Follower State + */ + void run_candidate_follower(); + + /** + * \brief Run Follower State + */ + void run_follower(); + + // ECEF position of the host vehicle + cav_msgs::LocationECEF pose_ecef_point_; + + // -------------- UCLA: add two states for frontal join ------------------ + /** + * \brief Run Leader Aborting State + */ + void run_leader_aborting(); + + /** + * \brief Run Candidate Leader State + */ + void run_candidate_leader(); + + // -------------- UCLA: add two states for cut-in join ------------------ + /** + * \brief UCLA Run lead with operation State + */ + void run_lead_with_operation(); + + /** + * \brief UCLA Run prepare to join State + */ + void run_prepare_to_join(); + + /** + * \brief UCLA Update the private variable pose_ecef_point_ + */ + void setHostECEF(cav_msgs::LocationECEF pose_ecef_point); + + /** + * \brief UCLA Getter: for PlatoonManager class + */ + PlatoonManager getHostPM(); + + /** + * \brief UCLA Setter: function to set pm_.platoon_state + */ + void setPMState(PlatoonState desiredState); + + /** + * \brief UCLA Setter: Update platoon list (Unit Test). + */ + void updatePlatoonList(std::vector platoon_list); + + /** + * \brief UCLA Setter: Set the host to follower state (Unit Test). + */ + void setToFollower(); + + private: + + PublishPluginDiscoveryCB plugin_discovery_publisher_; + MobilityRequestCB mobility_request_publisher_; + MobilityResponseCB mobility_response_publisher_; + MobilityOperationCB mobility_operation_publisher_; + PlatooningInfoCB platooning_info_publisher_; + + // Platoon Manager Object + PlatoonManager pm_; + + + // wm listener pointer and pointer to the actual wm object + std::shared_ptr wml_; + carma_wm::WorldModelConstPtr wm_; + + // local copy of configuration file + PlatoonPluginConfig config_; + + // local copy of pose + // Current vehicle pose in map + geometry_msgs::PoseStamped pose_msg_; + + //Internal Variables used in unit tests + // Current vehicle command speed, m/s + double cmd_speed_ = 0; + // Current vehicle measured speed, m/s + double current_speed_ = 0; + // Current vehicle downtrack distance in route, m + double current_downtrack_ = 0; + // Current vehicle crosstrack distance in route, m + double current_crosstrack_ = 0; + // start time of leaderwaiting state, s + long waitingStartTime = 0; + // start time of candidate follower state, s + long candidatestateStartTime = 0; + + // Host Mobility ID + std::string HostMobilityId = "hostid"; + + // ROS Publishers + ros::Publisher platoon_strategic_ihp_plugin_discovery_pub_; + ros::Publisher mob_op_pub_; + ros::Publisher mob_req_pub_; + + // ROS Subscribers + ros::Subscriber pose_sub_; + ros::Subscriber mob_req_sub_; + ros::Subscriber mob_resp_sub_; + ros::Subscriber mob_op_sub_; + + + /** + * \brief Function to determine if a target vehicle is in the front of host vehicle + * note: This is only applicable for same lane application, so it will check cross track distance. + * + * \param downtrack target vehicle downtrack distance relative to host's route (m) + * crosstrack: target vehicle crosstrack (m) + * + * \return true or false + */ + bool isVehicleRightInFront(double downtrack, double crosstrack); + + /** + * \brief Function to find speed limit of a lanelet + * + * \param llt inout lanelet + * + * \return speed limit value (m/s) + */ + double findSpeedLimit(const lanelet::ConstLanelet& llt); + + /** + * \brief Function to process mobility request in leader state + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_leader(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in leader waiting state + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in follower state + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_follower(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in candidate follower state + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in standby state + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_standby(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility response in leader state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in leader waiting state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in follower state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in candidate follower state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in standby state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility operation in leader state + * + * \param msg incoming mobility operation + */ + void mob_op_cb_leader(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in leader waiting state + * + * \param msg incoming mobility operation + */ + void mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in follower state + * + * \param msg incoming mobility operation + */ + void mob_op_cb_follower(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in candidate follower state + * + * \param msg incoming mobility operation + */ + void mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in standby state + * + * \param msg incoming mobility operation + */ + void mob_op_cb_standby(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to compose mobility operation in leader state + * + * \param type type of mobility operation (info or status) + * + * \return mobility operation msg + */ + cav_msgs::MobilityOperation composeMobilityOperationLeader(const std::string& type); + + /** + * \brief Function to compose mobility operation in follower state + * + * \return mobility operation msg + */ + cav_msgs::MobilityOperation composeMobilityOperationFollower(); + + /** + * \brief Function to compose mobility operation in leader waiting state + * + * \return mobility operation msg + */ + cav_msgs::MobilityOperation composeMobilityOperationLeaderWaiting(); + + /** + * \brief Function to compose mobility operation in candidate follower state + * + * \return mobility operation msg + */ + cav_msgs::MobilityOperation composeMobilityOperationCandidateFollower(); + + /** + * \brief Function to convert pose from map frame to ecef location + * + * \param pose_msg pose message + * + * \return mobility operation msg + */ + cav_msgs::LocationECEF pose_to_ecef(geometry_msgs::PoseStamped pose_msg); + + /** + * \brief Function to convert ecef location to a 2d point in map frame + * + * \param ecef_point ecef location point + * + * \return 2d point in map frame + */ + lanelet::BasicPoint2d ecef_to_map_point(cav_msgs::LocationECEF ecef_point); + + // -------------- UCLA implemented functions ----------------------- + + // ----- 0. Extract data ------- + + /** + * \brief Function to determine if the given downtrack distance (m) is behind the host vehicle. + * + * note: This is only applicable for same lane application, so it will check cross track distance. + * + * \param downtrack: The downtrack distance (m) of the target vehicle to compare with the host. + * crosstrack: The crosstrack distance (m) of the target vehicle to compart with the host. + * + * \return (boolean): if target vehicle is behind the host vehicle. + */ + bool isVehicleRightBehind(double downtrack, double crosstrack); + + /** + * \brief The method for platoon leader to determine if the joining vehicle is closeby. (Note: Used when host vehicle is the platoon leader) + * + * + * \param joining_downtrack: The downtrack distance of the joining vehicle. + * joining_crosstrack: The crosstrack distance of the joining vehicle. + * + * + * \return (boolean): if the host vehicle is close to the target platoon. + */ + bool isJoiningVehicleNearPlatoon(double joining_downtrack, double joining_crosstrack); + + /** + * \brief Function to determine if the host vehicle is close to the target platoon (used for cut-in join scenarios). + * + * + * \param rearVehicleDtd: The downtrack of the neighbor platoon rear vehicle. + * \param frontVehicleDtd: The downtrack of the neighbor platoon leader. + * \param frontVehicleCtd: The crosstrack of the neighbor platoon leader. + * + * \return (boolean): if the host vehicle is close to the target platoon. + */ + bool isVehicleNearTargetPlatoon(double rearVehicleDtd, double frontVehicleDtd, double frontVehicleCtd); + + /** + * \brief Function to find the starting and ending lanelet ID for lane change in a two-lane scenario (used for cut-in join scenarios). + * + * Note: This is a temporary function for internal test only. The scenario is not genrealized. Can only find adjacebt lanletID based on predefiend direction (left, right). + * + * \TODO: This function should be replaced by the complete arbitrary lane change module. + * + * \param start_downtrack: The downtrack distance (m) of the starting point. + * end_downtrack: The downtrack distance (m) of the target (end) point. + * + * \return (int): The adjacent laneletID based on the provided dontrack distance range. + */ + int find_target_lanelet_id(double start_downtrack, double end_downtrack); + + // ----- 1. compose message --------- + + /** + * \brief Function to compose mobility operation message with STATUS params. + * + * \return mobility operation msg. + */ + cav_msgs::MobilityOperation composeMobilityOperationSTATUS(); + + /** + * \brief Function to compose mobility operation message with INFO params. + * + * \return mobility operation msg. + */ + cav_msgs::MobilityOperation composeMobilityOperationINFO(); + + /** + * \brief Function to compose mobility operation in LeaderAborting state. + * + * \return mobility operation msg. + */ + cav_msgs::MobilityOperation composeMobilityOperationLeaderAborting(); + + /** + * \brief Function to compose mobility operation in CandidateLeader. + * + * \return mobility operation msg. + */ + cav_msgs::MobilityOperation composeMobilityOperationCandidateLeader(); + + /** + * \brief Function to compose mobility operation in LeadWithOperation (cut-in join) + * + * \param type type of mobility operation (info or status) + * + * \return msg: mobility operation msg + * Return null ("") if the incoming message is trashed/unrecognized. + */ + cav_msgs::MobilityOperation composeMobilityOperationLeadWithOperation(const std::string& type); + + /** + * \brief Function to compose mobility operation in PrepareToJoin (cut-in join) + * + * \param type type of mobility operation (info or status) + * + * \return msg: mobility operation msg + * Return null ("") if the incoming message is trashed/unrecognized. + */ + cav_msgs::MobilityOperation composeMobilityOperationPrepareToJoin(); + + // --------- 2. Mobility operation callback ----------- + + /** + * \brief Function to process mobility operation message with STATUS params, + * read ecef location and update platoon member info. + * + * \param msg incoming mobility operation message. + */ + void mob_op_cb_STATUS(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation INFO params to find platoon length in m. + * + * \param strategyParams The parsed strategy params, used to find ecef locaton. + * + * \return The length of the platoon in m. + */ + double mob_op_find_platoon_length_from_INFO_params(std::string strategyParams); + + /** + * \brief Function to process mobility operation INFO params to find platoon leader's ecef location. + * + * \param strategyParams The parsed strategy params, used to find ecef locaton. + * + * \return ecef location of the sender. + */ + cav_msgs::LocationECEF mob_op_find_ecef_from_INFO_params(std::string strategyParams); + + /** + * \brief Function to process mobility operation for STATUS params. + * + * \param strategyParams The parsed strategy params, used to find ecef locaton. + * + * \return ecef location of the sender. + */ + cav_msgs::LocationECEF mob_op_find_ecef_from_STATUS_params(std::string strategyParams); + + /** + * \brief Function to process mobility operation in leaderaborting state. + * + * \param msg incoming mobility operation message. + */ + void mob_op_cb_leaderaborting(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in candidateleader state. + * + * \param msg incoming mobility operation message. + */ + void mob_op_cb_candidateleader(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in LeadWithOperation state (cut-in join) + * + * \param msg incoming mobility operation + */ + void mob_op_cb_leadwithoperation(const cav_msgs::MobilityOperation& msg); + + /** + * \brief Function to process mobility operation in PrepareToJoin state (cut-in join) + * + * \param msg incoming mobility operation + */ + void mob_op_cb_preparetojoin(const cav_msgs::MobilityOperation& msg); + + //------- 3. Mobility request callback ----------- + + /** + * \brief Function to process mobility request in leaderaborting state. + * + * \param msg incoming mobility request. + * + * \return ACK, NACK, or No response. + */ + MobilityRequestResponse mob_req_cb_leaderaborting(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in candidateleader state. + * + * \param msg incoming mobility request. + * + * \return ACK, NACK, or No response. + */ + MobilityRequestResponse mob_req_cb_candidateleader(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in leadwithoperation state (cut-in join) + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_leadwithoperation(const cav_msgs::MobilityRequest& msg); + + /** + * \brief Function to process mobility request in preparetojoin state (cut-in join) + * + * \param msg incoming mobility request + * + * \return ACK, NACK, or No response + */ + MobilityRequestResponse mob_req_cb_preparetojoin(const cav_msgs::MobilityRequest& msg); + + // ------ 4. Mobility response callback ------ + /** + * \brief Function to process mobility response in leaderaborting state. + * + * \param msg incoming mobility response. + */ + void mob_resp_cb_leaderaborting(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in candidateleader state. + * + * \param msg incoming mobility response. + */ + void mob_resp_cb_candidateleader(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in leadwithoperation state (cut-in join) + * + * \param msg incoming mobility response + */ + void mob_resp_cb_leadwithoperation(const cav_msgs::MobilityResponse& msg); + + /** + * \brief Function to process mobility response in preparetojoin state + * + * \param msg incoming mobility response + */ + void mob_resp_cb_preparetojoin(const cav_msgs::MobilityResponse& msg); + + + // Pointer for map projector + std::shared_ptr map_projector_; + + // flag to check if map is loaded + bool map_loaded_ = false; + + // ros service servers + ros::ServiceServer maneuver_srv_; + + // Plugin discovery message + cav_msgs::Plugin plugin_discovery_msg_; + + // Number of calls to the run_leader_aborting() method + int numLeaderAbortingCalls_ = 0; + + // Number of calls to run_candidate_follower() method after MobReq message sent + int candidate_follower_delay_count_ = 0; + + double maxAllowedJoinTimeGap_ = 15.0; + double maxAllowedJoinGap_ = 90; + int maxPlatoonSize_ = 10; + double vehicleLength_ = 5.0; + unsigned long infoMessageInterval_ = 200; // ms + unsigned long prevHeartBeatTime_ = 0.0; + int statusMessageInterval_ = 100; // ms + unsigned long NEGOTIATION_TIMEOUT = 15000; // ms + unsigned long LANE_CHANGE_TIMEOUT = 300000; // ms (5 min) + int noLeaderUpdatesCounter = 0; + int LEADER_TIMEOUT_COUNTER_LIMIT = 5; + double waitingStateTimeout = 25.0; // s + double desiredJoinGap = 30.0; // m + double desiredJoinTimeGap = 4.0; // s + + lanelet::BasicPoint2d target_cutin_pose_; + + // Speed below which platooning will not be attempted; non-zero value allows for sensor noise + const double STOPPED_SPEED = 0.5; // m/s + + // UCLA: add member variable for state prepare to join (default -2, front join -1, mid/rear join other integer) + int target_join_index_ = -2; + + // Is there a sufficient gap open in the platoon for a cut-in join? + bool safeToLaneChange_ = false; + + // Strategy types + const std::string PLATOONING_STRATEGY = "Carma/Platooning"; + const std::string OPERATION_INFO_TYPE = "INFO"; + const std::string OPERATION_STATUS_TYPE = "STATUS"; + + // UCLA: edit INFO params to store platoon front info (front join needs 10 info params) + /** + * index = 0, LENGTH, physical length of the platoon, in m. + * index = 1, SPEED, in m/s. + * index = 2, SIZE, number of members. + * index = 3, ECEFX, in cm. + * index = 4, ECEFY, in cm. + * index = 5, ECEFZ, in cm. + */ + const std::string OPERATION_INFO_PARAMS = "INFO|LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f"; + + /** + * index = 0, CMDSPEED, in m/s. + * index = 1, SPEED, in m/s. + * index = 2, ECEFX, in cm. + * index = 3, ECEFY, in cm. + * index = 4, ECEFZ, in cm. + */ + const std::string OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%"; + + // JOIN Strategy Params + /** + * index = 0, SIZE, number of members. + * index = 1, SPEED, in m/s. + * index = 2, ECEFX, in cm. + * index = 3, ECEFY, in cm. + * index = 4, ECEFZ, in cm. + * index = 5, JOINIDX, index of the position in platoon the incoming vehicle tries to join + */ + /** note: The cut-in index is zero-based and points to the gap-leading vehicle's index. + * eg: for rear join, cut-in index == platoon.size()-1; + * for join from front, index == -1; + * for cut-in in middle, index indicate the gap leading vehicle's index. + */ + const std::string JOIN_PARAMS = "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%"; + }; +} diff --git a/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h new file mode 100644 index 0000000000..ac24ba36cb --- /dev/null +++ b/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h @@ -0,0 +1,125 @@ +#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. + */ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "platoon_strategic_ihp.h" +#include "platoon_config_ihp.h" + +namespace platoon_strategic_ihp +{ +/** + * \brief ROS node for the YieldPlugin + */ +class PlatoonStrategicIHPPluginNode +{ +public: + + /** + * \brief Entrypoint for this node + */ + void run() + { + ros::CARMANodeHandle nh; + ros::CARMANodeHandle pnh("~"); + + carma_wm::WMListener wml; + auto wm_ = wml.getWorldModel(); + + ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); + ros::Publisher mob_response_pub = nh.advertise("outgoing_mobility_response", 1); + ros::Publisher mob_request_pub = nh.advertise("outgoing_mobility_request", 1); + ros::Publisher mob_operation_pub = nh.advertise("outgoing_mobility_operation", 1); + ros::Publisher platoon_info_pub = nh.advertise("platoon_info", 1); + + + PlatoonPluginConfig config; + + pnh.param("maxPlatoonSize", config.maxPlatoonSize, config.maxPlatoonSize); + pnh.param("algorithmType", config.algorithmType, config.algorithmType); + pnh.param("statusMessageInterval", config.statusMessageInterval, config.statusMessageInterval); + pnh.param("infoMessageInterval", config.infoMessageInterval, config.infoMessageInterval); + pnh.param("timeHeadway", config.timeHeadway, config.timeHeadway); + pnh.param("standStillHeadway", config.standStillHeadway, config.standStillHeadway); + pnh.param("maxAllowedJoinTimeGap", config.maxAllowedJoinTimeGap, config.maxAllowedJoinTimeGap); + pnh.param("maxAllowedJoinGap", config.maxAllowedJoinGap, config.maxAllowedJoinGap); + pnh.param("minAllowedJoinGap", config.minAllowedJoinGap, config.minAllowedJoinGap); + pnh.param("desiredJoinTimeGap", config.desiredJoinTimeGap, config.desiredJoinTimeGap); + pnh.param("desiredJoinGap", config.desiredJoinGap, config.desiredJoinGap); + pnh.param("waitingStateTimeout", config.waitingStateTimeout, config.waitingStateTimeout); + pnh.param("cmdSpeedMaxAdjustment", config.cmdSpeedMaxAdjustment, config.cmdSpeedMaxAdjustment); + pnh.param("minAllowableHeadaway", config.minAllowableHeadaway, config.minAllowableHeadaway); + pnh.param("maxAllowableHeadaway", config.maxAllowableHeadaway, config.maxAllowableHeadaway); + pnh.param("headawayStableLowerBond", config.headawayStableLowerBond, config.headawayStableLowerBond); + pnh.param("headawayStableUpperBond", config.headawayStableUpperBond, config.headawayStableUpperBond); + pnh.param("minCutinGap", config.minCutinGap, config.minCutinGap); + pnh.param("maxCutinGap", config.maxCutinGap, config.maxCutinGap); + pnh.param("maxCrosstrackError", config.maxCrosstrackError, config.maxCrosstrackError); + pnh.param("test_front_join", config.test_front_join, config.test_front_join); + pnh.param("minPlatooningSpeed", config.minPlatooningSpeed, config.minPlatooningSpeed); + pnh.param("allowCutinJoin", config.allowCutinJoin, config.allowCutinJoin); + pnh.param("significantDTDchange", config.significantDTDchange, config.significantDTDchange); + pnh.param("longitudinalCheckThresold", config.longitudinalCheckThresold, config.longitudinalCheckThresold); + pnh.param("test_cutin_join", config.test_cutin_join, config.test_cutin_join); + pnh.param("join_index", config.join_index, config.join_index); + pnh.getParam("/vehicle_length", config.vehicleLength); + pnh.getParam("/vehicle_id", config.vehicleID); + + ROS_INFO_STREAM("PlatoonPluginConfig Params" << config); + + PlatoonStrategicIHPPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }, [&mob_response_pub](auto msg) { mob_response_pub.publish(msg); }, + [&mob_request_pub](auto msg) { mob_request_pub.publish(msg); }, [&mob_operation_pub](auto msg) { mob_operation_pub.publish(msg); }, + [&platoon_info_pub](auto msg) { platoon_info_pub.publish(msg); } ); + + ros::ServiceServer maneuver_srv_ = nh.advertiseService("plugins/PlatooningStrategicIHPPlugin/plan_maneuvers", + &PlatoonStrategicIHPPlugin::plan_maneuver_cb, &worker); + ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 1, &PlatoonStrategicIHPPlugin::mob_req_cb, &worker); + ros::Subscriber mob_response_sub = nh.subscribe("incoming_mobility_response", 1, &PlatoonStrategicIHPPlugin::mob_resp_cb, &worker); + ros::Subscriber mob_operation_sub = nh.subscribe("incoming_mobility_operation", 1, &PlatoonStrategicIHPPlugin::mob_op_cb, &worker); + ros::Subscriber current_pose_sub = nh.subscribe("current_pose", 1, &PlatoonStrategicIHPPlugin::pose_cb, &worker); + ros::Subscriber current_twist_sub = nh.subscribe("current_velocity", 1, &PlatoonStrategicIHPPlugin::twist_cb, &worker); + // not use BSMID, consider delete + // ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &PlatoonStrategicIHPPlugin::bsm_cb, &worker); + ros::Subscriber cmd_sub = nh.subscribe("twist_raw", 1, &PlatoonStrategicIHPPlugin::cmd_cb, &worker); + ros::Subscriber georeference_sub = nh.subscribe("georeference", 1, &PlatoonStrategicIHPPlugin::georeference_cb, &worker); + + + ros::Timer discovery_pub_timer_ = nh.createTimer( + ros::Duration(ros::Rate(10.0)), + [&worker](const auto&) { worker.onSpin(); }); + + ros::CARMANodeHandle::spin(); + } +}; + +} // namespace platoon_strategicIHP diff --git a/trajectory_executor/test/trajectory_executor_2.test b/platooning_strategic_IHP/launch/platoon_strategic_ihp.launch similarity index 62% rename from trajectory_executor/test/trajectory_executor_2.test rename to platooning_strategic_IHP/launch/platoon_strategic_ihp.launch index 1f24a8299b..bb9dfa400c 100644 --- a/trajectory_executor/test/trajectory_executor_2.test +++ b/platooning_strategic_IHP/launch/platoon_strategic_ihp.launch @@ -1,5 +1,5 @@ - + - - + + - diff --git a/platooning_strategic_IHP/package.xml b/platooning_strategic_IHP/package.xml new file mode 100644 index 0000000000..9e7f8ac83c --- /dev/null +++ b/platooning_strategic_IHP/package.xml @@ -0,0 +1,21 @@ + + + platoon_strategic_ihp + 3.3.0 + The node is to plan a platooning maneuver + carma + Apache 2.0 License + catkin + carma_utils + cav_msgs + roscpp + std_msgs + carma_wm + cav_srvs + lanelet2_extension + tf + tf2 + tf2_ros + tf2_geometry_msgs + carma_cmake_common + diff --git a/trajectory_executor/src/trajectory_executor/trajectory_executor_node.cpp b/platooning_strategic_IHP/src/main_ihp.cpp similarity index 59% rename from trajectory_executor/src/trajectory_executor/trajectory_executor_node.cpp rename to platooning_strategic_IHP/src/main_ihp.cpp index 5cb1a7d187..fecada04de 100644 --- a/trajectory_executor/src/trajectory_executor/trajectory_executor_node.cpp +++ b/platooning_strategic_IHP/src/main_ihp.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2019-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 @@ -14,20 +14,20 @@ * the License. */ -#include -#include "trajectory_executor/trajectory_executor.hpp" - -/** - * Main application entry point for TrajectoryExecutor Node +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma */ -int main(int argc, char** argv) -{ - ros::init(argc, argv, "trajectory_executor"); - ROS_DEBUG("Entered main function for Trajectory executor, starting node..."); - trajectory_executor::TrajectoryExecutor te; - te.init(); - te.run(); +#include +#include "platoon_strategic_plugin_node_ihp.h" - return 0; +int main(int argc, char** argv) +{ + ros::init(argc, argv, "platoon_strategic_ihp"); + platoon_strategic_ihp::PlatoonStrategicIHPPluginNode node; + node.run(); + return 0; } diff --git a/platooning_strategic_IHP/src/platoon_manager_ihp.cpp b/platooning_strategic_IHP/src/platoon_manager_ihp.cpp new file mode 100644 index 0000000000..89330be577 --- /dev/null +++ b/platooning_strategic_IHP/src/platoon_manager_ihp.cpp @@ -0,0 +1,980 @@ +/* + * Copyright (C) 2019-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. + */ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + +#include "platoon_manager_ihp.h" +#include "platoon_config_ihp.h" +#include +#include +#include + +namespace platoon_strategic_ihp +{ + /** + * Implementation notes: + * + * 1. platoon vector indexing: + * A vector of platoon members (vehicles), sorted by downtrack distance in descending order (i.e., [dtd_1 > dtd_2 > ... > dtd_n] ). + * + * 2. speed vector indexing: + * A vector that only contains speed (m/s) of each platoon member. Same order with platoon list (i.e., [platoon_front, follwer_1, ..., follower_n]). + * + * 3. downtrackDistance vector indexing: + * A vector that only contains downtrack distance (m) of each platoon member. Same order with platoon list (i.e., [platoon_front, follwer_1, ..., follower_n]) + * + * 4. timeHeadway vector indexing + * A vector that only contains time headaway (s) behind each platoon member (i.e., the time gap between host vehicle and its following vehicle). + * Same order with platoon list. For APF, when gap too small, the dynamic leader will be the front vehicle of the small gap. If gap too large, the dynamic leader + * wil be the rear vehicle of the large gap. + * + */ + + PlatoonManager::PlatoonManager() + { + ROS_DEBUG_STREAM("Top of PlatoonManager ctor."); + } + + + // Update the location of the host in the vector of platoon members + void PlatoonManager::updateHostPose(const double downtrack, const double crosstrack) + { + ROS_DEBUG_STREAM("Host (index " << hostPosInPlatoon_ << "): downtrack = " << downtrack << ", crosstrack = " << crosstrack); + host_platoon_[hostPosInPlatoon_].vehiclePosition = downtrack; + host_platoon_[hostPosInPlatoon_].vehicleCrossTrack = crosstrack; + } + + // Update the speed info of the host in the vector of platoon members + void PlatoonManager::updateHostSpeeds(const double cmdSpeed, const double actualSpeed) + { + host_platoon_[hostPosInPlatoon_].commandSpeed = cmdSpeed; + host_platoon_[hostPosInPlatoon_].vehicleSpeed = actualSpeed; + } + + // Update/add one member's information from STATUS messages, update platoon ID if needed. + void PlatoonManager::hostMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params, + const double& DtD, const double& CtD) + { + + // parse params, read member data + std::vector inputsParams; + boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); + // read command speed, m/s + std::vector cmd_parsed; + boost::algorithm::split(cmd_parsed, inputsParams[0], boost::is_any_of(":")); + double cmdSpeed = std::stod(cmd_parsed[1]); + // get DtD directly instead of parsing message, m + double dtDistance = DtD; + // get CtD directly + double ctDistance = CtD; + // read current speed, m/s + std::vector cur_parsed; + boost::algorithm::split(cur_parsed, inputsParams[1], boost::is_any_of(":")); + double curSpeed = std::stod(cur_parsed[1]); + + // If we are currently in a follower state: + // 1. We will update platoon ID based on leader's STATUS + // 2. We will update platoon members info based on platoon ID for all members + if (isFollower) + { + // Sanity check + if (platoonLeaderID.compare(host_platoon_[0].staticId) != 0) + { + ROS_DEBUG_STREAM("///// platoonLeaderID NOT PROPERLY ASSIGNED! Value = " << platoonLeaderID + << ", host_platoon_[0].staticId = " << host_platoon_[0].staticId); + } + + // read message status + bool isFromLeader = platoonLeaderID == senderId; + bool needPlatoonIdChange = isFromLeader && (currentPlatoonID != platoonId); + + if(needPlatoonIdChange) + { + // TODO: better to have leader explicitly inform us that it is merging into another platoon, rather than require us to deduce it here. + // This condition may also result from missing some message traffic, new leader in this platoon, or other confusion/incorrect code. + // Consider using a new STATUS msg param that tells members of new platoon ID and new leader ID when a change is made. + ROS_DEBUG_STREAM("It seems that the current leader is joining another platoon."); + ROS_DEBUG_STREAM("So the platoon ID is changed from " << currentPlatoonID << " to " << platoonId); + currentPlatoonID = platoonId; + updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); + } + else if (currentPlatoonID == platoonId) + { + ROS_DEBUG_STREAM("This STATUS messages is from our platoon. Updating the info..."); + updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); + ROS_DEBUG_STREAM("The first vehicle in our list is now " << host_platoon_[0].staticId); + } + else //sender is in a different platoon + { + ROS_DEBUG_STREAM("This STATUS message is not from a vehicle we care about. Ignore this message with id: " << senderId); + } + } + else //host is leader + { + // If we are currently in any leader state, we only update platoon member based on platoon ID + ROS_DEBUG_STREAM("Host is leader: currentPlatoonID = " << currentPlatoonID << ", incoming platoonId = " << platoonId); + if (currentPlatoonID == platoonId) + { + ROS_DEBUG_STREAM("This STATUS messages is from our platoon. Updating the info..."); + updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); + } + else + { + ROS_DEBUG_STREAM("Platoon IDs not matched"); + ROS_DEBUG_STREAM("currentPlatoonID: " << currentPlatoonID); + ROS_DEBUG_STREAM("incoming platoonId: " << platoonId); + } + } + + // update host vehicle information each time new member is updated. Now platoon contains host vehicle. + std::string hostStaticId = getHostStaticID(); + double hostcmdSpeed = getCommandSpeed(); + double hostDtD = getCurrentDowntrackDistance(); + double hostCtD = getCurrentCrosstrackDistance(); + double hostCurSpeed = getCurrentSpeed(); + updatesOrAddMemberInfo(host_platoon_, hostStaticId, hostcmdSpeed, hostDtD, hostCtD, hostCurSpeed); + } + + // Update/add one member's information from STATUS messages, update platoon ID if needed. + void PlatoonManager::neighborMemberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& params, + const double& DtD, const double& CtD) + { + + // parse params, read member data + std::vector inputsParams; + boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); + // read command speed, m/s + std::vector cmd_parsed; + boost::algorithm::split(cmd_parsed, inputsParams[0], boost::is_any_of(":")); + double cmdSpeed = std::stod(cmd_parsed[1]); + // get DtD directly instead of parsing message, m + double dtDistance = DtD; + // get CtD directly + double ctDistance = CtD; + // read current speed, m/s + std::vector cur_parsed; + boost::algorithm::split(cur_parsed, inputsParams[1], boost::is_any_of(":")); + double curSpeed = std::stod(cur_parsed[1]); + + if (neighborPlatoonID == platoonId) + { + updatesOrAddMemberInfo(neighbor_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); + ROS_DEBUG_STREAM("This STATUS messages is from the target platoon. Updating the info..."); + ROS_DEBUG_STREAM("The first vehicle in that platoon is now " << neighbor_platoon_[0].staticId); + + // If we have data on all members of a neighboring platoon, set a complete record flag + if (neighbor_platoon_info_size_ > 1 && + neighbor_platoon_.size() == neighbor_platoon_info_size_) + { + is_neighbor_record_complete_ = true; + ROS_DEBUG_STREAM("Neighbor record is complete!"); + } + } + else //sender is in a different platoon + { + ROS_DEBUG_STREAM("This STATUS message is not from a vehicle we care about. Ignore this message from sender: " + << senderId << " about platoon: " << platoonId); + } + } + + // Check a new vehicle's existence; add its info to the platoon if not in list, update info if already existed. + void PlatoonManager::updatesOrAddMemberInfo(std::vector& platoon, std::string senderId, double cmdSpeed, + double dtDistance, double ctDistance, double curSpeed) + { + bool isExisted = false; + bool sortNeeded = false; + + // update/add this info into the list + for (size_t i = 0; i < platoon.size(); ++i){ + if(platoon[i].staticId == senderId) { + if (abs(dtDistance - platoon[i].vehiclePosition)/(platoon[i].vehiclePosition + 0.01) > config_.significantDTDchange) + { + ROS_DEBUG_STREAM( "DTD of member " << platoon[i].staticId << " is changed significantly, so a new sort is needed"); + + sortNeeded = true; + } + platoon[i].commandSpeed = cmdSpeed; // m/s + platoon[i].vehiclePosition = dtDistance; // m + platoon[i].vehicleCrossTrack = ctDistance; // m + platoon[i].vehicleSpeed = curSpeed; // m/s + platoon[i].timestamp = ros::Time::now().toNSec()/1000000; + ROS_DEBUG_STREAM("Receive and update platooning info on member " << i << ", ID:" << platoon[i].staticId); + ROS_DEBUG_STREAM(" CommandSpeed = " << platoon[i].commandSpeed); + ROS_DEBUG_STREAM(" Actual Speed = " << platoon[i].vehicleSpeed); + ROS_DEBUG_STREAM(" Downtrack Location = " << platoon[i].vehiclePosition); + ROS_DEBUG_STREAM(" Crosstrack dist = " << platoon[i].vehicleCrossTrack); + + if (senderId == HostMobilityId) + { + hostPosInPlatoon_ = i; + ROS_DEBUG_STREAM(" This is the HOST vehicle"); + } + isExisted = true; + } + } + + if (sortNeeded) + { + // sort the platoon member based on dowtrack distance (m) in an descending order. + std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;}); + ROS_DEBUG_STREAM("Platoon is re-sorted due to large difference in dtd update."); + ROS_DEBUG_STREAM(" Platoon order is now:"); + for (size_t i = 0; i < platoon.size(); ++i) + { + std::string hostFlag = " "; + if (platoon[i].staticId == getHostStaticID()) + { + hostPosInPlatoon_ = i; + hostFlag = "Host"; + } + ROS_DEBUG_STREAM(" " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); + } + } + + // if not already exist, add to platoon list. + if(!isExisted) { + long cur_t = ros::Time::now().toNSec()/1000000; // time in millisecond + + PlatoonMember newMember = PlatoonMember(senderId, cmdSpeed, curSpeed, dtDistance, ctDistance, cur_t); + platoon.push_back(newMember); + // sort the platoon member based on dowtrack distance (m) in an descending order. + std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;}); + + ROS_DEBUG_STREAM("Add a new vehicle into our platoon list " << newMember.staticId << " platoon.size now = " << platoon.size()); + ROS_DEBUG_STREAM(" Platoon order is now:"); + for (size_t i = 0; i < platoon.size(); ++i) + { + std::string hostFlag = " "; + if (platoon[i].staticId == getHostStaticID()) + { + hostPosInPlatoon_ = i; + hostFlag = "Host"; + } + ROS_DEBUG_STREAM(" " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); + } + } + } + + // TODO: Place holder for delete member info due to dissolve operation. + + // Get the platoon size. + int PlatoonManager::getHostPlatoonSize() { + ROS_DEBUG_STREAM("host platoon size: " << host_platoon_.size()); + return host_platoon_.size(); + } + + // Reset variables to indicate there is no current action in work + void PlatoonManager::clearActionPlan() + { + current_plan.valid = false; + current_plan.planId = dummyID; + current_plan.peerId = dummyID; + targetPlatoonID = dummyID; + // Leave the platoon & leader IDs alone since we might continue to be in one + } + + // Reset variables to indicate there is no platoon - host is a solo vehicle again + void PlatoonManager::resetHostPlatoon() + { + // Remove any elements in the platoon vector other than the host vehicle + if (host_platoon_.size() > hostPosInPlatoon_ + 1) + { + host_platoon_.erase(host_platoon_.begin() + hostPosInPlatoon_ + 1, host_platoon_.end()); + } + host_platoon_.erase(host_platoon_.begin(), host_platoon_.begin() + hostPosInPlatoon_); + + // Clean up other variables + currentPlatoonID = dummyID; + platoonLeaderID = dummyID; + hostPosInPlatoon_ = 0; + isCreateGap = false; + dynamic_leader_index_ = 0; + } + + // Reset variables to indicate there is no known neighbor platoon + void PlatoonManager::resetNeighborPlatoon() + { + neighbor_platoon_.clear(); + neighbor_platoon_info_size_ = 0; + neighborPlatoonID = dummyID; + neighbor_platoon_leader_id_ = dummyID; + is_neighbor_record_complete_ = false; + } + + bool PlatoonManager::removeMember(const size_t mem) + { + // Don't remove ourselves! + if (hostPosInPlatoon_ == mem) + { + return false; + } + + // Don't remove a member that isn't there + else if (host_platoon_.size() <= 1 || mem >= host_platoon_.size()) + { + return false; + } + + if (mem < hostPosInPlatoon_) + { + --hostPosInPlatoon_; + } + host_platoon_.erase(host_platoon_.begin() + mem, host_platoon_.begin() + mem + 1); + + // If host is the only remaining member then clean up the other platoon data + if (host_platoon_.size() == 1) + { + currentPlatoonID = dummyID; + platoonLeaderID = dummyID; + hostPosInPlatoon_ = 0; + } + + return true; + } + + bool PlatoonManager::removeMemberById(const std::string id) + { + // Don't remove ourselves! + if (id.compare(HostMobilityId) == 0) + { + return false; + } + + // Search for the member with a matching ID and remove it + for (size_t m = 0; m < host_platoon_.size(); ++m) + { + if (id.compare(host_platoon_[m].staticId) == 0) + { + return removeMember(m); + } + } + + // Indicate the member was not found + return false; + } + + // Find the downtrack distance of the last vehicle of the platoon, in m. + double PlatoonManager::getPlatoonRearDowntrackDistance(){ + // due to downtrack descending order, the 1ast vehicle in list is the platoon rear vehicle. + // Even if host is solo, platoon size is 1 so this works. + return host_platoon_[host_platoon_.size()-1].vehiclePosition; + } + + // Find the downtrack distance of the first vehicle of the platoon, in m. + double PlatoonManager::getPlatoonFrontDowntrackDistance(){ + // due to downtrack descending order, the firest vehicle in list is the platoon front vehicle. + return host_platoon_[0].vehiclePosition; + } + + // Return the dynamic leader (i.e., the vehicle to follow) of the host vehicle. + PlatoonMember PlatoonManager::getDynamicLeader(){ + PlatoonMember dynamicLeader; + ROS_DEBUG_STREAM("host_platoon_ size: " << host_platoon_.size()); + if(isFollower) + { + ROS_DEBUG_STREAM("Leader initially set as first vehicle in platoon"); + // return the first vehicle in the platoon as default if no valid algorithm applied + // due to downtrack descending order, the platoon front veihcle is the first in list. + dynamicLeader = host_platoon_[0]; + if (algorithmType_ == "APF_ALGORITHM"){ + size_t newLeaderIndex = allPredecessorFollowing(); + + dynamic_leader_index_ = (int)newLeaderIndex; + ROS_DEBUG_STREAM("dynamic_leader_index_: " << dynamic_leader_index_); + if(newLeaderIndex < host_platoon_.size()) { //this must always be true! + dynamicLeader = host_platoon_[newLeaderIndex]; + ROS_DEBUG_STREAM("APF output: " << dynamicLeader.staticId); + previousFunctionalDynamicLeaderIndex_ = newLeaderIndex; + previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId; + } + else //something is terribly wrong in the logic! + { + ROS_WARN_STREAM("newLeaderIndex = " << newLeaderIndex << " is invalid coming from allPredecessorFollowing!"); + /** + * it might happened when the subject vehicle gets far away from the preceding vehicle, + * in which case the host vehicle will follow the one in front. + */ + dynamicLeader = host_platoon_[getNumberOfVehicleInFront() - 1]; + // update index and ID + previousFunctionalDynamicLeaderIndex_ = getNumberOfVehicleInFront()-1; + previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId; + ROS_DEBUG_STREAM("Based on the output of APF algorithm we start to follow our predecessor."); + } + } + } + return dynamicLeader; + + } + + // The implementation of all predecessor following algorithm. Determine the dynamic leader for the host vehicle to follow. + int PlatoonManager::allPredecessorFollowing(){ + + ///***** Case Zero *****/// + // If the host vehicle is the second follower of a platoon, it will always follow the platoon leader in the front + if(getNumberOfVehicleInFront() == 1) + { + // If the host is the second vehicle, then follow the leader. + ROS_DEBUG_STREAM("As the second vehicle in the platoon, it will always follow the leader. Case Zero"); + return 0; + } + ///***** Case One *****/// + // If there weren't a leader in the previous time step, follow the first vehicle (i.e., the platoon leader) as default. + if(previousFunctionalDynamicLeaderID_ == "") { + ROS_DEBUG_STREAM("APF algorithm did not found a dynamic leader in previous time step. Case one"); + return 0; + } + + //***** Formulate speed and downtrack vector *****// + // Update host vehicle info when update member info, so platoon list include host vehicle, direct use platoon size for downtrack/speed vector. + // Record downtrack distance (m) of each member + std::vector downtrackDistance(hostPosInPlatoon_); + for(size_t i = 0; i < hostPosInPlatoon_; i++) { + downtrackDistance[i] = host_platoon_[i].vehiclePosition; // m + } + // Record speed (m/s) of each member + std::vector speed(host_platoon_.size()); + for(size_t i = 0; i < host_platoon_.size(); i++) { + speed[i] = host_platoon_[i].vehicleSpeed; // m/s + } + + + ///***** Case Two *****/// + // If the distance headway between the subject vehicle and its predecessor is an issue + // according to the "min_gap" and "max_gap" thresholds, then it should follow its predecessor + // The following line will not throw exception because the length of downtrack array is larger than two in this case + double distHeadwayWithPredecessor = downtrackDistance[hostPosInPlatoon_ - 1] - downtrackDistance[hostPosInPlatoon_]; + gapWithPred_ = distHeadwayWithPredecessor; + if(insufficientGapWithPredecessor(distHeadwayWithPredecessor)) { + ROS_DEBUG_STREAM("APF algorithm decides there is an issue with the gap with preceding vehicle: " << distHeadwayWithPredecessor << " m. Case Two"); + return hostPosInPlatoon_ - 1; + } + else{ + // implementation of the main part of APF algorithm + // calculate the time headway between every consecutive pair of vehicles + std::vector timeHeadways = calculateTimeHeadway(downtrackDistance, speed); + ROS_DEBUG_STREAM("APF calculate time headways: " ); + for (const auto& value : timeHeadways) + { + ROS_DEBUG_STREAM("APF time headways: " << value); + } + ROS_DEBUG_STREAM("APF found the previous dynamic leader is " << previousFunctionalDynamicLeaderID_); + // if the previous dynamic leader is the first vehicle in the platoon + + if(previousFunctionalDynamicLeaderIndex_ == 0) { + + ///***** Case Three *****/// + // If there is a violation, the return value is the desired dynamic leader index + ROS_DEBUG_STREAM("APF use violations on lower boundary or maximum spacing to choose dynamic leader. Case Three."); + return determineDynamicLeaderBasedOnViolation(timeHeadways); + } + else{ + // if the previous dynamic leader is not the first vehicle + // get the time headway between every consecutive pair of vehicles from index Of Previous dynamic Leader + std::vector partialTimeHeadways = getTimeHeadwayFromIndex(timeHeadways, previousFunctionalDynamicLeaderIndex_); + for (const auto& value : partialTimeHeadways) + { + ROS_DEBUG_STREAM("APF partial time headways: " << value); + } + int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(partialTimeHeadways); + int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(partialTimeHeadways); + // if there are no violations anywhere between the subject vehicle and the current dynamic leader, + // then depending on the time headways of the ENTIRE platoon, the subject vehicle may switch + // dynamic leader further downstream. This is because the subject vehicle has determined that there are + // no time headways between itself and the current dynamic leader which would cause the platoon to be unsafe. + // if there are violations somewhere betweent the subject vehicle and the current dynamic leader, + // then rather than assigning dynamic leadership further DOWNSTREAM, we must go further UPSTREAM in the following lines + if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation == -1) { + // In order for the subject vehicle to assign dynamic leadership further downstream, + // two criteria must be satisfied: first the leading vehicle and its immediate follower must + // have a time headway greater than "upper_boundary." The purpose of this criteria is to + // introduce a hysteresis in order to eliminate the possibility of a vehicle continually switching back + // and forth between two dynamic leaders because one of the time headways is hovering right around + // the "lower_boundary" threshold; second the leading vehicle and its predecessor must have + // a time headway less than "min_spacing" second. Just as with "upper_boundary", "min_spacing" exists to + // introduce a hysteresis where dynamic leaders are continually being switched. + bool condition1 = timeHeadways[previousFunctionalDynamicLeaderIndex_] > config_.headawayStableLowerBond; + bool condition2 = timeHeadways[previousFunctionalDynamicLeaderIndex_ - 1] < config_.headawayStableUpperBond; + + ///***** Case Four *****/// + //we may switch dynamic leader further downstream + if(condition1 && condition2) { + ROS_DEBUG_STREAM("APF found two conditions for assigning local dynamic leadership further downstream are satisfied. Case Four"); + return determineDynamicLeaderBasedOnViolation(timeHeadways); + } else { + + ///***** Case Five *****/// + // We may not switch dynamic leadership to another vehicle further downstream because some criteria are not satisfied + ROS_DEBUG_STREAM("APF found two conditions for assigning local dynamic leadership further downstream are not satisfied. Case Five."); + ROS_DEBUG_STREAM("condition1: " << condition1 << " & condition2: " << condition2); + return previousFunctionalDynamicLeaderIndex_; + } + } else if(closestLowerBoundaryViolation != -1 && closestMaximumSpacingViolation == -1) { + // The rest four cases have roughly the same logic: locate the closest violation and assign dynamic leadership accordingly + + ///***** Case Six *****/// + ROS_DEBUG_STREAM("APF found closestLowerBoundaryViolation on partial time headways. Case Six."); + return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation; + + } else if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation != -1) { + + ///***** Case Seven *****/// + ROS_DEBUG_STREAM("APF found closestMaximumSpacingViolation on partial time headways. Case Seven."); + return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation; + } else{ + ROS_DEBUG_STREAM("APF found closestMaximumSpacingViolation and closestLowerBoundaryViolation on partial time headways."); + if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { + + ///***** Case Eight *****/// + ROS_DEBUG_STREAM("closest LowerBoundaryViolation is higher than closestMaximumSpacingViolation on partial time headways. Case Eight."); + return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation; + } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation) { + + ///***** Case Nine *****/// + ROS_DEBUG_STREAM("closestMaximumSpacingViolation is higher than closestLowerBoundaryViolation on partial time headways. Case Nine."); + return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation; + } else { + ROS_DEBUG_STREAM("APF dynamic Leader selection parameter is wrong!"); + return 0; + } + } + } + + } + } + + // Find the time headaway (s) sub-list based on the platoon wise comprehensive time headaway list, starting index is indicated by the parameter: "start". + std::vector PlatoonManager::getTimeHeadwayFromIndex(std::vector timeHeadways, int start) const { + std::vector result(timeHeadways.begin() + start-1, timeHeadways.end()); + return result; + } + + // Determine if the gap (m) between host and predecessor is big enough, with regards to minGap_ (m) and maxGap_ (m). + bool PlatoonManager::insufficientGapWithPredecessor(double distanceToPredVehicle) { + + // For normal operation, gap > minGap is necessary. + bool frontGapIsTooSmall = distanceToPredVehicle < config_.minCutinGap; + + // Host vehicle was following predecessor vehicle. --> The predecessor vehicle was violating gap threshold. + bool previousLeaderIsPredecessor = previousFunctionalDynamicLeaderID_ == host_platoon_[host_platoon_.size() - 1].staticId; + + // Gap greater than maxGap_ is necessary for host to stop choosing predecessor as dynamic leader. + bool frontGapIsNotLargeEnough = distanceToPredVehicle < config_.maxCutinGap && previousLeaderIsPredecessor; + + return (frontGapIsTooSmall || frontGapIsNotLargeEnough); + } + + // Calculate the time headway (s) behind each vehicle of the platoon. If no one behind or following car stoped, return infinity. + std::vector PlatoonManager::calculateTimeHeadway(std::vector downtrackDistance, std::vector speed) const{ + std::vector timeHeadways(downtrackDistance.size() - 1); + // Due to downtrack descending order, the platoon member with smaller index has larger downtrack, hence closer to the front of the platoon. + for (size_t i = 0; i < timeHeadways.size(); i++){ + // Calculate time headaway between the vehicle behind. + if (speed[i+1] >= config_.ss_theta) // speed is in m/s + { + timeHeadways[i] = (downtrackDistance[i] - downtrackDistance[i+1]) / speed[i+1]; // downtrack is in m, speed is in m/s. + } + // If no one behind or following car stoped, return infinity. + else + { + timeHeadways[i] = std::numeric_limits::infinity(); + } + } + return timeHeadways; // time is in s. + } + + // Determine the dynamic leader ID based on gap threshold violation's index. + int PlatoonManager::determineDynamicLeaderBasedOnViolation(std::vector timeHeadways){ + + /** + * Note: For both condition, the host will always choose to follow the vechile that has a relatively larger gap in front. + * + * + * max-vilation (follow veh2): [*veh3*] ---------- [*veh2*] ------------------------------- [*veh1*] ---------- [*veh0*] + * ^ + * gap2 gap1(max violation) gap0 + * + * min-vilation (follow veh1): [*veh3*] -----------[*veh2*]---[*veh1*] ---------- [*veh0*] + * ^ + * gap2 gap1 gap0 + * (min violation) + */ + // Find the closest violations. + int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(timeHeadways); + int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(timeHeadways); + + // Compare the violation locations, always following the closer violation vehicle (larger index) first, then the furthur ones. + if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { + ROS_DEBUG_STREAM("APF found violation on closestLowerBoundaryViolation at " << closestLowerBoundaryViolation); + return closestLowerBoundaryViolation; // Min violation, following the vehicle that is in font of the violating gap. + } + else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation){ + ROS_DEBUG_STREAM("APF found violation on closestMaximumSpacingViolation at " << closestMaximumSpacingViolation); + return closestMaximumSpacingViolation + 1; // Max violation, follow the vehicle that is behinf the violating gap. + } + else{ + ROS_DEBUG_STREAM("APF found no violations on both closestLowerBoundaryViolation and closestMaximumSpacingViolation"); + return 0; + } + } + + // Find the lower boundary violation vehicle that closest to the host vehicle. If no violation found, return -1. + int PlatoonManager::findLowerBoundaryViolationClosestToTheHostVehicle(std::vector timeHeadways) const{ + // Due to descending downtrack order, the search starts from the platoon rear, which corresponds to last in list. + for(int i = timeHeadways.size()-1; i >= 0; i--) { + if(timeHeadways[i] < config_.minAllowableHeadaway) // in s + { + return i; + } + } + return -1; + } + + // Find the maximum spacing violation vehicle that closest to the host vehicle. If no violation found, return -1. + int PlatoonManager::findMaximumSpacingViolationClosestToTheHostVehicle(std::vector timeHeadways) const { + // UCLA: Add maxAllowableHeadaway adjuster to increase the threshold during gap creating period. + double maxAllowableHeadaway_adjusted = config_.maxAllowableHeadaway; + if (isCreateGap) { + // adjust maximum allowed headway to allow for a bigger gap + maxAllowableHeadaway_adjusted = maxAllowableHeadaway_adjusted*(1 + config_.createGapAdjuster); + } + + // Due to descending downtrack order, the search starts from the platoon rear, which corresponds to last in list. + for(int i = timeHeadways.size()-1; i >= 0 ; i--) { + if(timeHeadways[i] > maxAllowableHeadaway_adjusted) // in s + { + return i; + } + } + return -1; + } + + // Change the local platoon manager from follower operation state to leader operation state for single vehicle status change. + // This could happen because host is 2nd in line and leader is departing, or because APF algorithm decided host's gap is too + // large and we need to separate from front part of platoon. + void PlatoonManager::changeFromFollowerToLeader() { + + // Get current host info - assumes departing leader or front of platoon hasn't already been removed from the vector + PlatoonMember hostInfo = host_platoon_[hostPosInPlatoon_]; + + // Clear the front part of the platoon info, since we are splitting off from it; leaves host as element 0 + if (hostPosInPlatoon_ > 0) + { + host_platoon_.erase(host_platoon_.begin(), host_platoon_.begin() + hostPosInPlatoon_); + }else + { + ROS_WARN("### Host becoming leader, but is already at index 0 in host_platoon_ vector! Vector unchanged."); + } + + hostPosInPlatoon_ = 0; + isFollower = false; + currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()()); + previousFunctionalDynamicLeaderID_ = ""; + previousFunctionalDynamicLeaderIndex_ = -1; + ROS_DEBUG_STREAM("The platoon manager is changed from follower state to leader state. New platoon ID = " << currentPlatoonID); + } + + // Change the local platoon manager from leader operation state to follower operation state for single vehicle status change. + // This could happen because another vehicle did a front join and host is now 2nd in line, or because host was a solo vehicle + // that just completed joining a platoon at any position. + // Note: The platoon list will be firstly reset to only include the new leader and the host, then allowed to gradually repopulate via + // updateMembers() as new MobilityOperation messages come in. + void PlatoonManager::changeFromLeaderToFollower(std::string newPlatoonId, std::string newLeaderId) { + + // Save the current host info + PlatoonMember hostInfo = host_platoon_[hostPosInPlatoon_]; + + // Clear contents of the platoon vector and rebuild it with the two known members at this time, leader & host. + // Remaining leader info and info about any other members will get populated as messages come in. + PlatoonMember newLeader = PlatoonMember(); + newLeader.staticId = newLeaderId; + host_platoon_.clear(); + host_platoon_.push_back(newLeader); //can get location info updated later with a STATUS or INFO message + host_platoon_.push_back(hostInfo); + + hostPosInPlatoon_ = 1; //since host was previously leader it is now guaranteed to be 2nd in the line (index 1) + isFollower = true; + currentPlatoonID = newPlatoonId; + + // Clear the record of neighbor platoon, since we likely just joined it + resetNeighborPlatoon(); + + ROS_DEBUG_STREAM("The platoon manager is changed from leader state to follower state. Platoon vector re-initialized. Plan ID = " << newPlatoonId); + } + + // Return the number of vehicles in the front of the host vehicle. If host is leader or a single vehicle, return 0. + int PlatoonManager::getNumberOfVehicleInFront() { + return hostPosInPlatoon_; + } + + // Return the distance (m) to the predecessor vehicle. + double PlatoonManager::getDistanceToPredVehicle() { + return gapWithPred_; + } + + // Return the current host vehicle speed in m/s. + double PlatoonManager::getCurrentSpeed() const { + return host_platoon_[hostPosInPlatoon_].vehicleSpeed; + } + + // Return the current command speed of host vehicle in m/s. + double PlatoonManager::getCommandSpeed() const + { + return host_platoon_[hostPosInPlatoon_].commandSpeed; + } + + // Return the current downtrack distance in m. + double PlatoonManager::getCurrentDowntrackDistance() const + { + return host_platoon_[hostPosInPlatoon_].vehiclePosition; + } + + // Return the current crosstrack distance, in m. + double PlatoonManager::getCurrentCrosstrackDistance() const + { + return host_platoon_[hostPosInPlatoon_].vehicleCrossTrack; + } + + // UCLA: return the host vehicle static ID. + std::string PlatoonManager::getHostStaticID() const + { + return HostMobilityId; + } + + // Return the physical length from platoon front vehicle (front bumper) to platoon rear vehicle (rear bumper) in m. + double PlatoonManager::getCurrentPlatoonLength() { + //this works even if platoon size is 1 (can't be 0) + return host_platoon_[0].vehiclePosition - host_platoon_[host_platoon_.size() - 1].vehiclePosition + config_.vehicleLength; + } + + // ---------------------- UCLA: IHP platoon trajectory regulation --------------------------- // + // Calculate the time headway summation of the vehicle in front of the host + double PlatoonManager::getPredecessorTimeHeadwaySum() + { + // 1. read dtd vector + // dtd vector + std::vector downtrackDistance(host_platoon_.size()); + for (size_t i = 0; i < host_platoon_.size(); i++) + { + downtrackDistance[i] = host_platoon_[i].vehiclePosition; + } + // speed vector + std::vector speed(host_platoon_.size()); + for (size_t i = 0; i < host_platoon_.size(); i++) + { + speed[i] = host_platoon_[i].vehicleSpeed; + } + + // 2. find the summation of "veh_len/veh_speed" for all predecessors + double tmp_time_hdw = 0.0; + double cur_dtd; + for (size_t index = 0; index < downtrackDistance.size(); index++) + { + cur_dtd = downtrackDistance[index]; + if (cur_dtd > getCurrentDowntrackDistance()) + { + // greater dtd ==> in front of host veh + tmp_time_hdw += config_.vehicleLength / (speed[index] + 0.00001); + } + } + + // Return the calcualted value + return tmp_time_hdw; + } + + // Return the predecessor speed + double PlatoonManager::getPredecessorSpeed() + { + return host_platoon_[hostPosInPlatoon_].vehicleSpeed; // m/s + } + + // Return the predecessor location + double PlatoonManager::getPredecessorPosition() + { + // Read host index. + int host_platoon_index = getNumberOfVehicleInFront(); + + // Return speed + return host_platoon_[host_platoon_index].vehiclePosition; // m + } + + // Trajectory based platoon trajectory regulation. + double PlatoonManager::getIHPDesPosFollower(double time_step) + { + /** + * Calculate desired position based on previous vehicle's trajectory for followers. + * + * TODO: The platoon trajectory regulation is derived with the assumption that all vehicle + * have identical length (i.e., 5m). Future development is needed to include variable + * vehicle length into consideration. + */ + + // 1. read dtd vector + // dtd vector + std::vector downtrackDistance(host_platoon_.size()); + for (size_t i = 0; i < host_platoon_.size(); i++) + { + downtrackDistance[i] = host_platoon_[i].vehiclePosition; + } + // speed vector + std::vector speed(host_platoon_.size()); + for (size_t i = 0; i < host_platoon_.size(); i++) + { + speed[i] = host_platoon_[i].vehicleSpeed; + } + + // 2. find the summation of "veh_len/veh_speed" for all predecessors + double tmp_time_hdw = 0.0; + double cur_dtd; + for (size_t index = 0; index < downtrackDistance.size(); index++) + { + cur_dtd = downtrackDistance[index]; + if (cur_dtd > getCurrentDowntrackDistance()) + { + // greater dtd ==> in front of host veh + tmp_time_hdw += config_.vehicleLength / (speed[index] + 0.00001); + } + } + + // 3. read host veh and front veh info + // Predecessor vehicle data. + double pred_spd = speed[getNumberOfVehicleInFront()-1]; // m/s + double pred_pos = downtrackDistance[getNumberOfVehicleInFront()-1]; // m + + // host data. + double ego_spd = getCurrentSpeed(); // m/s + double ego_pos = getCurrentDowntrackDistance(); // m + + // platoon position index + int pos_idx = getNumberOfVehicleInFront(); + + double desirePlatoonGap = config_.intra_tau; //s + + // IHP desired position calculation methods + double pos_g; // desired downtrack position calculated with time-gap, in m. + double pos_h; // desired downtrack position calculated with distance headaway, in m. + + // 4. IHP gap regualtion + + // intermediate variables + double timeGapAndStepRatio = desirePlatoonGap/time_step; // The ratio between desired platoon time gap and the current time_step. + double totalTimeGap = desirePlatoonGap*pos_idx; // The overall time gap from host vehicle to the platoon leader, in s. + + // calcilate pos_gap and pos_headway + if ((pred_spd <= ego_spd) && (ego_spd <= config_.ss_theta)) + { + // ---> 4.1 pos_g + pos_g = (pred_pos - config_.vehicleLength - config_.standstill + ego_pos*timeGapAndStepRatio) / (1 + timeGapAndStepRatio); + // ---> 4.2 pos_h + double pos_h_nom = (pred_pos - config_.standstill + ego_pos*(totalTimeGap + tmp_time_hdw)/time_step); + double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/time_step)); + pos_h = pos_h_nom/pos_h_denom; + + } + else + { + // ---> 4.1 pos_g + pos_g = (pred_pos - config_.vehicleLength + ego_pos*(timeGapAndStepRatio)) / (1 + timeGapAndStepRatio); + // ---> 4.2 pos_h + double pos_h_nom = (pred_pos + ego_pos*(totalTimeGap + tmp_time_hdw)/time_step); + double pos_h_denom = (1 + ((totalTimeGap + tmp_time_hdw)/time_step)); + pos_h = pos_h_nom/pos_h_denom; + } + + // ---> 4.3 desire speed and desire location + double pos_des = config_.gap_weight*pos_g + (1.0 - config_.gap_weight)*pos_h; + // double des_spd = (pos_des - ego_pos) / time_step; + + // ---> 4.4 return IHP calculated desired location + return pos_des; + } + + // UCLA: find the index of the closest vehicle in the target platoon that is in front of the host vehicle (cut-in joiner). + // Note: The joiner will cut-in at the back of this vehicle, which make this index points to the vehicle that is leading the cut-in gap. + int PlatoonManager::getClosestIndex(double joinerDtD) + { + /* + A naive way to find the closest member that is in front of the host that point to the gap to join the platoon. + Note: The potential gap leading vehicle should be in front of the joiner (i.e., gap leading vehicle's dtd > joiner's dtd). + If the joiner is already in front of the platoon leader, this function will return -1 (i.e., cut-in front). + */ + + double min_diff = 99999.0; + int cut_in_index = -1; //-2 is meaningless default + + ROS_DEBUG_STREAM("neighbor_platoon_.size(): " << neighbor_platoon_.size()); + + // Loop through all target platoon members + for(size_t i = 0; i < neighbor_platoon_.size(); i++) + { + double current_member_dtd = neighbor_platoon_[i].vehiclePosition; + ROS_DEBUG_STREAM("current_member_dtd: "<< current_member_dtd); + double curent_dtd_diff = current_member_dtd - joinerDtD; + ROS_DEBUG_STREAM("curent_dtd_diff: "<< curent_dtd_diff); + // update min index + if (curent_dtd_diff > 0 && curent_dtd_diff < min_diff) + { + min_diff = current_member_dtd; + cut_in_index = i; + } + } + + return cut_in_index; + } + + // UCLA: find the current cut-in join gap size in downtrack distance (m). The origin of the vehicle when calculating DtD is locate at the rear axle. + double PlatoonManager::getCutInGap(const int gap_leading_index, const double joinerDtD) + { + /* + Locate the target cut-in join gap size based on the index. + */ + + // Initiate variables + double gap_size = -0.999; + size_t index = 0; + if (gap_leading_index >= 0) + { + index = static_cast(gap_leading_index); + } + + // cut-in from front + if (gap_leading_index == -1) + { + double gap_rear_dtd = neighbor_platoon_[0].vehiclePosition; + gap_size = joinerDtD - gap_rear_dtd - config_.vehicleLength; + } + // cut-in from behind + else if (index == neighbor_platoon_.size() - 1) + { + double gap_leading_dtd = neighbor_platoon_[index].vehiclePosition; + gap_size = gap_leading_dtd - joinerDtD - config_.vehicleLength;; + } + // cut-in in the middle + else + { + double gap_leading_dtd = neighbor_platoon_[index].vehiclePosition; + double gap_rear_dtd = neighbor_platoon_[index + 1].vehiclePosition; + gap_size = gap_leading_dtd - gap_rear_dtd - config_.vehicleLength; + } + + // return gap_size value + return gap_size; + } +} \ No newline at end of file diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp new file mode 100644 index 0000000000..ec04a6d538 --- /dev/null +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp @@ -0,0 +1,3495 @@ +/* + * Copyright (C) 2019-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. + */ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + + +#include +#include +#include "platoon_strategic_ihp.h" +#include +#include + + +namespace platoon_strategic_ihp +{ + + PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin() + { + } + + // -------------- constructor --------------// + PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, + PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, + MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, + PlatooningInfoCB platooning_info_publisher) + : plugin_discovery_publisher_(plugin_discovery_publisher), mobility_request_publisher_(mobility_request_publisher), + mobility_response_publisher_(mobility_response_publisher), mobility_operation_publisher_(mobility_operation_publisher), + platooning_info_publisher_(platooning_info_publisher), wm_(wm), config_(config) + { + ROS_DEBUG_STREAM("Top of PlatoonStrategicIHP ctor."); + std::string hostStaticId = config_.vehicleID; //static ID for this vehicle + pm_.HostMobilityId = hostStaticId; + + // construct platoon member for host vehicle as the first element in the vector, since it starts life as a solo vehicle + long cur_t = ros::Time::now().toNSec()/1000000; // time in millisecond + PlatoonMember hostVehicleMember = PlatoonMember(hostStaticId, 0.0, 0.0, 0.0, 0.0, cur_t); + pm_.host_platoon_.push_back(hostVehicleMember); + + plugin_discovery_msg_.name = "PlatooningStrategicIHPPlugin"; + plugin_discovery_msg_.version_id = "v1.0"; + plugin_discovery_msg_.available = true; + plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; + plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; + ROS_DEBUG_STREAM("ctor complete. hostStaticId = " << hostStaticId); + } + + + //-------------------------------- Extract Data --------------------------------------// + + // Find ecef point based on pose message + cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::pose_to_ecef(geometry_msgs::PoseStamped pose_msg) + { + + if (!map_projector_) + { + throw std::invalid_argument("No map projector available for ecef conversion"); + } + + cav_msgs::LocationECEF location; + + // note: ecef point read from map projector is in m. + lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({pose_msg.pose.position.x, pose_msg.pose.position.y, 0.0}, 1); + location.ecef_x = ecef_point.x() * 100.0; + location.ecef_y = ecef_point.y() * 100.0; + location.ecef_z = ecef_point.z() * 100.0; + + + // ROS_DEBUG_STREAM("location.ecef_x: " << location.ecef_x); + // ROS_DEBUG_STREAM("location.ecef_y: " << location.ecef_y); + // ROS_DEBUG_STREAM("location.ecef_z: " << location.ecef_z); + + // note: the returned ecef is in cm. + return location; + } + + // Function to assign host pose_ecef_point_ + void PlatoonStrategicIHPPlugin::setHostECEF(cav_msgs::LocationECEF pose_ecef_point) + { + // Note, the ecef here is in cm. + pose_ecef_point_ = pose_ecef_point; + } + + // Function to get pm_ object + PlatoonManager PlatoonStrategicIHPPlugin::getHostPM() + { + return pm_; + } + + // Function to set platoon manager state + void PlatoonStrategicIHPPlugin::setPMState(PlatoonState desiredState) + { + pm_.current_platoon_state = desiredState; + } + + // Update platoon list (Unit Test function) + void PlatoonStrategicIHPPlugin::updatePlatoonList(std::vector platoon_list) + { + pm_.host_platoon_ = platoon_list; + } + + // Callback to calculate downtrack based on pose message. + void PlatoonStrategicIHPPlugin::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg) + { + pose_msg_ = geometry_msgs::PoseStamped(*msg.get()); + + if (pm_.current_platoon_state != PlatoonState::STANDBY) + { + lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); + carma_wm::TrackPos tc = wm_->routeTrackPos(current_loc); + + // update host's DtD and CtD + current_downtrack_ = tc.downtrack; + current_crosstrack_ = tc.crosstrack; + // ROS_DEBUG_STREAM("current_downtrack_ = " << current_downtrack_ << ", current_crosstrack_ = " << current_crosstrack_); + pm_.updateHostPose(current_downtrack_, current_crosstrack_); + + // note: the ecef read from "pose_ecef_point" is in cm. + cav_msgs::LocationECEF pose_ecef_point = pose_to_ecef(pose_msg_); + setHostECEF(pose_ecef_point); + } + } + + // callback kto update the command speed on x direction, in m/s. + void PlatoonStrategicIHPPlugin::cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg) + { + cmd_speed_ = msg->twist.linear.x; + } + + // twist command, linear speed on x direction, in m/s. + void PlatoonStrategicIHPPlugin::twist_cb(const geometry_msgs::TwistStampedConstPtr& msg) + { + current_speed_ = msg->twist.linear.x; + if (current_speed_ < STOPPED_SPEED) + { + current_speed_ = 0.0; + } + } + + // Return the lanelet id. + int PlatoonStrategicIHPPlugin::findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path) + { + for(size_t i = 0; i < path.size(); ++i) + { + if(path[i].id() == target_id) + { + return i; + } + } + return -1; + } + + // Find the speed limit for the current road (also used as the desired speed). + double PlatoonStrategicIHPPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt) + { + double target_speed = 0.0; + + lanelet::Optional traffic_rules = wm_->getTrafficRules(); + if (traffic_rules) + { + target_speed =(*traffic_rules)->speedLimit(llt).speedLimit.value(); + } + else + { + throw std::invalid_argument("Valid traffic rules object could not be built"); + } + + ROS_DEBUG_STREAM("target speed (limit) " << target_speed); + + return target_speed; + } + + // Find the lane width based on current location + double PlatoonStrategicIHPPlugin::findLaneWidth() + { + lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); + + auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10); + lanelet::ConstLanelet current_lanelet = current_lanelets[0].second; + + // find left and right bound + lanelet::ConstLineString3d left_bound = current_lanelet.leftBound(); + lanelet::ConstLineString3d right_bound = current_lanelet.leftBound(); + + // find lane width + double dx = abs (left_bound[0].x() - right_bound[0].x()); + double dy = abs (left_bound[0].y() - right_bound[0].y()); + double laneWidth = sqrt(dx*dx + dy*dy); + ROS_DEBUG_STREAM("calculated lane width: " << laneWidth); + + // TODO temporary disable this function and return constant value + laneWidth = 3.5; + + + return laneWidth; + } + + // Check if target platoon is in front of the host vehicle, and within the same lane (downtrack is the DTD of the target vehicle). + bool PlatoonStrategicIHPPlugin::isVehicleRightInFront(double downtrack, double crosstrack) + { + // Position info of the host vehcile + double currentDtd = current_downtrack_; + double currentCtd = current_crosstrack_; + bool samelane = abs(currentCtd-crosstrack) <= config_.maxCrosstrackError; + + if (downtrack > currentDtd && samelane) + { + ROS_DEBUG_STREAM("Found a platoon in front. We are able to join"); + return true; + } + else + { + ROS_DEBUG_STREAM("Ignoring platoon that is either behind host or in another lane."); + ROS_DEBUG_STREAM("The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); + return false; + } + } + + // UCLA: check if target platoon at back, and within the same lane. Used for same-lane frontal join. + bool PlatoonStrategicIHPPlugin::isVehicleRightBehind(double downtrack, double crosstrack) + { + double currentDtd = current_downtrack_; + double currentCtd = current_crosstrack_; + bool samelane = abs(currentCtd-crosstrack) <= config_.maxCrosstrackError; + + if (downtrack < currentDtd && samelane) + { + ROS_DEBUG_STREAM("Found a platoon at behind. We are able to join"); + return true; + } + else + { + ROS_DEBUG_STREAM("Ignoring platoon that is either ahead of us or in another lane."); + ROS_DEBUG_STREAM("The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); + return false; + } + } + + // Return the ecef point projected to local map point. + lanelet::BasicPoint2d PlatoonStrategicIHPPlugin::ecef_to_map_point(cav_msgs::LocationECEF ecef_point) + { + if (!map_projector_) + { + throw std::invalid_argument("No map projector available for ecef conversion"); + } + + lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , -1); + + lanelet::BasicPoint2d output {map_point.x(), map_point.y()}; + return output; + } + + // Build map projector from proj string (georefernce). + void PlatoonStrategicIHPPlugin::georeference_cb(const std_msgs::StringConstPtr& msg) + { + map_projector_ = std::make_shared(msg->data.c_str()); + } + + + //-------------------------------- Mobility Communication --------------------------------------// + + // ------ 1. compose Mobility Operation messages and platoon info ------ // + + // UCLA: Return a Mobility operation message with STATUS params. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationSTATUS() + { + /** + * Note: STATUS params format: + * STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%" + * |----------0----------1---------2---------3---------4------| + */ + + //TODO future: consider setting recipient_id to the platoon ID to make it obvious that anyone in that group is intended reader. + // This requires an architectural agreement on use of group messaging protocol. + + // Extract data + cav_msgs::MobilityOperation msg; + msg.m_header.plan_id = pm_.currentPlatoonID; + msg.m_header.recipient_id = ""; + std::string hostStaticId = config_.vehicleID; + msg.m_header.sender_id = hostStaticId; + msg.m_header.timestamp = ros::Time::now().toNSec() / 1000000; + msg.strategy = PLATOONING_STRATEGY; + + // form message + double cmdSpeed = cmd_speed_; + boost::format fmter(OPERATION_STATUS_PARAMS); + fmter% cmdSpeed; // index = 0, in m/s. + fmter% current_speed_; // index = 1, in m/s. + fmter% pose_ecef_point_.ecef_x; // index = 2, in cm. + fmter% pose_ecef_point_.ecef_y; // index = 3, in cm. + fmter% pose_ecef_point_.ecef_z; // index = 4, in cm. + + // compose message + std::string statusParams = fmter.str(); + msg.strategy_params = statusParams; + ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); + return msg; + } + + // UCLA: Return a Mobility operation message with INFO params. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationINFO() + { + /** + * Note: INFO param format: + * "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" + * |-------0-----------1---------2--------3----------4----------5-------| + */ + cav_msgs::MobilityOperation msg; + msg.m_header.plan_id = pm_.currentPlatoonID; // msg.m_header.plan_id is the platoon ID of the request sender (rear join and frontal join). + msg.m_header.recipient_id = ""; + std::string hostStaticId = config_.vehicleID; + msg.m_header.sender_id = hostStaticId; + msg.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + msg.strategy = PLATOONING_STRATEGY; + + double CurrentPlatoonLength = pm_.getCurrentPlatoonLength(); + int PlatoonSize = pm_.getHostPlatoonSize(); + + boost::format fmter(OPERATION_INFO_PARAMS); + // Note: need to update "OPERATION_INFO_PARAMS" in m_header file --> strategic_platoon_ihp.h + fmter% CurrentPlatoonLength; // index = 0, physical length of the platoon, in m. + fmter% current_speed_; // index = 1, in m/s. + fmter% PlatoonSize; // index = 2, number of members + fmter% pose_ecef_point_.ecef_x; // index = 3, in cm. + fmter% pose_ecef_point_.ecef_y; // index = 4, in cm. + fmter% pose_ecef_point_.ecef_z; // index = 5, in cm. + + std::string infoParams = fmter.str(); + msg.strategy_params = infoParams; + ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); + return msg; + } + + // ----------------------------- UCLA: helper functions for cut-in from front -------------------------------// + + // Note: The function "find_target_lanelet_id" was used to test the IHP platooning logic and is only a pre-written scenario. + // The IHP platooning should provide necessary data in a maneuver plan for the arbitrary lane change module. + + // This is the platoon leader's function to determine if the joining vehicle is closeby + bool PlatoonStrategicIHPPlugin::isJoiningVehicleNearPlatoon(double joining_downtrack, double joining_crosstrack) + { + // Position info of the host vehicle + double frontVehicleDtd = current_downtrack_; + double frontVehicleCtd = current_crosstrack_; + // platoon rear positions + int lastVehicleIndex = pm_.host_platoon_.size()-1; + double rearVehicleDtd = pm_.host_platoon_[lastVehicleIndex].vehiclePosition; + + // lateral error for two lanes (lane width was calculated based on current lanelet) + double two_lane_cross_error = 2*config_.maxCrosstrackError + findLaneWidth(); + // add longitudinal check threshold in config file + bool longitudinalCheck = joining_downtrack >= rearVehicleDtd - config_.longitudinalCheckThresold || + joining_downtrack <= frontVehicleDtd + config_.maxCutinGap; + bool lateralCheck = joining_crosstrack >= frontVehicleCtd - two_lane_cross_error || + joining_crosstrack <= frontVehicleCtd + two_lane_cross_error; + // logs for longitudinal and lateral check + ROS_DEBUG_STREAM("The longitudinalCheck result is: " << longitudinalCheck ); + ROS_DEBUG_STREAM("The lateralCheck result is: " << lateralCheck ); + + if (longitudinalCheck && lateralCheck) + { + ROS_DEBUG_STREAM("Joining vehicle is nearby. It is able to join."); + return true; + } + else + { + ROS_DEBUG_STREAM("The joining vehicle is not close by, the join request will not be approved."); + ROS_DEBUG_STREAM("The joining vehicle downtrack is " << joining_downtrack << " and the host (platoon leader) downtrack is " << frontVehicleDtd); + ROS_DEBUG_STREAM("The joining vehicle crosstrack is " << joining_crosstrack << " and the host (platoon leader) crosstrack is " << frontVehicleCtd); + return false; + } + } + + // Check if the host vehicle is close to the platoon for cut-in join. + bool PlatoonStrategicIHPPlugin::isVehicleNearTargetPlatoon(double rearVehicleDtd, double frontVehicleDtd, double frontVehicleCtd) + { + + // lateral error for two lanes (lane width was calculated based on current lanelet) + double two_lane_cross_error = 2*config_.maxCrosstrackError + findLaneWidth(); + // add longitudinal check threshold in config file + bool longitudinalCheck = current_downtrack_ >= rearVehicleDtd - config_.longitudinalCheckThresold || + current_downtrack_ <= frontVehicleDtd + config_.longitudinalCheckThresold; + bool lateralCheck = abs(current_crosstrack_ - frontVehicleCtd) <= two_lane_cross_error; + // current_crosstrack_ >= frontVehicleCtd - two_lane_cross_error || + // current_crosstrack_ <= frontVehicleCtd + two_lane_cross_error; + // logs for longitudinal and lateral check + ROS_DEBUG_STREAM("The longitudinalCheck result is: " << longitudinalCheck ); + ROS_DEBUG_STREAM("The lateralCheck result is: " << lateralCheck ); + + if (longitudinalCheck && lateralCheck) + { + // host vehicle is close to target platoon longitudinally (within 10m) and laterally (within 5m) + ROS_DEBUG_STREAM("Found a platoon nearby. We are able to join."); + return true; + } + else + { + ROS_DEBUG_STREAM("Ignoring platoon."); + ROS_DEBUG_STREAM("The platoon leader dtd is " << frontVehicleDtd << " and we are current at " << current_downtrack_); + ROS_DEBUG_STREAM("The platoon leader ctd is " << frontVehicleCtd << " and we are current at " << current_crosstrack_); + return false; + } + } + + // Compose platoon info msg for all states. + cav_msgs::PlatooningInfo PlatoonStrategicIHPPlugin::composePlatoonInfoMsg() + { + /** + * Note: There is a difference between the "platoon info status" versus the the "platoon strategic plugin states". + * The "platooning info status" reflect the overall operating status. + * The "platoon strategic plugin states" manage the negotiation strategies and vehicle communication in a more refined manner. + * A more detailed note can be found in the corresponding function declaration in "platoon_strategic_ihp.h" file. + */ + + cav_msgs::PlatooningInfo status_msg; + + if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + status_msg.state = cav_msgs::PlatooningInfo::DISABLED; + } + else if (pm_.current_platoon_state == PlatoonState::LEADER) + { + status_msg.state = pm_.getHostPlatoonSize() == 1 ? cav_msgs::PlatooningInfo::SEARCHING : cav_msgs::PlatooningInfo::LEADING; + } + else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + } + else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + } + else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + status_msg.state = cav_msgs::PlatooningInfo::FOLLOWING; + } + // UCLA: add leader aborting for frontal join (inherited from candidate follower). + else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + } + // UCLA: add candidate leader for frontal join (inherited from leader waiting). + else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + } + // UCLA: add "lead with operation" for frontal join (inherited from leader waiting). + else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + } + // UCLA: add "prepare to join" for frontal join (inherited from leader waiting). + else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) + { + status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + } + //TODO: Place holder for departure (PREPARE TO DEPART) + + // compose msgs for anything other than standby + if (pm_.current_platoon_state != PlatoonState::STANDBY) + { + status_msg.platoon_id = pm_.currentPlatoonID; + status_msg.size = pm_.getHostPlatoonSize(); + status_msg.size_limit = config_.maxPlatoonSize; + + if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + PlatoonMember platoon_leader = pm_.getDynamicLeader(); + status_msg.leader_id = platoon_leader.staticId; + status_msg.leader_downtrack_distance = platoon_leader.vehiclePosition; + status_msg.leader_cmd_speed = platoon_leader.commandSpeed; + status_msg.host_platoon_position = pm_.getNumberOfVehicleInFront(); + ROS_DEBUG_STREAM("pm platoonsize: " << pm_.getHostPlatoonSize() << ", platoon_leader " << platoon_leader.staticId); + + int numOfVehiclesGaps = pm_.getNumberOfVehicleInFront() - pm_.dynamic_leader_index_; + ROS_DEBUG_STREAM("The host vehicle have " << numOfVehiclesGaps << " vehicles between itself and its leader (includes the leader)"); + + // use current position to find lanelet ID + lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); + + auto llts = wm_->getLaneletsFromPoint(current_loc, 1); + + double lanelet_digitalgap = config_.standStillHeadway; + + if (!llts.empty()) + { + auto geofence_gaps = llts[0].regulatoryElementsAs(); + + if (!geofence_gaps.empty()) + { + lanelet_digitalgap = geofence_gaps[0]->getMinimumGap(); + } + } + + else + { + ROS_DEBUG_STREAM("No lanelets in this location!!!: "); + } + + ROS_DEBUG_STREAM("lanelet_digitalgap: " << lanelet_digitalgap); + double desired_headway = std::max(current_speed_ * config_.timeHeadway, lanelet_digitalgap); + ROS_DEBUG_STREAM("speed based gap: " << current_speed_ * config_.timeHeadway); + ROS_DEBUG_STREAM("max desired_headway " << desired_headway); + // TODO: currently the average length of the vehicle is obtained from a config parameter. In future, plugin needs to be updated to receive each vehicle's actual length through status or BSM messages for more accuracy. + status_msg.desired_gap = std::max(config_.standStillHeadway * numOfVehiclesGaps, desired_headway * numOfVehiclesGaps) + (numOfVehiclesGaps - 1) * 5.0;//config_.averageVehicleLength; + ROS_DEBUG_STREAM("The desired gap with the leader is " << status_msg.desired_gap); + + + // TODO: To uncomment the following lines, platooninfo msg must be updated + // UCLA: Add the value of the summation of "veh_len/veh_speed" for all predecessors + status_msg.current_predecessor_time_headway_sum = pm_.getPredecessorTimeHeadwaySum(); + // UCLA: preceding vehicle info + status_msg.predecessor_speed = pm_.getPredecessorSpeed(); + status_msg.predecessor_position = pm_.getPredecessorPosition(); + + // Note: use isCreateGap to adjust the desired gap send to control plugin + double regular_gap = status_msg.desired_gap; + ROS_DEBUG_STREAM("regular_gap: " << regular_gap); + ROS_DEBUG_STREAM("current_speed_: " << current_speed_); + ROS_DEBUG_STREAM("speed based gap: " << desired_headway); + if (pm_.isCreateGap){ + // enlarged desired gap for gap creation + status_msg.desired_gap = regular_gap*(1 + config_.createGapAdjuster); + } + status_msg.actual_gap = platoon_leader.vehiclePosition - current_downtrack_; + ROS_DEBUG_STREAM("status_msg.actual_gap: " << status_msg.actual_gap); + } + else + { + status_msg.leader_id = config_.vehicleID; + status_msg.leader_downtrack_distance = current_downtrack_; + status_msg.leader_cmd_speed = cmd_speed_; + status_msg.host_platoon_position = 0; + + } + + // This info is updated at platoon control plugin + status_msg.host_cmd_speed = cmd_speed_; + + } + return status_msg; + } + + // Compose the Mobility Operation message for leader state. Message parameter types: STATUS and INFO. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeader(const std::string& type) + { + cav_msgs::MobilityOperation msg; + + // info params + if (type == OPERATION_INFO_TYPE) + { + msg = composeMobilityOperationINFO(); + } + + // status params + else if (type == OPERATION_STATUS_TYPE) + { + msg = composeMobilityOperationSTATUS(); + } + // Unknown strategy param. + else + { + ROS_ERROR("UNKNOWN strategy param string!!!"); + msg.strategy_params = ""; + } + return msg; + } + + // Compose the Mobility Operation message for Follower state. Message parameter types: STATUS. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationFollower() + { + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + + // Compose the Mobility Operation message for LeaderWaiting state. Message parameter types: STATUS. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderWaiting() + { + //TODO: shouldn't a leaderwaiting also be sending INFO messages since it is still leading? + + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + + // Compose the Mobility Operation message for CandidateFollower state. Message parameter types: STATUS. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateFollower() + { + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + + // UCLA: add compose msgs for LeaderAborting (inherited from candidate follower). Message parameter types: STATUS. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderAborting() + { + /* + UCLA Implementation note: + Sending STATUS info for member updates and platoon trajectory regulation. + */ + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + + // UCLA: add compose msgs for CandidateLeader (inherited from leader waiting). Message parameter types: STATUS. + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateLeader() + { + /* + UCLA Implementation note: + This is the joiner which will later become the new leader, + host vehicle publish status msgs and waiting to lead the rear platoon + */ + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + + // UCLA: compose mobility message for prepare to join (cut-in join state, inherited from follower state's compose mob_op) + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationPrepareToJoin() + { + /* + UCLA Implementation note: + This is the joiner that is preapring for cut-in join. + host vehicle publish status msgs and waiting to lead the rear platoon. + */ + cav_msgs::MobilityOperation msg; + msg = composeMobilityOperationSTATUS(); + return msg; + } + // UCLA: compose mobility message for leading with operation (cut-in join state, inherited from leader state's compose mob_op) + cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeadWithOperation(const std::string& type) + { + cav_msgs::MobilityOperation msg; + + // info params + if (type == OPERATION_INFO_TYPE) + { + msg = composeMobilityOperationINFO(); + } + + // status params + else if (type == OPERATION_STATUS_TYPE) + { + msg = composeMobilityOperationSTATUS(); + } + // Unknown strategy param. + else + { + ROS_ERROR("UNKNOWN strategy param string!!!"); + msg.strategy_params = ""; + } + return msg; + } + + // TODO: Place holder for compose mobility operation msg for prepare to depart. + + // ------ 2. Mobility operation callback ------ // + + // read ecef pose from STATUS + cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_STATUS_params(std::string strategyParams) + { + /* + * Helper function that extract ecef location from STATUS msg. + * Note: STATUS params format: + * STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%" + * |----------0----------1---------2---------3---------4------| + */ + + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + + std::vector ecef_x_parsed; + boost::algorithm::split(ecef_x_parsed, inputsParams[2], boost::is_any_of(":")); + double ecef_x = std::stod(ecef_x_parsed[1]); + + std::vector ecef_y_parsed; + boost::algorithm::split(ecef_y_parsed, inputsParams[3], boost::is_any_of(":")); + double ecef_y = std::stod(ecef_y_parsed[1]); + + std::vector ecef_z_parsed; + boost::algorithm::split(ecef_z_parsed, inputsParams[4], boost::is_any_of(":")); + double ecef_z = std::stod(ecef_z_parsed[1]); + + cav_msgs::LocationECEF ecef_loc; + ecef_loc.ecef_x = ecef_x; + ecef_loc.ecef_y = ecef_y; + ecef_loc.ecef_z = ecef_z; + + return ecef_loc; + } + + // UCLA: Handle STATUS operation messages + void PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(const cav_msgs::MobilityOperation& msg) + { + /** + * Note: STATUS params format: + * STATUS | --> "CMDSPEED:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%" + * |----------0----------1---------2---------3---------4------| + */ + + ROS_DEBUG_STREAM("Entered mob_op_cb_STATUS"); + std::string strategyParams = msg.strategy_params; + std::string vehicleID = msg.m_header.sender_id; + std::string platoonId = msg.m_header.plan_id; + ROS_DEBUG_STREAM("strategyParams = " << strategyParams); + ROS_DEBUG_STREAM("platoonId = " << platoonId << ", sender ID = " << vehicleID); + std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1); + ROS_DEBUG_STREAM("pm_.currentPlatoonID = " << pm_.currentPlatoonID << ", targetPlatoonID = " << pm_.targetPlatoonID); + + // read Downtrack + cav_msgs::LocationECEF ecef_loc = mob_op_find_ecef_from_STATUS_params(strategyParams); + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); + double dtd = wm_->routeTrackPos(incoming_pose).downtrack; + ROS_DEBUG_STREAM("DTD calculated from ecef is: " << dtd); + // read Crosstrack + double ctd = wm_->routeTrackPos(incoming_pose).crosstrack; + ROS_DEBUG_STREAM("CTD calculated from ecef is: " << ctd); + + // If it comes from a member of an identified neighbor platoon, then + if (platoonId.compare(pm_.neighborPlatoonID) == 0 && platoonId.compare(pm_.dummyID) != 0) + { + ROS_DEBUG_STREAM("Incoming platoonID matches target platoon id"); + // // Update this member's status (or add if it's unknown to us) + pm_.neighborMemberUpdates(vehicleID, platoonId, statusParams, dtd, ctd); + } + + // else if this message is for our platoon then store its info + else if (platoonId.compare(pm_.currentPlatoonID) == 0 && platoonId.compare(pm_.dummyID) != 0) + { + pm_.hostMemberUpdates(vehicleID, platoonId, statusParams, dtd, ctd); + } + + // else it represents an uninteresting platoon + else + { + ROS_DEBUG_STREAM("Received mob op for platoon " << platoonId << " that doesn't match our platoon: " << pm_.currentPlatoonID + << " or known neighbor platoon: " << pm_.targetPlatoonID); + } + } + + // + double PlatoonStrategicIHPPlugin::mob_op_find_platoon_length_from_INFO_params(std::string strategyParams) + { + /** + * Note: INFO param format: + * "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" + * |-------0-----------1---------2--------3----------4----------5-------| + */ + // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + + // Use the strategy params' length value and leader location to determine DTD of its rear + std::vector target_platoon_length; + boost::algorithm::split(target_platoon_length, inputsParams[0], boost::is_any_of(":")); + double platoon_length = std::stod(target_platoon_length[1]); + + return platoon_length; + } + + // UCLA: Parse ecef location from INFO params + cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_INFO_params(std::string strategyParams) + { + /** + * Note: INFO param format: + * "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" + * |-------0-----------1---------2--------3----------4----------5-------| + */ + // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + + std::vector ecef_x_parsed; + boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":")); + double ecef_x = std::stod(ecef_x_parsed[1]); + + std::vector ecef_y_parsed; + boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":")); + double ecef_y = std::stod(ecef_y_parsed[1]); + + std::vector ecef_z_parsed; + boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); + double ecef_z = std::stod(ecef_z_parsed[1]); + + cav_msgs::LocationECEF ecef_loc; + ecef_loc.ecef_x = ecef_x; + ecef_loc.ecef_y = ecef_y; + ecef_loc.ecef_z = ecef_z; + + return ecef_loc; + } + + // handle message for each states. + void PlatoonStrategicIHPPlugin::mob_op_cb(const cav_msgs::MobilityOperation& msg) + { + if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + return; + } + + ROS_DEBUG_STREAM("mob_op_cb received msg with sender ID " << msg.m_header.sender_id + << ", plan ID " << msg.m_header.plan_id); + ROS_DEBUG_STREAM("...strategy " << msg.strategy << ", strategy params " << msg.strategy_params); + + // Check that this is a message about platooning (could be from some other Carma activity nearby) + std::string strategy = msg.strategy; + if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) + { + ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + return; + } + + // Ignore messages as long as host vehicle is stopped + if (current_speed_ < STOPPED_SPEED) + { + ROS_DEBUG_STREAM("Ignoring message since host is stopped."); + return; + } + + // Perform common operations that apply to all states + std::string strategyParams = msg.strategy_params; + bool isPlatoonStatusMsg = strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0; + bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0; + ROS_DEBUG_STREAM("strategyParams: " << strategyParams << "isPlatoonStatusMsg: " << isPlatoonStatusMsg << "isPlatoonInfoMsg: " << isPlatoonInfoMsg); + if (isPlatoonStatusMsg) + { + mob_op_cb_STATUS(msg); + } + else if (isPlatoonInfoMsg) + { + // Note: this is where confusion will reign if multiple neighbor platoons are discovered; need more sophisticated + // logic to handle that situation + + // If it is a legitimate platoon (2 or more members) other than our own then + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + std::vector p_size; + boost::algorithm::split(p_size, inputsParams[2], boost::is_any_of(":")); + int platoon_size = std::stoi(p_size[1]); + ROS_DEBUG_STREAM("neighbor platoon_size from INFO: " << platoon_size); + if (platoon_size > 1 && msg.m_header.plan_id.compare(pm_.currentPlatoonID) != 0) + { + // If platoon ID doesn't match our known target platoon then clear any old neighbor platoon info and record + // the platoon ID and the sender as the leader (only leaders send INFO) + if (msg.m_header.plan_id.compare(pm_.neighborPlatoonID) != 0) + { + pm_.resetNeighborPlatoon(); + pm_.neighborPlatoonID = msg.m_header.plan_id; + } + pm_.neighbor_platoon_leader_id_ = msg.m_header.sender_id; + ROS_DEBUG_STREAM("pm_.neighbor_platoon_leader_id_: " << pm_.neighbor_platoon_leader_id_); + pm_.neighbor_platoon_info_size_ = platoon_size; + ROS_DEBUG_STREAM("pm_.neighbor_platoon_info_size_: " << pm_.neighbor_platoon_info_size_); + } + } + + else + { + ROS_DEBUG_STREAM("Invalid Mob Op received"); + } + + // Perform state-specific additional actions + if (pm_.current_platoon_state == PlatoonState::LEADER) + { + mob_op_cb_leader(msg); + } + else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + mob_op_cb_follower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) + { + mob_op_cb_candidatefollower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) + { + mob_op_cb_leaderwaiting(msg); + } + else if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + mob_op_cb_standby(msg); + } + // UCLA: add leader aborting + else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) + { + mob_op_cb_leaderaborting(msg); + } + // UCLA: add candidate leader + else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) + { + mob_op_cb_candidateleader(msg); + } + // UCLA: add lead with operation for cut-in join + else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) + { + mob_op_cb_leadwithoperation(msg); + } + // UCLA: add prepare to join for cut-in join + else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) + { + mob_op_cb_preparetojoin(msg); + } + // TODO: Place holder for prepare to depart + + // TODO: If needed (with large size platoons), add a queue for status messages + // INFO messages always processed, STATUS messages if saved in que + } + + void PlatoonStrategicIHPPlugin::mob_op_cb_standby(const cav_msgs::MobilityOperation& msg) + { + // In standby state, it will ignore operation message since it is not actively operating + ROS_DEBUG_STREAM("STANDBY state no further action on message from " << msg.m_header.sender_id); + } + + // Handle STATUS operation message + void PlatoonStrategicIHPPlugin::mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg) + { + ROS_DEBUG_STREAM("CANDIDATEFOLLOWER state no further action on message from " << msg.m_header.sender_id); + } + + // Handle STATUS operation message + void PlatoonStrategicIHPPlugin::mob_op_cb_follower(const cav_msgs::MobilityOperation& msg) + { + //std::string strategyParams = msg.strategy_params; + //bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); + + // TODO: Place holder for prepare to depart + + ROS_DEBUG_STREAM("FOLLOWER state no further action on message from " << msg.m_header.sender_id); + } + + // Handle STATUS operation message + void PlatoonStrategicIHPPlugin::mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg) + { + ROS_DEBUG_STREAM("LEADERWAITING state no further action on message from " << msg.m_header.sender_id); + } + + // UCLA: Handle both STATUS and INFO operation message. Front join and rear join are all handled if incoming operation message have INFO param. + void PlatoonStrategicIHPPlugin::mob_op_cb_leader(const cav_msgs::MobilityOperation& msg) + { + /** + * Note: This is the function to handle the mobility operation message. Vehicle in leader state is either a single ADS vehicle or a platoon leader. + * + * Single ADS will send out INFO messages. Platoon leaders will send out both INFO and STATUS messages. + * + * If the host is single vehicle, it should have "isPlatoonInfoMsg = true" and "isInNegotiation = false". In such condition, single leader will start + * joining and send request to the platoon leader. mob_op_cb_leader( + * + * If the host vehicle is platoon leader, then it should have "isPlatoonInfoMsg = true" and "isInNegotiation = true". But existing platoon leader do not + * need to send out joining request. + * + * Both the platoon leader and single vehicle need to subscribe to the STATUS message to populate the platoon manager with existing platoon members, + * so the PM can calculate dtd and ctd corresponds to host vehicle's origin position, hence to be used for later calculation. + */ + + // Read incoming message info + std::string strategyParams = msg.strategy_params; + std::string senderId = msg.m_header.sender_id; + std::string platoonId = msg.m_header.plan_id; + + // In the current state, host vehicle care about the INFO heart-beat operation message if we are not currently in + // a negotiation, and host also need to care about operation from members in our current platoon. + bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0; // INFO message only broadcast by leader and single CAV. + bool isInNegotiation = pm_.current_plan.valid || pm_.currentPlatoonID.compare(pm_.dummyID) != 0; // In negotiation indicates host is not available to become a joiner + // (i.e., not currently in a platoon or trying to join a platoon). + ROS_DEBUG_STREAM("Top of mob_op_cb_leader, isInNegotiation = " << isInNegotiation); + + // Condition 1. Host vehicle is the single CAV joining the platoon. + if (isPlatoonInfoMsg && !isInNegotiation) + { + //TODO future enhancement: add logic here for if/how to join two existing platoons + // (e.g. the shorter platoon should join the longer one, or rear joins front if same length) + + // step 1. read INFO message from the target platoon leader + + // read ecef location from strategy params. + cav_msgs::LocationECEF ecef_loc; + ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams); + + // use ecef_loc to calculate front Dtd in m. + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); + double frontVehicleDtd = wm_->routeTrackPos(incoming_pose).downtrack; + + // use ecef_loc to calculate front Ctd in m. + double frontVehicleCtd = wm_->routeTrackPos(incoming_pose).crosstrack; + // downtrack and crosstrack of the platoon leader --> used for frontal join + ROS_DEBUG_STREAM("Neighbor platoon frontVehicleDtd from ecef: " << frontVehicleDtd << ", frontVehicleCtd from ecef: " << frontVehicleCtd); + + // use INFO param to find platoon rear vehicle DTD and CTD. + double platoon_length = mob_op_find_platoon_length_from_INFO_params(strategyParams); // length of the entire platoon in meters. + /** + * note: + * [**veh3*] ---------- [*veh2*] ----------------- [*veh1*] ---------- [*veh0**] + * |<------------------ front-rear DTD difference -------------------->| + * |<----------------------------- platoon length ---------------------------->| + * + * front-rear DTD difference = platoon_length + one_vehicle_length + * Vehicle length is already accounted for in the message's LENGTH value + */ + + double rearVehicleDtd = frontVehicleDtd - platoon_length; + ROS_DEBUG_STREAM("rear veh dtd from platoon length: " << rearVehicleDtd); + if (!pm_.neighbor_platoon_.empty()) + { + rearVehicleDtd = pm_.neighbor_platoon_.back().vehiclePosition; + ROS_DEBUG_STREAM("rear veh dtd from neighbor platoon: " << rearVehicleDtd); + } + + // Note: For one platoon, we assume all members are in the same lane. + double rearVehicleCtd = frontVehicleCtd; + ROS_DEBUG_STREAM("Neighbor platoon rearVehicleDtd: " << rearVehicleDtd << ", rearVehicleCtd: " << rearVehicleCtd); + + // Parse the strategy params + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + + // Get the target platoon's size (number of members) from strategy params + std::vector targetPlatoonSize_parsed; + boost::algorithm::split(targetPlatoonSize_parsed, inputsParams[2], boost::is_any_of(":")); + int targetPlatoonSize = std::stoi(targetPlatoonSize_parsed[1]); + ROS_DEBUG_STREAM("target Platoon Size: " << targetPlatoonSize); + ROS_DEBUG_STREAM("Found a vehicle/platoon with id = " << platoonId << " within range."); + + //TODO future: add logic here to assess closeness of the neighbor platoon, as well as its speed, destination + // & other attributes to decide if we want to join before assembling a join request + + // step 2. Generate default info for join request + cav_msgs::MobilityRequest request; + request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); + request.m_header.recipient_id = senderId; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = ros::Time::now().toNSec()/1000000; + request.location = pose_to_ecef(pose_msg_); + request.strategy = PLATOONING_STRATEGY; + request.urgency = 50; + + int platoon_size = pm_.getHostPlatoonSize(); + + // step 3. Request Rear Join + if(isVehicleRightInFront(rearVehicleDtd, rearVehicleCtd) && !config_.test_front_join) + { + /** + * Note: "isVehicleRightInFront" tests for same lane + */ + ROS_DEBUG_STREAM("Neighbor platoon is right in front of us"); + + request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; + + /* + * JOIN_PARAMS format: + * JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%" + * |-------0------ --1---------2---------3---------4----------5-------| + */ + + boost::format fmter(JOIN_PARAMS); + int dummy_join_index = -2; //not used for this message, but message spec requires it + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2, in cm. + fmter %pose_ecef_point_.ecef_y; // index = 3, in cm. + fmter %pose_ecef_point_.ecef_z; // index = 4, in cm. + fmter %dummy_join_index; // index = 5 + request.strategy_params = fmter.str(); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Publishing request to leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + + // Create a new join plan + pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); + + // If we are asking to join an actual platoon (not a solo vehicle), then save its ID for later use + if (platoonId.compare(pm_.dummyID) != 0) + { + pm_.targetPlatoonID = platoonId; + ROS_DEBUG_STREAM("Detected neighbor as a real platoon & storing its ID: " << platoonId); + } + } + + // step 4. Request frontal join, if the neighbor is a real platoon + else if ((targetPlatoonSize > 1 || config_.test_front_join) && isVehicleRightBehind(frontVehicleDtd, frontVehicleCtd)) + { + /** + * Note: "isVehicleRightBehind" tests for same lane + */ + ROS_DEBUG_STREAM("Neighbor platoon leader is right behind us"); + + // UCLA: assign a new plan type + request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT; + + /** + * JOIN_PARAMS format: + * JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%" + * |-------0------ --1---------2---------3---------4----------5-------| + */ + boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition. + int dummy_join_index = -2; //not used for this message, but message spec requires it + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2, in cm. + fmter %pose_ecef_point_.ecef_y; // index = 3, in cm. + fmter %pose_ecef_point_.ecef_z; // index = 4, in cm. + fmter %dummy_join_index; // index = 5 + request.strategy_params = fmter.str(); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Publishing front join request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + + // Create a new join plan + pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); + + // If testing with a solo vehicle to represent the target platoon, then use the current join plan ID for that platoon; + // otherwise, it will already have and ID so store it for future use + if (config_.test_front_join) + { + pm_.targetPlatoonID = request.m_header.plan_id; + ROS_DEBUG_STREAM("Since neighbor is a fake platoon, storing " << pm_.targetPlatoonID << " as its platoon ID"); + } + else + { + pm_.targetPlatoonID = platoonId; + ROS_DEBUG_STREAM("Storing real neighbor platoon's ID as target: " << pm_.targetPlatoonID); + } + } + + // step 5. Request cut-in join (front, middle or rear, from adjacent lane) + else if ((targetPlatoonSize > 1 || config_.test_cutin_join) && !config_.test_front_join + && isVehicleNearTargetPlatoon(rearVehicleDtd, frontVehicleDtd, frontVehicleCtd)) + { + + ROS_DEBUG_STREAM("starting cut-in join process"); + ROS_DEBUG_STREAM("rearVehicleDtd " << rearVehicleDtd); + ROS_DEBUG_STREAM("rearVehicleCtd " << rearVehicleCtd); + + // If we are asking to join an actual platoon (not a solo vehicle), then save its ID for later use + if (platoonId.compare(pm_.dummyID) != 0) + { + pm_.targetPlatoonID = platoonId; + ROS_DEBUG_STREAM("Detected neighbor as a real platoon & storing its ID: " << platoonId); + } + + + carma_wm::TrackPos target_trackpose(rearVehicleDtd, rearVehicleCtd); + auto target_rear_pose = wm_->pointFromRouteTrackPos(target_trackpose); + if (target_rear_pose) + { + target_cutin_pose_ = incoming_pose; + + auto target_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, target_rear_pose.get(), 1); + if (!target_lanelets.empty()) + { + long target_rear_pose_lanelet_id = target_lanelets[0].second.id(); + ROS_DEBUG_STREAM("target_rear_pose_lanelet_id: " << target_rear_pose_lanelet_id); + } + else + { + ROS_DEBUG_STREAM("target_rear_pose_lanelet not found!!"); + } + } + + else + { + ROS_DEBUG_STREAM("No target pose is found, so we cannot prodeed with a cutin join request."); + return; + } + + /** + * UCLA Implementation note: + * 1. isVehicleNearTargetPlatoon --> determine if the platoon is next to host vehicle + * 2. sender_id == requesting veh ID --> joiner ID + */ + + //TODO: verify purpose of this logic then its correctness accordingly: + // -is step 5 (this block) really for all 3 types of joining? + // -ensure join_index gets an appropriate value for the type of join + // -ensure plan type corresponds to join_index + + // complete the request + // UCLA: this is a newly added plan type + // Note: Request conposed outside of if conditions + // UCLA: Desired joining index for cut-in join, indicate the index of gap-leading vehicle. -1 indicate cut-in from front. + // Note: remove join_index to info param. + request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + + // At this step all cut-in types start with this request, so the join_index at this point is set to default, -2. + int join_index = -2; + boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition. + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %join_index; // index = 5 + request.strategy_params = fmter.str(); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Publishing request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + + // Create a new join plan + pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); + } + + // step 6. Return none if no platoon nearby + else + { + ROS_DEBUG_STREAM("Ignore platoon with platoon id: " << platoonId << " because it is too far away to join."); + } + } + + // TODO: Place holder for prepare to depart (else if isdepart) + + } + + // UCLA: mob_op_cb for the new leader aborting state (inherited from candidate follower), handle STATUS message. + void PlatoonStrategicIHPPlugin::mob_op_cb_leaderaborting(const cav_msgs::MobilityOperation& msg) + { + ROS_DEBUG_STREAM("LEADERABORTING state no further action on message from " << msg.m_header.sender_id); + } + + // UCLA: mob_op_candidateleader for the new candidate leader state (inherited from leader waiting), handle STATUS message. + void PlatoonStrategicIHPPlugin::mob_op_cb_candidateleader(const cav_msgs::MobilityOperation& msg) + { + ROS_DEBUG_STREAM("CANDIDATELEADER state no further action on message from " << msg.m_header.sender_id); + } + + // UCLA: Mobility operation callback for lead_with_operation state (cut-in join). + void PlatoonStrategicIHPPlugin::mob_op_cb_leadwithoperation(const cav_msgs::MobilityOperation& msg) + { + ROS_DEBUG_STREAM("LEADWITHOPERATION state no further action on message from " << msg.m_header.sender_id); + } + + // UCLA: Mobility operation callback for prepare to join state (cut-in join). + void PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(const cav_msgs::MobilityOperation& msg) + { + /* + * If same lane with leader, then send request to do same lane join. + * Otherwise, just send status params. + * Note: leader in state 'leading with operation' also send out INFO msg + */ + + // read parameters + std::string strategyParams = msg.strategy_params; + std::string senderId = msg.m_header.sender_id; + + // locate INFO type + bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0; + + // If this is an INFO message and our record of the neighbor platoon is complete then + // pm_.is_neighbor_record_complete_ = true; //TODO temporary + ROS_DEBUG_STREAM("pm_.is_neighbor_record_complete_" << pm_.is_neighbor_record_complete_); + + if (isPlatoonInfoMsg && pm_.is_neighbor_record_complete_) + { + + //TODO: would be good to have a timeout here; if a neighbor platoon has been identified, and no INFO messages + // from it are received in a while, then its record should be erased, and any in-work joining should be + // aborted. + + // read ecef location from strategy params. + cav_msgs::LocationECEF ecef_loc; + ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams); + // use ecef_loc to calculate front Dtd in m. + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); + + double frontVehicleDtd = wm_->routeTrackPos(incoming_pose).downtrack; + + // use ecef_loc to calculate front Ctd in m. + double frontVehicleCtd = wm_->routeTrackPos(incoming_pose).crosstrack; + + // // Find neighbor platoon end vehicle and its downtrack in m + int rearVehicleIndex = pm_.neighbor_platoon_.size() - 1; + ROS_DEBUG_STREAM("rearVehicleIndex: " << rearVehicleIndex); + double rearVehicleDtd = pm_.neighbor_platoon_[rearVehicleIndex].vehiclePosition; + ROS_DEBUG_STREAM("Neighbor rearVehicleDtd from ecef: " << rearVehicleDtd); + + // If lane change has not yet been authorized, stop here (this method will be running before the negotiations + // with the platoon leader are complete) + if (!safeToLaneChange_) + { + return; + } + + // determine if the lane change is finished + bool isSameLaneWithPlatoon = abs(frontVehicleCtd - current_crosstrack_) <= config_.maxCrosstrackError; + ROS_DEBUG_STREAM("Lane change has been authorized. isSameLaneWithPlatoon = " << isSameLaneWithPlatoon); + ROS_DEBUG_STREAM("crosstrack diff" << abs(frontVehicleCtd - current_crosstrack_)); + if (isSameLaneWithPlatoon) + { + // request 1. reset the safeToChangLane indicators if lane change is finished + safeToLaneChange_ = false; + + // request 2. change to same lane operation states (determine based on DTD differences) + cav_msgs::MobilityRequest request; + request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); + request.m_header.recipient_id = senderId; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = ros::Time::now().toNSec()/1000000; + request.location = pose_to_ecef(pose_msg_); + + // UCLA: send request based on cut-in type + if (frontVehicleDtd < current_downtrack_) + { + request.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + } + else + { + request.plan_type.type = cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE; + } + request.strategy = PLATOONING_STRATEGY; + double host_platoon_size = pm_.getHostPlatoonSize(); + + boost::format fmter(JOIN_PARAMS); + fmter %host_platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %target_join_index_; // index = 5 + request.strategy_params = fmter.str(); + request.urgency = 50; + + mobility_request_publisher_(request); + if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0) + { + pm_.currentPlatoonID = request.m_header.plan_id; + } + + ROS_DEBUG_STREAM("new platoon id: " << pm_.currentPlatoonID); + pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); + + ROS_DEBUG_STREAM("Published Mobility request to revert to same-lane operation"); + } + else + { + ROS_DEBUG_STREAM("Lane Change not completed"); + } + } + } + + // TODO: Place holder for prepare to depart (mob_op_cb_depart) + + //------- 3. Mobility Request Callback ------- + MobilityRequestResponse PlatoonStrategicIHPPlugin::handle_mob_req(const cav_msgs::MobilityRequest& msg) + { + MobilityRequestResponse mobility_response = MobilityRequestResponse::NO_RESPONSE; + + // Check that this is a message about platooning (could be from some other Carma activity nearby) + std::string strategy = msg.strategy; + if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) + { + ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + return MobilityRequestResponse::NO_RESPONSE; + } + + // Handle the message based on host's current state + if (pm_.current_platoon_state == PlatoonState::LEADER) + { + mobility_response = mob_req_cb_leader(msg); + } + else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + mobility_response = mob_req_cb_follower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) + { + mobility_response = mob_req_cb_candidatefollower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) + { + mobility_response = mob_req_cb_leaderwaiting(msg); + } + else if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + mobility_response = mob_req_cb_standby(msg); + } + // UCLA: leader aborting + else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) + { + mobility_response = mob_req_cb_leaderaborting(msg); + } + // UCLA: candidate leader + else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) + { + mobility_response = mob_req_cb_candidateleader(msg); + } + + // UCLA: lead with operation (for cut-in join) + else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) + { + mobility_response = mob_req_cb_leadwithoperation(msg); + } + // UCLA: prepare to join (for cut-in join) + else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) + { + mobility_response = mob_req_cb_preparetojoin(msg); + } + // TODO: Place holder for prepare to depart + + return mobility_response; + } + + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_standby(const cav_msgs::MobilityRequest& msg) + { + // In standby state, the plugin is not responsible for replying to any request messages + ROS_DEBUG_STREAM("STANDBY state does nothing with msg from " << msg.m_header.sender_id); + return MobilityRequestResponse::NO_RESPONSE; + } + + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg) + { + // This state does not handle any mobility request for now + // TODO Maybe it should handle some ABORT request from a waiting leader + ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + return MobilityRequestResponse::NO_RESPONSE; + } + + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_follower(const cav_msgs::MobilityRequest& msg) + { + /** + * For cut-in join, the gap rear vehicle need to slow down once they received the request from platoon leader. + * Note:1. (cut-in) When joiner is in position, the leader will send the request to relevent platoon member. So no need to + * check for "in-position" one more time. + * 2. (cut-in) If a new member is added in middle, "mob_op_cb_STATUS" will update and resort all vehicle information. + * So the ordering of the platoon can be updated. + * 3. (depart) If the current leader is departing, the closest follower (i.e., 2nd in platoon) will become the new leader, + * and switch to candidate follower state. While the previous leader depart and operating in single leader state. + */ + + cav_msgs::PlanType plan_type = msg.plan_type; + std::string reccipientID = msg.m_header.recipient_id; + std::string reqSenderID = msg.m_header.sender_id; + + // Check joining plan type. + bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + bool isGapCreated = plan_type.type == cav_msgs::PlanType::STOP_CREATE_GAP; + // TODO: Place holder for departure + + // Check if host is intended recipient + bool isHostRecipent = pm_.getHostStaticID() == reccipientID; + + if (isCutInJoin && isHostRecipent) + { + // Read requesting vehicle's joining index + std::string strategyParams = msg.strategy_params; + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + std::vector join_index_parsed; + boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":")); + int req_sender_join_index = std::stoi(join_index_parsed[1]); + ROS_DEBUG_STREAM("Requesting join_index parsed: " << req_sender_join_index); + + // Control vehicle speed based on cut-in type + // 1. cut-in from rear + if (static_cast(req_sender_join_index) == pm_.host_platoon_.size()-1) + { + // Accept plan and idle (becasue rear join, gap leading vehicle do not need to slow down). + ROS_WARN("Requested cut-in from rear, start approve cut-in and wait for lane change."); + ROS_WARN("Due to the rear join nature, there is no need to slow down or create gap."); + return MobilityRequestResponse::ACK; + + } + // 2. cut-in from middle + else if (req_sender_join_index >= 0 && static_cast(req_sender_join_index) < pm_.host_platoon_.size()-1) + { + // Accept plan and slow down to create gap. + pm_.isCreateGap = true; + ROS_DEBUG_STREAM("Requested cut-in index is: " << req_sender_join_index << ", approve cut-in and start create gap."); + return MobilityRequestResponse::ACK; + } + // 3. Abnormal join index + else + { + // Note: Leader will abort plan if reponse is not ACK for plantype "PLATOON_CUT_IN_JOIN". + ROS_DEBUG_STREAM("Abnormal cut-in index, abort operation."); + return MobilityRequestResponse::NACK; + } + } + + + // 4. Reset to normal speed once the gap is created. + else if (isGapCreated) + { + ROS_DEBUG_STREAM("Gap is created, revert to normal operating speed."); + // Only reset create gap indicator, no need to send response. + pm_.isCreateGap = false; + return MobilityRequestResponse::NO_RESPONSE; + } + + // 5. Place holder for departure (host is follower) + + // 6. Same-lane join, no need to respond. + else + { + return MobilityRequestResponse::NO_RESPONSE; + } + } + + // Middle state that decided whether to accept joiner + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg) + { + bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; + bool isCandidateJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); + double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack; + bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError; + ROS_DEBUG_STREAM("current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); + ROS_DEBUG_STREAM("inTheSameLane = " << inTheSameLane); + + // If everything is agreeable then approve the request; if it is from an unexpected vehicle or + // is not a candidate join request, then we can just ignore it with no action + MobilityRequestResponse response = MobilityRequestResponse::NO_RESPONSE; + if (isTargetVehicle && isCandidateJoin) + { + if (inTheSameLane) + { + ROS_DEBUG_STREAM("Target vehicle " << pm_.current_plan.peerId << " is actually joining."); + ROS_DEBUG_STREAM("Changing to PlatoonLeaderState and send ACK to target vehicle"); + + // Change state to LEADER + pm_.current_platoon_state = PlatoonState::LEADER; + + // If we are not already in a platoon, then use this activity plan ID for the newly formed platoon + if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0) + { + pm_.currentPlatoonID = msg.m_header.plan_id; + } + + // Add the joiner to our platoon record (ASSUMES that we have not yet received a mob_op STATUS msg from joiner!) + PlatoonMember newMember = PlatoonMember(); + newMember.staticId = msg.m_header.sender_id; + newMember.vehiclePosition = wm_->routeTrackPos(incoming_pose).downtrack; + ROS_DEBUG_STREAM("New member being added to platoon vector whose size is currently " << pm_.host_platoon_.size()); + pm_.host_platoon_.push_back(newMember); + ROS_DEBUG_STREAM("pm_ now thinks platoon size is " << pm_.getHostPlatoonSize()); + + // Send approval of the request + response = MobilityRequestResponse::ACK; + // Indicate the current join activity is complete + pm_.clearActionPlan(); + } + else //correct vehicle and intent, but it's in the wrong lane + { + ROS_DEBUG_STREAM("Received platoon request with vehicle id = " << msg.m_header.sender_id << " but in wrong lane. NACK"); + response = MobilityRequestResponse::NACK; + + // // Remove the candidate joiner from the platoon structure + // if (!pm_.removeMemberById(msg.m_header.sender_id)) + // { + // ROS_DEBUG_STREAM("Failed to remove candidate joiner from platoon record: " << msg.m_header.sender_id); + // } + } + } + + + return response; + } + + // UCLA: add condition to handle frontal join request + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leader(const cav_msgs::MobilityRequest& msg) + { + /** + * UCLA implementation note: + * 1. Here the mobility requests of joining platoon (front and rear) get processed and responded. + * 2. The host is the leader of the existing platoon or single vehicle in default leader state. + * 3. Request sender is the joiner. + * 4. when two single vehicle meet, only allow backjoin. + * + * Note: For front and rear jon, the JOIN_PARAMS format: + * JOIN_PARAMS| --> "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%" + * |-------0------ --1---------2---------3---------4----------5-------| + */ + + // Check joining plan type. + cav_msgs::PlanType plan_type = msg.plan_type; + /** + * Note: + * JOIN_FROM_FRONT indicate a same-lane front join. + * JOIN_PLATOON_AT_REAR indicate a same-lane rear join. + * PLATOON_CUT_IN_JOIN indicate a cut-in join, which include three cut-in methods: cut-in front, cut-in middle, and cut-in rear. + */ + bool isFrontJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT; + bool isRearJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; + bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + bool isDepart = (plan_type.type == cav_msgs::PlanType::PLATOON_DEPARTURE); + + // Ignore the request if we are already working with a join/departure process or if no join type was requested (prevents multiple applicants) + if (isFrontJoin || isRearJoin || isCutInJoin || isDepart) + { + if (pm_.current_plan.valid){ + ROS_DEBUG_STREAM("Ignoring incoming request since we are already negotiating a join."); + return MobilityRequestResponse::NO_RESPONSE; //TODO: replace with NACK that indicates to ask me later + } + } + else + { + ROS_WARN_STREAM("Received request with bogus message type " << plan_type.type << "; ignoring"); + return MobilityRequestResponse::NO_RESPONSE; + } + + // TODO: Generalize - We currently ignore the lane information for now and assume the applicant is in the same lane with us. + // Determine intra-platoon conditions + // We are currently checking two basic JOIN conditions: + // 1. The size limitation on current platoon based on the plugin's parameters. + // 2. Calculate how long that vehicle can be in a reasonable distance to actually join us. + cav_msgs::MobilityHeader msgHeader = msg.m_header; + std::string params = msg.strategy_params; + std::string applicantId = msgHeader.sender_id; + ROS_DEBUG_STREAM("Received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id); + ROS_DEBUG_STREAM("The strategy parameters are " << params); + if (params.length() == 0) + { + ROS_DEBUG_STREAM("The strategy parameters are empty, return no response"); + return MobilityRequestResponse::NO_RESPONSE; + } + + // The incoming message is "mobility Request", which has a location category. + std::vector inputsParams; + boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); + + // Parse applicantSize + std::vector applicantSize_parsed; + boost::algorithm::split(applicantSize_parsed, inputsParams[0], boost::is_any_of(":")); + int applicantSize = std::stoi(applicantSize_parsed[1]); + ROS_DEBUG_STREAM("applicantSize: " << applicantSize); + + // Parse applicant Current Speed in m/s + std::vector applicantCurrentSpeed_parsed; + boost::algorithm::split(applicantCurrentSpeed_parsed, inputsParams[1], boost::is_any_of(":")); + double applicantCurrentSpeed = std::stod(applicantCurrentSpeed_parsed[1]); + ROS_DEBUG_STREAM("applicantCurrentSpeed: " << applicantCurrentSpeed); + + // Calculate downtrack (m) based on incoming pose. + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); + double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack; + ROS_DEBUG_STREAM("applicantCurrentmemberUpdates from ecef pose: " << applicantCurrentDtd); + + // Calculate crosstrack (m) based on incoming pose. + double applicantCurrentCtd = wm_->routeTrackPos(incoming_pose).crosstrack; + ROS_DEBUG_STREAM("applicantCurrentCtd from ecef pose: " << applicantCurrentCtd); + bool isInLane = abs(applicantCurrentCtd - current_crosstrack_) < config_.maxCrosstrackError; + ROS_DEBUG_STREAM("isInLane = " << isInLane); + + // Check if we have enough room for that applicant + int currentPlatoonSize = pm_.getHostPlatoonSize(); + bool hasEnoughRoomInPlatoon = applicantSize + currentPlatoonSize <= config_.maxPlatoonSize; + + // rear join; platoon leader --> leader waiting + if (isRearJoin) + { + // Log the request type + ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane REAR-JOIN request !"); + + // -- core condition to decided accept joiner or not + if (hasEnoughRoomInPlatoon && isInLane) + { + ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); + double currentRearDtd = pm_.getPlatoonRearDowntrackDistance(); + ROS_DEBUG_STREAM("The current platoon rear dtd is " << currentRearDtd); + double currentGap = currentRearDtd - applicantCurrentDtd - config_.vehicleLength; + double currentTimeGap = currentGap / applicantCurrentSpeed; + ROS_DEBUG_STREAM("The gap between current platoon rear and applicant is " << currentGap << "m or " << currentTimeGap << "s"); + if (currentGap < config_.minAllowedJoinGap) + { + ROS_WARN("We should not receive any request from the vehicle in front of us. NACK it."); + return MobilityRequestResponse::NACK; + } + + // Check if the applicant can join based on max timeGap/gap + bool isDistanceCloseEnough = currentGap <= config_.maxAllowedJoinGap || currentTimeGap <= config_.maxAllowedJoinTimeGap; + if (isDistanceCloseEnough) + { + ROS_DEBUG_STREAM("The applicant is close enough and we will allow it to try to join"); + ROS_DEBUG_STREAM("Change to LeaderWaitingState and waiting for " << msg.m_header.sender_id << " to join"); + + // change state to leaderwaiting ! + pm_.current_platoon_state = PlatoonState::LEADERWAITING; + waitingStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId); + pm_.platoonLeaderID = pm_.HostMobilityId; + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("The applicant is too far away from us. NACK."); + return MobilityRequestResponse::NACK; //TODO: add reason & request to try again + } + } + else + { + ROS_DEBUG_STREAM("The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); + return MobilityRequestResponse::NACK; + } + } + + // front join; platoon leader --> leader aborting + else if (isFrontJoin) + { + // Log the request type + ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane FRONT-JOIN request !"); + + // -- core condition to decided accept joiner or not + if (hasEnoughRoomInPlatoon && isInLane) + { + ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); + + // UCLA: change to read platoon front info + double currentFrontDtd = pm_.getPlatoonFrontDowntrackDistance(); + ROS_DEBUG_STREAM("The current platoon front dtd is " << currentFrontDtd); + // UCLA: adjust for calculating gap between new leader and old leader + double currentGap = applicantCurrentDtd - currentFrontDtd - config_.vehicleLength; + double currentTimeGap = currentGap / applicantCurrentSpeed; + ROS_DEBUG_STREAM("The gap between current platoon front and applicant is " << currentGap << "m or " << currentTimeGap << "s"); + + if (currentGap < config_.minAllowedJoinGap) + { + ROS_WARN("The current time gap is not suitable for frontal join. NACK it."); + return MobilityRequestResponse::NACK; + } + + // Check if the applicant can join based on max timeGap/gap + bool isDistanceCloseEnough = currentGap <= config_.maxAllowedJoinGap || currentTimeGap <= config_.maxAllowedJoinTimeGap; + + // UCLA: add condition: only allow front join when host platoon size >= 2 (make sure when two single vehicle join, only use back join) + bool isPlatoonNotSingle = pm_.getHostPlatoonSize() >= 2 || config_.test_front_join; + + if (isDistanceCloseEnough && isPlatoonNotSingle) + { + ROS_DEBUG_STREAM("The applicant is close enough for frontal join, send acceptance response"); + ROS_DEBUG_STREAM("Change to LeaderAborting state and waiting for " << msg.m_header.sender_id << " to join as the new platoon leader"); + + // ----------------- give up leader position and look for new leader -------------------------- + + // adjust for frontal join. Platoon info is related to the platoon at back of the candidate leader vehicle. + // Don't want an action plan here + pm_.current_platoon_state = PlatoonState::LEADERABORTING; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + + // If we are testing with a single vehicle representing this platoon, then we don't yet have a platoon ID, + // so use the ID for the proposed joining action plan + if (config_.test_front_join) + { + pm_.currentPlatoonID = msgHeader.plan_id; + } + + // Store the leader ID as that of the joiner to allow run_leader_aborting to work correctly + pm_.platoonLeaderID = applicantId; + + waitingStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan.valid = false; + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("The joining gap (" << currentGap << " m) is too far away from us or the target platoon size (" << pm_.getHostPlatoonSize() << ") is one. NACK."); + return MobilityRequestResponse::NACK; //TODO: add reason & request to try again + } + } + else + { + ROS_DEBUG_STREAM("The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); + return MobilityRequestResponse::NACK; + } + } + + // UCLA: conditions for cut-in join; platoon leader --> leading with operation + else if (isCutInJoin) + { + // Log the request type + ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a CUT-IN-JOIN request !"); + + // -- core condition to decided accept joiner or not. It is necessary leader only process the first cut-in joining request. + // Note: The host is the platoon leader, need to use a different method to determine if joining vehicle is nearby. + if (hasEnoughRoomInPlatoon && isJoiningVehicleNearPlatoon(applicantCurrentDtd, applicantCurrentCtd)) + { + ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); + ROS_DEBUG_STREAM("The applicant is close enough for cut-in join, send acceptance response"); + ROS_DEBUG_STREAM("Change to Leading with operation state and waiting for " << msg.m_header.sender_id << " to change lane"); + // change state to lead with operation + pm_.current_platoon_state = PlatoonState::LEADWITHOPERATION; + waitingStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId); + pm_.platoonLeaderID = pm_.HostMobilityId; + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("The current platoon does not have enough room or the applicant is too far away from us. NACK the request."); + ROS_DEBUG_STREAM("The current applicant size: " << applicantSize << "."); + ROS_DEBUG_STREAM("The applicant downtrack is: " << current_downtrack_ << "."); + ROS_DEBUG_STREAM("The applicant crosstrack is: " << current_crosstrack_ << "."); + return MobilityRequestResponse::NACK; + } + } + + // TODO: Place holder for deaprture. + + // no response + else + { + ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " and ignored."); + return MobilityRequestResponse::NO_RESPONSE; + } + } + + // UCLA: mobility request leader aborting (inherited from candidate follower) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderaborting(const cav_msgs::MobilityRequest& msg) + { + // This state does not handle any mobility request for now + // TODO Maybe it should handle some ABORT request from a candidate leader + ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + return MobilityRequestResponse::NO_RESPONSE; + } + + // UCLA: mobility request candidate leader (inherited from leader waiting) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(const cav_msgs::MobilityRequest& msg) + { + bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; // need to check: senderID (old leader) + bool isCandidateJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FRONT_JOIN; + + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); + double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack; + bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError; + ROS_DEBUG_STREAM("current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); + ROS_DEBUG_STREAM("obj_cross_track = " << obj_cross_track); + ROS_DEBUG_STREAM("current_crosstrack_ = " << current_crosstrack_); + ROS_DEBUG_STREAM("inTheSameLane = " << inTheSameLane); + ROS_DEBUG_STREAM("isTargetVehicle = " << isTargetVehicle); + ROS_DEBUG_STREAM("isCandidateJoin = " << isCandidateJoin); + if (isCandidateJoin && inTheSameLane && isTargetVehicle) + { + ROS_DEBUG_STREAM("Old platoon leader " << pm_.current_plan.peerId << " has agreed to joining."); + ROS_DEBUG_STREAM("Changing to PlatoonLeaderState and send ACK to the previous leader vehicle"); + pm_.current_platoon_state = PlatoonState::LEADER; + + // Clean up planning info + pm_.clearActionPlan(); + pm_.platoonLeaderID = config_.vehicleID; + + // Clean up neighbor platoon info since we just joined it + pm_.resetNeighborPlatoon(); + + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("Received platoon request with vehicle id = " << msg.m_header.sender_id); + ROS_DEBUG_STREAM("The request type is " << msg.plan_type.type << " and we choose to ignore"); + pm_.clearActionPlan(); + pm_.resetHostPlatoon(); //ASSUMES host is a solo joiner + + // return to leader state as a solo vehicle + pm_.current_platoon_state = PlatoonState::LEADER; + return MobilityRequestResponse::NACK; + } + } + + // UCLA: add request call-back function for lead with operation state (for cut-in join) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(const cav_msgs::MobilityRequest& msg) + { + /* + * Current leader change state to lead with opertaion once the cut-in join request is accepeted. + For cut-in front, the leading vehicle will open gap for the joining vehicle. + For cut-in middle, the leading vechile will request gap following vehcile to create gap. + The host leading vheicle will also send lane cut-in approval response to the joining vehicle. + The host leading vehicle will change to same-lane state once the lanechange completion plan was recieved. + + */ + + // Check request plan type + cav_msgs::PlanType plan_type = msg.plan_type; + std::string strategyParams = msg.strategy_params; + std::string reqSenderID = msg.m_header.sender_id; + + // Calculate downtrack (m) based on ecef. + lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); + // read downtrack + double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack; + ROS_DEBUG_STREAM("Applicant downtrack from ecef pose: " << applicantCurrentDtd); + + // Read requesting join index + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + + std::vector join_index_parsed; + boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":")); + int req_sender_join_index = std::stoi(join_index_parsed[1]); + ROS_DEBUG_STREAM("Requesting join_index parsed: " << req_sender_join_index); + + if (plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN) + { + // Send response + // ----- CUT-IN front ----- + if (req_sender_join_index == -1) + { + // determine if joining vehicle in position for cut-in front - this value must be > 0 + double cutinDtdDifference = applicantCurrentDtd - current_downtrack_ - config_.vehicleLength; + // check dtd between current leader (host) and joining vehicle is far enough away to avoid a collision + bool isFrontJoinerInPosition = cutinDtdDifference >= 1.5*config_.vehicleLength; + + if (isFrontJoinerInPosition) + { + ROS_DEBUG_STREAM("The joining vehicle is cutting in from front. Gap is already sufficient."); + return MobilityRequestResponse::ACK; + } + else if (cutinDtdDifference > 0.0 && cutinDtdDifference < 1.5*config_.vehicleLength) + { + // slow down leader to allow joiner cut-in + pm_.isCreateGap = true; + ROS_DEBUG_STREAM("The joining vehicle is cutting in from front."); + ROS_DEBUG_STREAM("Host (leader) slow down notified, joining vehicle can prepare to join"); + return MobilityRequestResponse::ACK; + } + + else + { + ROS_DEBUG_STREAM("Front join geometry violation. NACK. cutinDtdDifference = " << cutinDtdDifference); + pm_.current_platoon_state = PlatoonState::LEADER; + return MobilityRequestResponse::NACK; + } + } + + // ----- CUT-IN rear ----- + else if (static_cast(req_sender_join_index) == pm_.host_platoon_.size()-1) + { + // determine if joining vehicle in position for cut-in rear + // To pass, the joining vehicle need to be behind the last member, within three vehicle length. + //TODO: should it also test CTE to ensure vehicle is in same lane? + double platoonEndVehicleDtd = pm_.host_platoon_[pm_.host_platoon_.size()-1].vehiclePosition - config_.vehicleLength; + double rearGap = platoonEndVehicleDtd - applicantCurrentDtd; + bool isRearJoinerInPosition = rearGap >= 0 && rearGap <= config_.maxCutinGap;//3*config_.vehicleLength; TODO: temporary increase + + if (isRearJoinerInPosition) + { + ROS_DEBUG_STREAM("Published Mobility cut-in-rear-Join request to relavent platoon member, host is leader."); + ROS_WARN("Published Mobility cut-in-rear-Join request to relavent platoon members to signal gap creation."); + pm_.isCreateGap = true; + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("Rear join geometry violation. NACK. rearGap = " << rearGap); + pm_.current_platoon_state = PlatoonState::LEADER; + return MobilityRequestResponse::NACK; + } + } + // ----- CUT-IN middle ----- + else //any other join_index value + { + // determine if joining vehicle in position for cut-in mid + // To pass, the joining vehicle should be in front of the gap following vehicle, within three vehicle length. + double gapFollowingVehicleDtd = pm_.host_platoon_[req_sender_join_index].vehiclePosition; + double gapFollowerDiff = applicantCurrentDtd - gapFollowingVehicleDtd; + bool isMidJoinerInPosition = gapFollowerDiff >=0 && gapFollowerDiff <= 3*config_.vehicleLength; + + // Task 4: send request to index member to slow down (i.e., set isCreateGap to true) + // Note: Request recieving vehicle set pm_.isCreateGap + if (isMidJoinerInPosition) + { + // compose request (Note: The recipient should be the gap following vehicle.) + cav_msgs::MobilityRequest request; + request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); + // Note: For cut-in mid, notify gap rear member to create/increase gap. + std::string recipient_ID = pm_.host_platoon_[req_sender_join_index+1].staticId; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + // UCLA: add plan type, add this in cav_mwgs/plan_type + request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + request.strategy = PLATOONING_STRATEGY; + + double platoon_size = pm_.getHostPlatoonSize(); + + boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition. + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %req_sender_join_index; // index = 5 + + request.strategy_params = fmter.str(); + request.urgency = 50; + request.location = pose_to_ecef(pose_msg_); + // note: for rear join, cut-in index == host_platoon_.size()-1; for join from front, index == -1 + // for cut-in in middle, index indicate the gap leading vehicle's index + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation, host is leader."); + ROS_WARN("Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation."); + ROS_DEBUG_STREAM("The joining vehicle is cutting in at index: "<< req_sender_join_index <<". Notify gap rear vehicle with ID: " << recipient_ID << " to slow down"); + return MobilityRequestResponse::ACK; + } + else + { + ROS_DEBUG_STREAM("Mid join geometry violation. NACK. gapFollwerDiff = " << gapFollowerDiff); + pm_.current_platoon_state = PlatoonState::LEADER; + return MobilityRequestResponse::NACK; + } + } + } + + // task 2: For cut-in from front, the leader need to stop creating gap + else if (plan_type.type == cav_msgs::PlanType::STOP_CREATE_GAP) + { + // reset create gap indicator + pm_.isCreateGap = false; + // no need to response, simple reset the indicator + return MobilityRequestResponse::NO_RESPONSE; + } + + // task 3 cut-in front: After creating gap, revert back to same-lane operation + else if (plan_type.type == cav_msgs::PlanType::CUT_IN_FRONT_DONE) + { + ROS_DEBUG_STREAM("Cut-in from front lane change finished, leader revert to same-lane maneuver."); + pm_.current_platoon_state = PlatoonState::LEADERABORTING; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + // if testing with two vehicles, use plan id as platoon id + if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0) + // if (config_.allowCutinJoin) + { + pm_.currentPlatoonID = msg.m_header.plan_id; + } + // Store the leader ID as that of the joiner to allow run_leader_aborting to work correctly + pm_.platoonLeaderID = msg.m_header.sender_id; + pm_.current_plan.valid = false; + return MobilityRequestResponse::ACK; + } + + // task 4 cut-in from middle/rear + else if (plan_type.type == cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE) + { + ROS_DEBUG_STREAM("Cut-in from mid/rear lane change finished, leader revert to same-lane maneuver."); + pm_.current_platoon_state = PlatoonState::LEADERWAITING; + waitingStartTime = ros::Time::now().toNSec() / 1000000; + return MobilityRequestResponse::ACK; + } + + + // task 5: if other joining vehicle send joning request, NACK it since there is already a cut-in join going on. + else + { + ROS_DEBUG_STREAM("CUT-IN join maneuver is already in operation, NACK incoming join requests from other candidates."); + ROS_DEBUG_STREAM("Plan Type: " << plan_type.type ); + return MobilityRequestResponse::NACK; + } + + // this statement should never be reached, but will ensure reasonable behavior in case of coding error above + ROS_WARN_STREAM("End of method reached! Apparent logic fault above."); + ROS_DEBUG_STREAM("End of method reached! Apparent logic fault above."); //since WARN doesn't always print + return MobilityRequestResponse::NO_RESPONSE; + } + + // UCLA: add request call-back function for prepare to join (for cut-in join) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_preparetojoin(const cav_msgs::MobilityRequest& msg) + { + // This state does not handle any mobility request for now + // TODO: if joining vehicle need to adjust speed, the leader should request it and the request should be handled here. + ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + return MobilityRequestResponse::NO_RESPONSE; + } + + // TODO: Place holder for departure + + // ------ 4. Mobility response callback ------ // + + // Mobility response callback for all states. + void PlatoonStrategicIHPPlugin::mob_resp_cb(const cav_msgs::MobilityResponse& msg) + { + // Firstly, check eligibility of the received message. + bool isCurrPlanValid = pm_.current_plan.valid; // Check if current plan is still valid (i.e., not timed out). + bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; // Check if plan Id matches. + bool isFromTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; // Check if expected peer ID and sender ID matches. + ROS_DEBUG_STREAM("mob_resp_cb: isCurrPlanValid = " << isCurrPlanValid << ", isForCurrentPlan = " << + isForCurrentPlan << ", isFromTargetVehicle = " << isFromTargetVehicle); + ROS_DEBUG_STREAM("sender ID = " << msg.m_header.sender_id << ", current peer ID = " << pm_.current_plan.peerId); + ROS_DEBUG_STREAM("incoming plan ID = " << msg.m_header.plan_id << "current plan ID = " << pm_.current_plan.planId); + + if (!(isCurrPlanValid && isForCurrentPlan && isFromTargetVehicle)) + { + /** + * If any of the three condition (i.e., isCurrPlanValid, isForCurrentPlan and isFromTargetVehicle) + * was not satisfied, return ignore as this message was not intended for the host. + */ + ROS_DEBUG_STREAM(" Ignore the received response message as it was not intended for the host vehicle."); + return; + } + else if (pm_.current_platoon_state == PlatoonState::LEADER) + { + mob_resp_cb_leader(msg); + } + else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + mob_resp_cb_follower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) + { + mob_resp_cb_candidatefollower(msg); + } + else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) + { + mob_resp_cb_leaderwaiting(msg); + } + else if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + mob_resp_cb_standby(msg); + } + // UCLA: add leader aboorting + else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) + { + mob_resp_cb_leaderaborting(msg); + } + //UCLA: add candidate leader + else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) + { + mob_resp_cb_candidateleader(msg); + } + // UCLA: add lead with operation for cut-in join + else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) + { + mob_resp_cb_leadwithoperation(msg); + } + // UCLA: add prepare to join for cut-in join + else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) + { + mob_resp_cb_preparetojoin(msg); + } + // TODO: Place holder for departure. + } + + void PlatoonStrategicIHPPlugin::mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg) + { + // In standby state, it will not send out any requests so it will also ignore all responses + ROS_DEBUG_STREAM("STANDBY state does nothing with msg from " << msg.m_header.sender_id); + } + + void PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg) + { + ROS_DEBUG_STREAM("Callback for candidate follower "); + + // Check if current plan is still valid (i.e., not timed out) + if (pm_.current_plan.valid) + { + bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; + ROS_DEBUG_STREAM("isForCurrentPlan " << isForCurrentPlan); + + // Check the response is received correctly (i.e., host vehicle is the desired receiver). + if (isForCurrentPlan) + { + if (msg.is_accepted) + { + // We change to follower state and start to actually follow that leader + // The platoon manager also need to change the platoon Id to the one that the target leader is using + pm_.current_platoon_state = PlatoonState::FOLLOWER; + ROS_DEBUG_STREAM("pm_.currentPlatoonID: " << pm_.currentPlatoonID); + ROS_DEBUG_STREAM("pm_.targetPlatoonID: " << pm_.currentPlatoonID); + + if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0) + { + pm_.currentPlatoonID = pm_.targetPlatoonID; + ROS_DEBUG_STREAM("pm_.currentPlatoonID now: " << pm_.currentPlatoonID); + pm_.resetNeighborPlatoon(); + } + else + { + pm_.currentPlatoonID = msg.m_header.plan_id; + } + + pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id); + ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " agreed on our join. Change to follower state."); + ROS_WARN("changed to follower"); + pm_.clearActionPlan(); + } + else + { + // We change back to normal leader state and try to join other platoons + ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " does not agree on our join. Change back to leader state."); + ROS_DEBUG_STREAM("Trying again.."); + // join plan failed, but we still need the peerid + pm_.current_plan.valid = false; + + // Clear out any platooning plan we don't need + if (pm_.getHostPlatoonSize() == 1) + { + pm_.resetHostPlatoon(); + } + } + + } + else + { + ROS_DEBUG_STREAM("Ignore received response message because it is not for the current plan."); + } + } + else + { + ROS_DEBUG_STREAM("Ignore received response message because we are not in any negotiation process."); + } + } + + void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg) + { + /** + * Leader waiting is the state to check joining vehicle is in proper position + * and to prevent platoon leader from receiving messages from other CAVs in leader state. + * There was no response involved in this state, hence no action needed in this section. + */ + ROS_DEBUG_STREAM("LEADERWAITING state does nothing with msg from " << msg.m_header.sender_id); + } + + void PlatoonStrategicIHPPlugin::mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg) + { + /** + * UCLA Note: + * + * This method was implemented with the purpose of updating the platoon related references + * (i.e., platoon leader, platoon Id) to the new leader that joined from front. Changing + * the existing follower to candidate follower state will initiate a member update and + * therefore point all refernce to the new leader. + * + * For rear join, since the existing follower already following the platoon leader. There is + * no need to establish communication hence no response will be handled for rear-join. + */ + + // UCLA: read plan type + cav_msgs::PlanType plan_type = msg.plan_type; + + // UCLA: determine joining type + bool isFrontJoin = (plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT); + + //TODO: when would this code block ever be used? A normal follower would have to talk to a front joiner. + // UCLA: add response so follower can change to candidate follower, then change leader + if (isFrontJoin && msg.is_accepted) + { + // if frontal join is accepted, change followers to candidate follower to update leader + ROS_DEBUG_STREAM("Received positive response for front-join plan id = " << pm_.current_plan.planId); + ROS_DEBUG_STREAM("Change to CandidateFollower state and prepare to update platoon information"); + // Change to candidate follower state and request a new plan to catch up with the front platoon + pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + } + + // TODO: Place holder for follower departure. + } + + // UCLA: add conditions to account for frontal join states (candidate follower) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg) + { + /** + * UCLA implementation note: + * This is where the Mobility response gets processed for leader state. + * + * If the host is a single vehicle in the leader state, then the host vehicle is the + * joiner vehicle (frontal join: candidate leader; back join: candidate follower), and + * the response sender is the existing platoon leader (front join: aborting leader, back join: waiting leader). + * + * If the host is the current platoon leader, all three case will be false and no further action is needed. + * + * Disclaimer: Currently, if the host vehicle is platoon leader, there is no further action needed + * when receiving the mobility response. However, future development may add functions in this mehtod. + */ + + // UCLA: read plan type + cav_msgs::PlanType plan_type = msg.plan_type; + ROS_DEBUG_STREAM("plan_type = " << plan_type); + ROS_DEBUG_STREAM("plan_type.type = " << plan_type.type); + + // UCLA: determine joining type + bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN && !config_.test_front_join; + bool isRearJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_AT_REAR && !config_.test_front_join; + bool isFrontJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT || config_.test_front_join; + ROS_DEBUG_STREAM("Joining type: isRearJoin = " << isRearJoin); + ROS_DEBUG_STREAM("Joining type: isFrontJoin = " << isFrontJoin); + ROS_DEBUG_STREAM("Joining type: isCutInJoin = " << isCutInJoin); + + // Check if current plan is still valid (i.e., not timed out). + if (pm_.current_plan.valid) + { + ROS_DEBUG_STREAM("My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); + ROS_DEBUG_STREAM("Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); + + // Check the response is received correctly (i.e., host vehicle is the desired receiver). + if (pm_.current_plan.planId == msg.m_header.plan_id && pm_.current_plan.peerId == msg.m_header.sender_id) + { + // rear join + if (isRearJoin && msg.is_accepted) + { + ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); + ROS_DEBUG_STREAM("Change to CandidateFollower state and notify trajectory failure in order to replan"); + + // Change to candidate follower state and wait to catch up with the front platoon + pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan.valid = false; //but leave peerId intact for use in second request + } + + // UCLA: frontal join (candidate leader, inherited from leaderwaiting) + else if (isFrontJoin && msg.is_accepted) + { + ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); + ROS_DEBUG_STREAM("Change to CandidateLeader state and prepare to become new leader. "); + + // Change to candidate leader and idle + pm_.current_platoon_state = PlatoonState::CANDIDATELEADER; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan.valid = false; //but leave peerId intact for use in second request + + // Set the platoon ID to that of the target platoon even though we haven't yet joined; + // for front join this is necessary for the aborting leader to recognize us as an incoming + // member (via our published op STATUS messages) + pm_.currentPlatoonID = pm_.targetPlatoonID; + } + + // UCLA: CutIn join + else if (isCutInJoin && msg.is_accepted) + { + ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); + ROS_DEBUG_STREAM("Change to Prepare to join state and prepare to change lane. "); + + // Change to candidate leader and idle + pm_.current_platoon_state = PlatoonState::PREPARETOJOIN; + pm_.neighbor_platoon_leader_id_ = msg.m_header.sender_id; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan.valid = false; //but leave peerId intact for use in second request + } + + // Current leader of an actual platoon (to be in this method host is in leader state) + else if(pm_.getHostPlatoonSize() >= 2) + { + //TODO future: add logic here to allow two platoons to join together + + // Keep the leader idling, since this must be a bogus response + ROS_WARN_STREAM("Host received response for joining vehicles, remain idling as the host is a current platoon leader."); + } + else + { + ROS_DEBUG_STREAM("Received negative response for plan id = " << pm_.current_plan.planId << ". Resetting plan & platoon info."); + // Forget about the previous plan totally + pm_.clearActionPlan(); + pm_.resetHostPlatoon(); + } + } + else + { + ROS_DEBUG_STREAM("Ignore the response message because planID match: " << (pm_.current_plan.planId == msg.m_header.plan_id)); + ROS_DEBUG_STREAM("My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); + ROS_DEBUG_STREAM("And peer id match " << (pm_.current_plan.peerId == msg.m_header.sender_id)); + ROS_DEBUG_STREAM("Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); + } + } + } + + // UCLA: response for leader aborting (inherited from candidate follower) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting(const cav_msgs::MobilityResponse& msg) + { + /** + * UCLA implementation note: + * This state is the middle state to handle the leader aborting process of + * the previous platoon leader in a front join scenario. Within this state, the + * previous leader will check for front joining vehicle's position and will not + * handle any further mobility requests. + * + * Note: As the previous leader will join the new leader (joined from front), the + * corresponding join request will be send out by the previos leader. + */ + + ROS_DEBUG_STREAM("Callback for leader aborting !"); + + // Check if current plan is still valid (i.e., not timed out). + if (pm_.current_plan.valid) + { + bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; + bool isForFrontJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FRONT_JOIN; + + if (msg.plan_type.type == cav_msgs::PlanType::UNKNOWN){ + ROS_DEBUG_STREAM("*** plan type UNKNOWN"); + }else if (msg.plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT){ + ROS_DEBUG_STREAM("*** plan type JOIN_PLATOON_FROM_FRONT"); + }else if (msg.plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN){ + ROS_DEBUG_STREAM("*** plan type PLATOON_CUT_IN_JOIN"); + }else { + ROS_DEBUG_STREAM("*** plan type not captured."); + } + + bool isFromTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; + ROS_DEBUG_STREAM("msg.header.sender_id " << msg.m_header.sender_id); + ROS_DEBUG_STREAM("Plan Type " << msg.plan_type.type); + ROS_DEBUG_STREAM("isForFrontJoin " << isForFrontJoin); + ROS_DEBUG_STREAM("isForCurrentPlan " << isForCurrentPlan); + ROS_DEBUG_STREAM("isFromTargetVehicle " << isFromTargetVehicle); + + // Check the response is received correctly (i.e., host vehicle is the desired receiver). + if (isForCurrentPlan && isFromTargetVehicle && isForFrontJoin) + { + if (msg.is_accepted) + { + // We change to follower state and start to actually follow the new leader + // The platoon manager also need to change the platoon Id to the one that the target leader is using + pm_.current_platoon_state = PlatoonState::FOLLOWER; + pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id); + ROS_DEBUG_STREAM("The new leader " << msg.m_header.sender_id << " agreed on the frontal join. Change to follower state."); + ROS_WARN("changed to follower"); + + // reset leader aborting request marker + numLeaderAbortingCalls_ = 0; + } + else + { + // We change back to normal leader state + ROS_DEBUG_STREAM("The new leader " << msg.m_header.sender_id << " does not agree on the frontal join. Change back to leader state."); + pm_.current_platoon_state = PlatoonState::LEADER; + // We were already leading a platoon, so don't erase any of that info. But we need to remove the erstwhile candidate + // leader from our platoon roster; we know it is in position 0, so just remove that element + if (!pm_.removeMember(0)) + { + ROS_DEBUG_STREAM("Failed to remove candidate leader from the platoon!"); + } + } + + // Clean up the joining plan + pm_.clearActionPlan(); + } + else + { + ROS_DEBUG_STREAM("Ignore received response message because it is not for the current plan."); + } + } + else + { + ROS_DEBUG_STREAM("Ignore received response message because we are not in any negotiation process."); + } + } + + // UCLA: response for candidate leader (inherited from leader waiting) + void PlatoonStrategicIHPPlugin::mob_resp_cb_candidateleader(const cav_msgs::MobilityResponse& msg) + { + ROS_DEBUG_STREAM("CANDIDATELEADER state does nothing with msg from " << msg.m_header.sender_id); + } + + // UCLA: response callback for lead with operation + void PlatoonStrategicIHPPlugin::mob_resp_cb_leadwithoperation(const cav_msgs::MobilityResponse& msg) + { + ROS_DEBUG_STREAM("LEADWITHOPERATION state does nothing with msg from " << msg.m_header.sender_id); + } + + // UCLA: response callback for prepare to join (inherited from leader waiting) + void PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(const cav_msgs::MobilityResponse& msg) + { + /* + If leader notify the member to slow down and ACK the request, + start to check the gap and change lane when gap is large enough + */ + + cav_msgs::PlanType plan_type = msg.plan_type; + bool isCreatingGap = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + bool isFinishLaneChangeFront = plan_type.type == cav_msgs::PlanType::CUT_IN_FRONT_DONE; + bool isFinishLaneChangeMidorRear = plan_type.type == cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE; + ROS_DEBUG_STREAM("isCreatingGap = " << isCreatingGap << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); + + if (!msg.is_accepted) + { + ROS_DEBUG_STREAM("Request " << msg.m_header.plan_id << " was rejected by leader."); + ROS_DEBUG_STREAM("Action Plan reset."); + ROS_DEBUG_STREAM("Trying again...."); + pm_.current_plan.valid = false; + pm_.current_platoon_state = PlatoonState::LEADER; + return; + } + // UCLA: Create Gap or perform a rear join (no gap creation necessary) + ROS_DEBUG_STREAM("pm_.is_neighbor_record_complete_ " << pm_.is_neighbor_record_complete_); + if (isCreatingGap && pm_.is_neighbor_record_complete_) + { + // task 1: check gap + double cut_in_gap = pm_.getCutInGap(target_join_index_, current_downtrack_); + // cut-in gap not needed for front and rear join, so ignored + + // task 2: set indicator if gap is safe + safeToLaneChange_ = true; + ROS_DEBUG_STREAM("Gap is now sufficiently large."); + ROS_DEBUG_STREAM("in mob_resp_cb safeToLaneChange_: " << safeToLaneChange_); + + // task 3: notify gap-rear vehicle to stop slowing down + cav_msgs::MobilityRequest request; + request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); + request.m_header.recipient_id = pm_.current_plan.peerId; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + // UCLA: A new plan type to stop creat gap. + request.plan_type.type = cav_msgs::PlanType::STOP_CREATE_GAP; + request.strategy = PLATOONING_STRATEGY; + request.urgency = 50; + request.location = pose_to_ecef(pose_msg_); + double platoon_size = pm_.getHostPlatoonSize(); + + boost::format fmter(JOIN_PARAMS); + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %target_join_index_; // index = 5 + + request.strategy_params = fmter.str(); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Published Mobility Candidate-Join request to the leader to stop creating gap"); + } + + // UCLA: Revert to same-lane for cut-in front + else if (isFinishLaneChangeFront) + { + ROS_DEBUG_STREAM("Cut-in from front lane change finished, the joining vehicle revert to same-lane maneuver."); + pm_.current_platoon_state = PlatoonState::CANDIDATELEADER; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + ROS_DEBUG_STREAM("pm_.currentPlatoonID: " << pm_.currentPlatoonID); + ROS_DEBUG_STREAM("pm_.targetPlatoonID: " << pm_.targetPlatoonID); + if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0) + { + pm_.currentPlatoonID = pm_.targetPlatoonID; + ROS_DEBUG_STREAM("pm_.currentPlatoonID now: " << pm_.currentPlatoonID); + } + + pm_.current_plan.valid = false; //but leave peerId intact for use in second request + } + + // UCLA: Revert to same-lane operation for cut-in from middle/rear + else if (isFinishLaneChangeMidorRear) + { + ROS_DEBUG_STREAM("Cut-in from mid or rear, the lane change finished, the joining vehicle revert to same-lane maneuver."); + pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; + candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + pm_.current_plan.valid = false; //but leave peerId intact for use in second request + + } + + else + { + ROS_DEBUG_STREAM("End of mob_resp_cb_preparetojoin"); + } + } + + // TODO: Place holder for departure. + + // ------ 5. response types ------- // + + // ACK --> yes,accept host as member; NACK --> no, cannot accept host as member + void PlatoonStrategicIHPPlugin::mob_req_cb(const cav_msgs::MobilityRequest& msg) + { + // Ignore messages as long as host vehicle is stopped + if (current_speed_ < config_.minPlatooningSpeed) + { + ROS_DEBUG_STREAM("Ignoring message since host speed is below platooning speed."); + return; + } + + // Check that this is a message about platooning (could be from some other Carma activity nearby) + std::string strategy = msg.strategy; + if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) + { + ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + return; + } + + cav_msgs::MobilityResponse response; + response.m_header.sender_id = config_.vehicleID; + response.m_header.recipient_id = msg.m_header.sender_id; + response.m_header.plan_id = msg.m_header.plan_id; + response.m_header.timestamp = ros::Time::now().toNSec() / 1000000; + + // UCLA: add plantype in response + response.plan_type.type = msg.plan_type.type; + + MobilityRequestResponse req_response = handle_mob_req(msg); + if (req_response == MobilityRequestResponse::ACK) + { + response.is_accepted = true; + mobility_response_publisher_(response); + } + else if (req_response == MobilityRequestResponse::NACK) + { + response.is_accepted = false; + mobility_response_publisher_(response); + } + else + { + ROS_DEBUG_STREAM(" NO response to mobility request. "); + } + } + + + //------------------------------------------ FSM states --------------------------------------------------// + + void PlatoonStrategicIHPPlugin::run_leader_waiting() + { + ROS_DEBUG_STREAM("Run LeaderWaiting State "); + long tsStart = ros::Time::now().toNSec() / 1000000; + // Task 1 + if (tsStart - waitingStartTime > waitingStateTimeout * 1000) + { + //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant + ROS_DEBUG_STREAM("LeaderWaitingState is timeout, changing back to PlatoonLeaderState."); + pm_.current_platoon_state = PlatoonState::LEADER; + pm_.clearActionPlan(); + } + // Task 2 + cav_msgs::MobilityOperation status; + status = composeMobilityOperationLeaderWaiting(); + mobility_operation_publisher_(status); + ROS_DEBUG_STREAM("publish status message"); + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + } + + void PlatoonStrategicIHPPlugin::run_leader() + { + unsigned long tsStart = ros::Time::now().toNSec() / 1000000; + + // If vehicle is not rolling then return + if (current_speed_ <= STOPPED_SPEED) + { + return; + } + + // Task 1: heart beat timeout: send INFO mob_op + bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_; + ROS_DEBUG_STREAM("time since last heart beat: " << tsStart - prevHeartBeatTime_); + if (isTimeForHeartBeat) + { + cav_msgs::MobilityOperation infoOperation; + infoOperation = composeMobilityOperationLeader(OPERATION_INFO_TYPE); + mobility_operation_publisher_(infoOperation); + prevHeartBeatTime_ = ros::Time::now().toNSec() / 1000000; + ROS_DEBUG_STREAM("Published heart beat platoon INFO mobility operation message"); + } + + // Task 3: plan time out, check if any current join plan is still valid (i.e., not timed out). + if (pm_.current_plan.valid) + { + bool isCurrentPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT; + if (isCurrentPlanTimeout) + { + ROS_DEBUG_STREAM("Give up current on waiting plan with planId: " << pm_.current_plan.planId); + pm_.clearActionPlan(); + } + } + + // Task 4: STATUS msgs + bool hasFollower = pm_.getHostPlatoonSize() > 1 || config_.test_cutin_join; + ROS_DEBUG_STREAM("hasFollower" << hasFollower); + // if has follower, publish platoon message as STATUS mob_op + if (hasFollower) + { + cav_msgs::MobilityOperation statusOperation; + statusOperation = composeMobilityOperationLeader(OPERATION_STATUS_TYPE); + // mob_op_pub_.publish(statusOperation); + mobility_operation_publisher_(statusOperation); + ROS_DEBUG_STREAM("Published platoon STATUS operation message as a Leader with Follower"); + } + + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + + // Job 5: Dissoleve request. + // TODO: Place holder for departure. Need to change to departing state and tracking departng ID. + + } + + void PlatoonStrategicIHPPlugin::run_follower() + { + ROS_DEBUG_STREAM("run follower"); + // This is a interrupted-safe loop. + // This loop has four tasks: + // 1. Check the state start time, if it exceeds a limit it will give up current plan and change back to leader state + // 2. Abort current request if we wait for long enough time for response from leader and change back to leader state + + long tsStart = ros::Time::now().toNSec() / 1000000; + // Job 1 + cav_msgs::MobilityOperation status; + status = composeMobilityOperationFollower(); + mobility_operation_publisher_(status); + // Job 2 + // Get the number of vehicles in this platoon who is in front of us + int vehicleInFront = pm_.getNumberOfVehicleInFront(); + if (vehicleInFront == 0) + { + noLeaderUpdatesCounter++; + if (noLeaderUpdatesCounter >= LEADER_TIMEOUT_COUNTER_LIMIT) + { + ROS_DEBUG_STREAM("noLeaderUpdatesCounter = " << noLeaderUpdatesCounter << " and change to leader state"); + pm_.changeFromFollowerToLeader(); + pm_.current_platoon_state = PlatoonState::LEADER; + noLeaderUpdatesCounter = 0; + } + } + else + { + // reset counter to zero when we get updates again + noLeaderUpdatesCounter = 0; + } + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + + // Job 3: Dissoleve request. + //TODO: set departure indicator + + } + + void PlatoonStrategicIHPPlugin::run_candidate_follower() + { + long tsStart = ros::Time::now().toNSec() / 1000000; + + // Task 1: state timeout + bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; + ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); + ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + if (isCurrentStateTimeout) + { + ROS_DEBUG_STREAM("The current candidate follower state is timeout. Change back to leader state."); + pm_.current_platoon_state = PlatoonState::LEADER; + pm_.clearActionPlan(); + } + + // Task 2: plan timeout, check if current plan is still valid (i.e., not timed out). + if (pm_.current_plan.valid) + { + ROS_DEBUG_STREAM("pm_.current_plan.planStartTime: " << pm_.current_plan.planStartTime); + ROS_DEBUG_STREAM("timeout2: " << tsStart - pm_.current_plan.planStartTime); + ROS_DEBUG_STREAM("NEGOTIATION_TIMEOUT: " << NEGOTIATION_TIMEOUT); + bool isPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT; + if (isPlanTimeout) + { + pm_.current_platoon_state = PlatoonState::LEADER; + pm_.clearActionPlan(); + ROS_DEBUG_STREAM("The current plan did not receive any response. Abort and change to leader state."); + ROS_DEBUG_STREAM("Changed the state back to Leader"); + } + } + + // Task 3: update plan calculate gap, update plan: send PLATOON_FOLLOWER_JOIN request with new gap + double desiredJoinGap2 = config_.desiredJoinTimeGap * current_speed_; + double maxJoinGap = std::max(config_.desiredJoinGap, desiredJoinGap2); + double currentGap = 0.0; + if (!pm_.neighbor_platoon_.empty()) + { + currentGap = pm_.neighbor_platoon_.back().vehiclePosition - current_downtrack_; + ROS_DEBUG_STREAM("curent gap calculated from back of neighbor platoon: " << currentGap); + ROS_DEBUG_STREAM("pm_.neighbor_platoon_.back().vehiclePosition " << pm_.neighbor_platoon_.back().vehiclePosition); + + } + else + { + currentGap = pm_.getDistanceToPredVehicle(); + ROS_DEBUG_STREAM("curent gap when there is no neighbor platoon: " << currentGap); + } + + ROS_DEBUG_STREAM("Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " ms"); + ROS_DEBUG_STREAM("Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); + ROS_DEBUG_STREAM("The current gap from radar is " << currentGap << " m"); + if (currentGap <= maxJoinGap && !pm_.current_plan.valid) + { + cav_msgs::MobilityRequest request; + std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); + long currentTime = ros::Time::now().toNSec() / 1000000; + request.m_header.plan_id = planId; + request.m_header.recipient_id = pm_.current_plan.peerId; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = currentTime; + + request.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + request.strategy = PLATOONING_STRATEGY; + request.strategy_params = ""; //params will not be read by receiver since this is 2nd msg in sequence + request.urgency = 50; + request.location = pose_to_ecef(pose_msg_); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Published Mobility Candidate-Join request to the leader"); + ROS_DEBUG_STREAM("current plan peer id: " << pm_.current_plan.peerId); + + // Update the local record of the new activity plan and now establish that we have a platoon plan as well, + // which allows us to start sending necessary op STATUS messages + pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.current_plan.peerId); + pm_.platoonLeaderID = pm_.current_plan.peerId; + + // Initialize counter to delay transmission of first operation STATUS message + candidate_follower_delay_count_ = 0; + } + + //Task 4: publish platoon status message (as single joiner) + if (pm_.current_plan.valid) + { + // Don't want to do this until after the above MobReq message is delivered, otherwise recipient will double-count us in their platoon + if (++candidate_follower_delay_count_ > 2) + { + cav_msgs::MobilityOperation status; + status = composeMobilityOperationCandidateFollower(); + mobility_operation_publisher_(status); + ROS_DEBUG_STREAM("Published platoon STATUS operation message as Candidate Follower"); + } + } + + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + } + + // UCLA: frontal join state (inherit from candidate follower: prepare to give up leading state and accept the new leader) + void PlatoonStrategicIHPPlugin::run_leader_aborting() + { + /* + UCLA implementation note: + 1. this function send step plan type: "PLATOON_FRONT_JOIN" + 2. the sender of the plan (host vehicle) is the previous leader, it wil prepare to follow front joiner (new leader) + */ + long tsStart = ros::Time::now().toNSec() / 1000000; + // Task 1: state timeout + bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; + ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); + ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + if (isCurrentStateTimeout) + { + ROS_DEBUG_STREAM("The current leader aborting state is timeout. Change back to leader state."); + pm_.current_platoon_state = PlatoonState::LEADER; + + //clear plan validity & end; leave platoon info alone, as we may still be leading a valid platoon + pm_.clearActionPlan(); + return; + } + + // Task 3: update plan: PLATOON_FRONT_JOIN with new gap + double desiredJoinGap2 = config_.desiredJoinTimeGap * current_speed_; + double maxJoinGap = std::max(config_.desiredJoinGap, desiredJoinGap2); + + // check if compatible for front join --> return front gap, no veh type check, is compatible + // Note that pm_ only represents host's platoon members. As the aborting leader, the only vehicle + // preceding host is the candidate joiner. For this code to work, it depends on the candidate to publish + // mobility operation STATUS messages so that host can include it in the pm_ platoon membership. + double currentGap = pm_.getDistanceToPredVehicle(); //returns 0 if we haven't received op STATUS from joiner yet + ROS_DEBUG_STREAM("Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " m"); + ROS_DEBUG_STREAM("Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); + ROS_DEBUG_STREAM("The current gap to joiner is " << currentGap << " m"); + + // NOTE: The front join depends upon the joiner to publish op STATUS messages with this platoon's ID, then host receives at least one + // and thereby adds the joiner to the platoon record. This process requires host's mob_req_cb_leader() to ACK the join request, then + // the remote vehicle to handle that ACK and broadcast its first op STATUS, then host to receive and process it. All that has to happen + // before the below code block runs, even though this method starts to spin immediately after we send out the mentioned ACK. To avoid + // a race condition, we must wait a few cycles to ensure the 2-way messaging has completed. The race is that above calculation for + // current gap will be returned as 0 until we have received said STATUS message from the joiner, which would prematurely trigger the + // process to move forward. + + // Check if gap is big enough and if there is no currently active plan and this method has been called several times + // Add a condition to prevent sending repeated requests (Note: This is a same-lane maneuver, so no need to consider lower bound of joining gap.) + ++numLeaderAbortingCalls_; + ROS_DEBUG_STREAM("numLeaderAbortingCalls = " << numLeaderAbortingCalls_ << ", max = " << config_.maxLeaderAbortingCalls); + if (currentGap <= maxJoinGap && !pm_.current_plan.valid && numLeaderAbortingCalls_ > config_.maxLeaderAbortingCalls) + { + // compose frontal joining plan, senderID is the old leader + cav_msgs::MobilityRequest request; + std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); + long currentTime = ros::Time::now().toNSec() / 1000000; + request.m_header.plan_id = planId; + request.m_header.recipient_id = pm_.platoonLeaderID; //the new joiner + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = currentTime; + + int platoon_size = pm_.getHostPlatoonSize(); //depends on joiner to send op STATUS messages while joining + + boost::format fmter(JOIN_PARAMS); // Note: Front and rear join uses same params, hence merge to one param for both condition. + int dummy_join_index = -2; //leader aborting doesn't need join_index so use default value + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %dummy_join_index; // index = 5 + request.strategy_params = fmter.str(); + + // assign a new plan type + request.plan_type.type = cav_msgs::PlanType::PLATOON_FRONT_JOIN; + request.strategy = PLATOONING_STRATEGY; + request.urgency = 50; + request.location = pose_to_ecef(pose_msg_); + mobility_request_publisher_(request); + ROS_WARN("Published Mobility Candidate-Join request to the new leader"); + + // Create a new join action plan + pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.platoonLeaderID); + } + + //Task 4: publish platoon status message + cav_msgs::MobilityOperation status; + status = composeMobilityOperationLeaderAborting(); + mobility_operation_publisher_(status); + ROS_DEBUG_STREAM("Published platoon STATUS operation message"); + + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + } + + // UCLA: frontal join state (inherited from leader waiting: prepare to join as th new leader) + void PlatoonStrategicIHPPlugin::run_candidate_leader() + { + ROS_DEBUG_STREAM("Run Candidate Leader State "); + long tsStart = ros::Time::now().toNSec() / 1000000; + // Task 1: State time out + if (tsStart - candidatestateStartTime > waitingStateTimeout * 1000) + { + //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant + ROS_DEBUG_STREAM("CandidateLeader state is timeout, changing back to PlatoonLeaderState."); + pm_.current_platoon_state = PlatoonState::LEADER; + pm_.clearActionPlan(); + pm_.resetHostPlatoon(); + } + + // Task 2: publish status message + cav_msgs::MobilityOperation status; + status = composeMobilityOperationCandidateLeader(); + mobility_operation_publisher_(status); + ROS_DEBUG_STREAM("publish status message"); + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + } + + // UCLA: add leading with operation state for cut-in join platoon leader + void PlatoonStrategicIHPPlugin::run_lead_with_operation() + { + long tsStart = ros::Time::now().toNSec() / 1000000; + // Task 1: heart beat timeout: constantly send INFO mob_op + bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_; + ROS_DEBUG_STREAM("time since last heart beat: " << tsStart - prevHeartBeatTime_); + if (isTimeForHeartBeat) + { + cav_msgs::MobilityOperation infoOperation; + infoOperation = composeMobilityOperationLeadWithOperation(OPERATION_INFO_TYPE); + mobility_operation_publisher_(infoOperation); + prevHeartBeatTime_ = ros::Time::now().toNSec() / 1000000; + } + + // // Task 3: plan time out + // if (pm_.current_plan.valid) + // { + // bool isCurrentPlanTimeout = ((ros::Time::now().toNSec() / 1000000 - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT); + // if (isCurrentPlanTimeout) + // { + // ROS_DEBUG_STREAM("Give up waiting on plan with planId: " << pm_.current_plan.planId << "; stay in LEADWITHOPERATION"); + // pm_.current_plan.valid = false; + // } + // } + + // Task 4: STATUS msgs + bool hasFollower = pm_.getHostPlatoonSize() > 1 || config_.test_cutin_join; + // if has follower, publish platoon message as STATUS mob_op + if (hasFollower) + { + cav_msgs::MobilityOperation statusOperation; + statusOperation = composeMobilityOperationLeadWithOperation(OPERATION_STATUS_TYPE); + mobility_operation_publisher_(statusOperation); + } + long tsEnd = ros::Time::now().toNSec() / 1000000; + long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); + ros::Duration(sleepDuration / 1000).sleep(); + } + + // UCLA: add prepare to join state for cut-in joining vehicle + void PlatoonStrategicIHPPlugin::run_prepare_to_join() + { + /* + * The prepare join state should have the following tasks: + * 1. Compose mobility operation param: status. + * 2. Time out check. + * 3. Calculate proper cut_in index + * 4. Send out lane change intend to leader. + * Note: 1. safeToLaneChange_ monitors the gap condition. + * 2. Once it is safe to lane change, the updated plan will send-out in "plan_maneuver_cb" + */ + + // Task 2.1: state timeout + long tsStart = ros::Time::now().toNSec() / 1000000; + bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; + ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); + ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + if (isCurrentStateTimeout) + { + ROS_DEBUG_STREAM("The current prepare to join state is timeout. Change back to leader state and abort lane change."); + pm_.current_platoon_state = PlatoonState::LEADER; + safeToLaneChange_ = false; + pm_.clearActionPlan(); + pm_.resetHostPlatoon(); + // Leave neighbor platoon info in place, as we may retry the join later + } + + // TODO: Plan timeout is not needed for this state + + // If we aren't already waiting on a response to one of these plans, create one once neighbor info is available + ROS_DEBUG_STREAM("current_plan.valid = " << pm_.current_plan.valid << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); + + if (!pm_.current_plan.valid && pm_.is_neighbor_record_complete_) + { + // Task 1: compose mobility operation (status) + cav_msgs::MobilityOperation status; + status = composeMobilityOperationPrepareToJoin(); //TODO: I bet we could consolidate a lot of these compose methods + mobility_operation_publisher_(status); + ROS_DEBUG_STREAM("Published platoon STATUS operation message"); + + // Task 3: Calculate proper cut_in index + // Note: The cut-in index is zero-based and points to the gap-leading vehicle's index. For cut-in from front, the join index = -1. + double joinerDtD = current_downtrack_; + target_join_index_ = pm_.getClosestIndex(joinerDtD); + ROS_DEBUG_STREAM("calculated join index: " << target_join_index_); + + // Task 4: Send out request to leader about cut-in position + cav_msgs::MobilityRequest request; + std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); + long currentTime = ros::Time::now().toNSec() / 1000000; + request.m_header.plan_id = planId; + request.m_header.recipient_id = pm_.neighbor_platoon_leader_id_; + request.m_header.sender_id = config_.vehicleID; + request.m_header.timestamp = currentTime; + request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + request.strategy = PLATOONING_STRATEGY; + request.urgency = 50; + request.location = pose_to_ecef(pose_msg_); + double platoon_size = pm_.getHostPlatoonSize(); + + boost::format fmter(JOIN_PARAMS); + fmter %platoon_size; // index = 0 + fmter %current_speed_; // index = 1, in m/s + fmter %pose_ecef_point_.ecef_x; // index = 2 + fmter %pose_ecef_point_.ecef_y; // index = 3 + fmter %pose_ecef_point_.ecef_z; // index = 4 + fmter %target_join_index_; // index = 5 + request.strategy_params = fmter.str(); + mobility_request_publisher_(request); + ROS_DEBUG_STREAM("Published Mobility cut-in join request to leader " << request.m_header.recipient_id << " with planId = " << planId); + + // Create a new join action plan + pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.neighbor_platoon_leader_id_); + } + } + + // UCLA: run prepare to depart state for depart from platoon + // TODO: Place holder for departure + + + //------------------------------------------- main functions for platoon plugin --------------------------------------------// + + // Platoon on spin + bool PlatoonStrategicIHPPlugin::onSpin() + { + plugin_discovery_publisher_(plugin_discovery_msg_); + + // Update the platoon manager for host's current location & speeds + pm_.updateHostPose(current_downtrack_, current_crosstrack_); + pm_.updateHostSpeeds(current_speed_, cmd_speed_); + + if (pm_.current_platoon_state == PlatoonState::LEADER) + { + run_leader(); + } + else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) + { + run_follower(); + } + else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) + { + run_candidate_follower(); + } + else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) + { + run_leader_waiting(); + } + // UCLA: added for frontal join + else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) + { + run_leader_aborting(); + } + // UCLA: added for frontal join + else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) + { + run_candidate_leader(); + } + // UCLA: added lead with operationfor CUT-IN join + else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) + { + run_lead_with_operation(); + } + // UCLA: added prepare to join for CUT-IN join + else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) + { + run_prepare_to_join(); + } + else if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + ROS_DEBUG_STREAM("standby state, nothing to do"); + } + // coding oversight + else + { + ROS_ERROR_STREAM("///// unhandled state " << pm_.current_platoon_state); + } + // TODO: Place holder for departure + + cav_msgs::PlatooningInfo platoon_status = composePlatoonInfoMsg(); + platooning_info_publisher_(platoon_status); + + return true; + } + + // ------- Generate maneuver plan (Service Callback) ------- // + + // compose maneuver message + cav_msgs::Maneuver PlatoonStrategicIHPPlugin::composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time) + { + cav_msgs::Maneuver maneuver_msg; + maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; + maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; + maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; + maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "PlatooningTacticalPlugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "PlatooningStrategicIHPPlugin"; + maneuver_msg.lane_following_maneuver.start_dist = current_dist; + maneuver_msg.lane_following_maneuver.start_speed = current_speed; + maneuver_msg.lane_following_maneuver.start_time = current_time; + maneuver_msg.lane_following_maneuver.end_dist = end_dist; + maneuver_msg.lane_following_maneuver.end_speed = target_speed; + + // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet + maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration(config_.time_step); + maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) }; + + ROS_DEBUG_STREAM("in compose maneuver lane id:"<< lane_id); + + lanelet::ConstLanelet current_lanelet = wm_->getMap()->laneletLayer.get(lane_id); + if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty()) + { + + auto next_lanelet_id = wm_->getMapRoutingGraph()->following(current_lanelet, false).front().id(); + ROS_DEBUG_STREAM("next_lanelet_id:"<< next_lanelet_id); + maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(next_lanelet_id)); + } + else + { + ROS_DEBUG_STREAM("No following lanelets"); + } + + current_time = maneuver_msg.lane_following_maneuver.end_time; + ROS_DEBUG_STREAM("Creating lane follow start dist:"<getMap()->laneletLayer, current_loc, 10); + + // raise warn if no path was found + if(current_lanelets.size() == 0) + { + ROS_WARN_STREAM("Cannot find any lanelet in map!"); + return true; + } + + // locate lanelet on shortest path + auto shortest_path = wm_->getRoute()->shortestPath(); // find path amoung route + + lanelet::ConstLanelet current_lanelet; + int last_lanelet_index = -1; + for (auto llt : current_lanelets) + { + if (boost::geometry::within(current_loc, llt.second.polygon2d())) + { + int potential_index = findLaneletIndexFromPath(llt.second.id(), shortest_path); // usage: findLaneletIndexFromPath(target_id, lanelet2_path) + if (potential_index != -1) + { + last_lanelet_index = potential_index; + current_lanelet = shortest_path[last_lanelet_index]; // find lanelet2 from map that corresponse to the path + break; + } + } + } + + + // read status data + double current_progress = wm_->routeTrackPos(current_loc).downtrack; + double speed_progress = current_speed_; + ros::Time time_progress = ros::Time::now(); + + // ---------------- use IHP platoon trajectory regulation here -------------------- + // Note: The desired gap will be adjusted and send to control plugin (via platoon_info_msg) where gap creation will be handled. + double target_speed; + + // Note: gap regulation has moved to control plug-in, no need to adjust speed here. + target_speed = findSpeedLimit(current_lanelet); //get Speed Limit + double total_maneuver_length = current_progress + config_.time_step * target_speed; + // ---------------------------------------------------------------- + + // pick smaller length, accomendate when host is close to the route end + double route_length = wm_->getRouteEndTrackPos().downtrack; + total_maneuver_length = std::min(total_maneuver_length, route_length); + + // Update current status based on prior plan + if(req.prior_plan.maneuvers.size()!= 0) + { + ROS_DEBUG_STREAM("Provided with initial plan..."); + time_progress = req.prior_plan.planning_completion_time; + int end_lanelet = 0; + updateCurrentStatus(req.prior_plan.maneuvers.back(), speed_progress, current_progress, end_lanelet); + last_lanelet_index = findLaneletIndexFromPath(end_lanelet, shortest_path); + } + + ROS_DEBUG_STREAM("Starting Loop"); + ROS_DEBUG_STREAM("total_maneuver_length: " << total_maneuver_length << " route_length: " << route_length); + + + ROS_DEBUG_STREAM("in mvr callback safeToLaneChange: " << safeToLaneChange_); + + // Note: Use current_lanlet list (which was determined based on vehicle pose) to find current lanelet ID. + long current_lanelet_id = current_lanelets[0].second.id(); + ROS_DEBUG_STREAM("current_lanelet_id: " << current_lanelet_id); + // lane change maneuver + if (safeToLaneChange_) + { + // for testing purpose only, check lane change status + double target_crosstrack = wm_->routeTrackPos(target_cutin_pose_).crosstrack; + ROS_DEBUG_STREAM("target_crosstrack: " << target_crosstrack); + double crosstrackDiff = current_crosstrack_ - target_crosstrack; + bool isLaneChangeFinished = abs(crosstrackDiff) <= config_.maxCrosstrackError; + ROS_DEBUG_STREAM("crosstrackDiff: " << crosstrackDiff); + ROS_DEBUG_STREAM("isLaneChangeFinished: " << isLaneChangeFinished); + + // lane change not finished, use lane change plan + if(!isLaneChangeFinished) + { + // send out lane change plan + while (current_progress < total_maneuver_length) + { + ROS_DEBUG_STREAM("Lane Change Maneuver for Cut-in join ! "); + ROS_DEBUG_STREAM("current_progress: "<< current_progress); + ROS_DEBUG_STREAM("speed_progress: " << speed_progress); + ROS_DEBUG_STREAM("target_speed: " << target_speed); + ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + + // set to next lane destination, consider sending ecef instead of dtd + double end_dist = total_maneuver_length; + ROS_DEBUG_STREAM("end_dist: " << end_dist); + // consider calculate dtd_diff and ctd_diff + double dist_diff = end_dist - current_progress; + ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + + + //TODO: target_cutin_pose_ represents the platoon leader. It seems this may be the wrong answer for mid- or rear-cutins? + //SAINA: currently, the functions do not provide the correct point of rear vehicle of the platoon + double lc_end_dist = wm_->routeTrackPos(target_cutin_pose_).downtrack; + ROS_DEBUG_STREAM("lc_end_dist before buffer: " << lc_end_dist); + lc_end_dist = std::max(lc_end_dist, current_progress + config_.maxCutinGap); + ROS_DEBUG_STREAM("lc_end_dist after buffer: " << lc_end_dist); + + //TODO: target_cutin_pose_ represents the platoon leader. Is this the best pose to use here? + // get the actually closest lanelets, + auto target_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, target_cutin_pose_, 1); + if (target_lanelets.empty()) + { + ROS_DEBUG_STREAM("The target cutin pose is not on a valid lanelet. So no lane change!"); + break; + } + int target_lanelet_id = target_lanelets[0].second.id(); + ROS_DEBUG_STREAM("target_lanelet_id: " << target_lanelet_id); + + lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(current_lanelet_id); + lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id); + + auto relation = wm_->getMapRoutingGraph()->routingRelation(starting_lanelet, ending_lanelet); + bool lanechangePossible = false; + // TODO: Assuming a lane change is only needed from an adjacent left/right lanelet. Only valid for IHP platooning. + // Need to generalize in future. Refer to issue #1864 + if (relation == lanelet::routing::RelationType::Left || relation == lanelet::routing::RelationType::Right) + { + lanechangePossible = true; + } + else + { + lanechangePossible = false; + } + + if (lanechangePossible) + { + ROS_DEBUG_STREAM("Lane change possible, planning it.. " ); + resp.new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(current_downtrack_, lc_end_dist, + speed_progress, target_speed, current_lanelet_id, target_lanelet_id , time_progress)); + + } + else + { + ROS_DEBUG_STREAM("Lane change impossible, planning lanefollow instead ... " ); + resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_downtrack_, end_dist, + speed_progress, target_speed, current_lanelet_id, time_progress)); + } + + + + current_progress += dist_diff; + // read lane change maneuver end time as time progress + time_progress = resp.new_plan.maneuvers.back().lane_change_maneuver.end_time; + speed_progress = target_speed; + if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + { + break; + } + + ++last_lanelet_index; + } + } + + // lane change finished, use lane following plan + else + { + // send out lane following plan + while (current_progress < total_maneuver_length) + { + ROS_DEBUG_STREAM("Same Lane Maneuver for platoon join ! "); + ROS_DEBUG_STREAM("current_progress: "<< current_progress); + ROS_DEBUG_STREAM("speed_progress: " << speed_progress); + ROS_DEBUG_STREAM("target_speed: " << target_speed); + ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + double end_dist = total_maneuver_length; + ROS_DEBUG_STREAM("end_dist: " << end_dist); + double dist_diff = end_dist - current_progress; + ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + if(end_dist < current_progress) + { + break; + } + // Note: The previous plan was generated at the beginning of the trip. It is necessary to update + // it as the lane ID and lanelet Index are different. + resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_downtrack_, end_dist, + speed_progress, target_speed, current_lanelet_id, time_progress)); + + current_progress += dist_diff; + time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time; + speed_progress = target_speed; + if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + { + break; + } + ++last_lanelet_index; + } + } + } + + // same-lane maneuver + else + { + ROS_DEBUG_STREAM("Planning Same Lane Maneuver! "); + while (current_progress < total_maneuver_length) + { + ROS_DEBUG_STREAM("Same Lane Maneuver for platoon join ! "); + ROS_DEBUG_STREAM("current_progress: "<< current_progress); + ROS_DEBUG_STREAM("speed_progress: " << speed_progress); + ROS_DEBUG_STREAM("target_speed: " << target_speed); + ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + double end_dist = total_maneuver_length; + ROS_DEBUG_STREAM("end_dist: " << end_dist); + double dist_diff = end_dist - current_progress; + ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + if (end_dist < current_progress) + { + break; + } + + resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_progress, end_dist, + speed_progress, target_speed,current_lanelet_id, time_progress)); + + + current_progress += dist_diff; + time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time; + speed_progress = target_speed; + if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + { + break; + } + ++last_lanelet_index; + } + } + + + if(resp.new_plan.maneuvers.size() == 0) + { + ROS_WARN_STREAM("Cannot plan maneuver because no route is found"); + } + + + if (pm_.getHostPlatoonSize() < 2 && !safeToLaneChange_) + { + resp.new_plan.maneuvers = {}; + ROS_WARN_STREAM("Platoon size 1 so Empty maneuver sent"); + } + else + { + ROS_DEBUG_STREAM("Planning maneuvers: "); + ROS_DEBUG_STREAM("safeToLaneChange_: " << safeToLaneChange_); + ROS_DEBUG_STREAM("pm_.getHostPlatoonSize(): " << pm_.getHostPlatoonSize()); + } + + if (pm_.current_platoon_state == PlatoonState::STANDBY) + { + pm_.current_platoon_state = PlatoonState::LEADER; + ROS_DEBUG_STREAM("change the state from standby to leader at start-up"); + } + + ROS_DEBUG_STREAM("current_downtrack: " << current_downtrack_); + + return true; + } + //--------------------------------------------------------------------------// +} diff --git a/platooning_strategic_IHP/test/main_test.cpp b/platooning_strategic_IHP/test/main_test.cpp new file mode 100644 index 0000000000..81d202fa20 --- /dev/null +++ b/platooning_strategic_IHP/test/main_test.cpp @@ -0,0 +1,27 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include +#include + +// Run all the tests +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/platooning_strategic_IHP/test/mobility_messages.cpp b/platooning_strategic_IHP/test/mobility_messages.cpp new file mode 100644 index 0000000000..d1457182e1 --- /dev/null +++ b/platooning_strategic_IHP/test/mobility_messages.cpp @@ -0,0 +1,315 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +using namespace std; + +class MobilityMessages{ + + private: + cav_msgs::MobilityRequest req1; + cav_msgs::MobilityRequest req2; + cav_msgs::MobilityRequest req3; + // UCLA add req4 to test added PlanType + cav_msgs::MobilityRequest req4; + cav_msgs::MobilityResponse res1; + cav_msgs::MobilityResponse res2; + cav_msgs::MobilityResponse res3; + cav_msgs::MobilityOperation op1; + cav_msgs::MobilityOperation op2; + cav_msgs::MobilityOperation op3; + + public: + MobilityMessages(){ + cav_msgs::MobilityHeader header; + cav_msgs::PlanType plan; + cav_msgs::LocationECEF location; + cav_msgs::LocationOffsetECEF offset; + cav_msgs::Trajectory traject; + vector offsets; + //mobility request messages + + //set up location + location.ecef_x = 10000; + location.ecef_y = 0; + location.ecef_z = 33333; + location.timestamp = 99999; + + + //set up trajectory + traject.location = location; + offset.offset_x = 14; + offset.offset_y = 200; + offset.offset_z = 150; + offsets.push_back(offset); + traject.offsets = offsets; + + //set up plan + plan.type = 1; + + //set up header + header.sender_id = "sender_id1"; + header.sender_bsm_id = "sender_bsm_id1"; + header.recipient_id = ""; + header.plan_id = "plan_id1"; + header.timestamp = 55555555555555; + + //set up mobility request + req1.header = header; + req1.strategy = "CARMA/platooning"; + req1.plan_type = plan; + req1.urgency = 1000; + req1.location = location; + req1.strategy_params = "param1 param2"; + req1.trajectory = traject; + req1.expiration = 5555555; + + + //set up location + location.ecef_x = 40; + location.ecef_y = 12312321; + location.ecef_z = 3177; + location.timestamp = 191934; + + + //set up trajectory + traject.location = location; + offset.offset_x = 189; + offset.offset_y = 2123124; + offset.offset_z = 14; + offsets.push_back(offset); + traject.offsets = offsets; + + //set up plan + plan.type = 2; + + //set up header + header.sender_id = "11"; + header.sender_bsm_id = "sender_bsm_id5"; + header.recipient_id = "recipient_id5"; + header.plan_id = "plan_id5"; + header.timestamp = 555555555555; + + //set up mobility request + req2.header = header; + req2.strategy = "CARMA/platooning"; + req2.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + req2.urgency = 1000; + req2.location = location; + req2.strategy_params = "param1 param2 param3 param4"; + req2.trajectory = traject; + req2.expiration = 98124172; + + + //set up location + location.ecef_x = 1238123; + location.ecef_y = 9875488; + location.ecef_z = 3434534; + location.timestamp = 88888888; + + + //set up trajectory + traject.location = location; + offset.offset_x = 987893934; + offset.offset_y = 5151; + offset.offset_z = 12312; + offsets.push_back(offset); + traject.offsets = offsets; + + //set up plan + plan.type = 3; + + //set up header + header.sender_id = "sender_id6"; + header.sender_bsm_id = "sender_bsm_id6"; + header.recipient_id = "recipient_id6"; + header.plan_id = "plan_id6"; + header.timestamp = 98745745; + + //set up mobility request + req3.header = header; + req3.strategy = "CARMA/platooning3"; + req3.plan_type = plan; + req3.urgency = 156; + req3.location = location; + req3.strategy_params = "param1 param2"; + req3.trajectory = traject; + req3.expiration = 15132; + + // UCLA: set up req4 to test platoon_front_join + + //set up location + location.ecef_x = 1223232; + location.ecef_y = 2323488; + location.ecef_z = 2323223; + location.timestamp = 66666666; + + + //set up trajectory + traject.location = location; + offset.offset_x = 232; + offset.offset_y = 23232334; + offset.offset_z = 23; + offsets.push_back(offset); + traject.offsets = offsets; + + //set up plan + plan.type = 4; + + //set up header + header.sender_id = "23"; + header.sender_bsm_id = "sender_bsm_id2"; + header.recipient_id = "recipient_id2"; + header.plan_id = "plan_id2"; + header.timestamp = 2222222222222; + + //set up mobility request + req4.header = header; + req4.strategy = "CARMA/platooning"; + req4.plan_type.type = cav_msgs::PlanType::PLATOON_FRONT_JOIN; + req4.urgency = 1300; + req4.location = location; + req4.strategy_params = "param1 param2 param3 param4"; + req4.trajectory = traject; + req4.expiration = 9823232323; + + + //mobility response messages + + + //set up header + header.sender_id = "sender_id2"; + header.sender_bsm_id = "sender_bsm_id2"; + header.recipient_id = "recipient_id1"; + header.plan_id = "plan_id2"; + header.timestamp = 66666666; + + //set up mobility response + res1.header = header; + res1.is_accepted = true; + res1.urgency = 1000; + + //set up header + header.sender_id = "sender_id9"; + header.sender_bsm_id = "sender_bsm_id9"; + header.recipient_id = "recipient_id9"; + header.plan_id = "plan_id9"; + header.timestamp = 1772327; + + //set up mobility response + res2.header = header; + res2.is_accepted = false; + res2.urgency = 50; + + //set up header + header.sender_id = "sender_id10"; + header.sender_bsm_id = "sender_bsm_id10"; + header.recipient_id = "recipient_id10"; + header.plan_id = "plan_id10"; + header.timestamp = 77777777; + + //set up mobility response + res3.header = header; + res3.is_accepted = true; + res3.urgency = 700; + + + + + //mobility operation messages + + //set up header + header.sender_id = "sender_id3"; + header.sender_bsm_id = "sender_bsm_id3"; + header.recipient_id = "recipient_id3"; + header.plan_id = "plan_id3"; + header.timestamp = 44444444; + + op1.header = header; + op1.strategy = "STATUS"; + op1.strategy_params = "CMDSPEED:1.0,DTD:1.0,SPEED:1.0"; + + //set up header + header.sender_id = "sender_id12"; + header.sender_bsm_id = "sender_bsm_id12"; + header.recipient_id = "recipient_id12"; + header.plan_id = "plan_id12"; + header.timestamp = 8989898; + + op2.header = header; + op2.strategy = "strategy3"; + op2.strategy_params = "param1 param2 param3 param4 param5"; + + + //set up header + header.sender_id = "sender_id23"; + header.sender_bsm_id = "sender_bsm_id23"; + header.recipient_id = "recipient_id23"; + header.plan_id = "plan_id23"; + header.timestamp = 181818; + + op3.header = header; + op3.strategy = "strategy23"; + op3.strategy_params = ""; + + } + + cav_msgs::MobilityRequest getRequest1(){ + return req1; + } + + cav_msgs::MobilityRequest getRequest2(){ + return req2; + } + + cav_msgs::MobilityRequest getRequest3(){ + return req3; + } + + + cav_msgs::MobilityResponse getResponse1(){ + return res1; + } + + cav_msgs::MobilityResponse getResponse2(){ + return res2; + } + + cav_msgs::MobilityResponse getResponse3(){ + return res3; + } + + cav_msgs::MobilityOperation getOperation1(){ + return op1; + } + + cav_msgs::MobilityOperation getOperation2(){ + return op2; + } + + cav_msgs::MobilityOperation getOperation3(){ + return op3; + } + +}; diff --git a/platooning_strategic_IHP/test/test_platoon_manager.cpp b/platooning_strategic_IHP/test/test_platoon_manager.cpp new file mode 100644 index 0000000000..8e87ea8261 --- /dev/null +++ b/platooning_strategic_IHP/test/test_platoon_manager.cpp @@ -0,0 +1,275 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +#include "platoon_manager_ihp.h" +#include "platoon_strategic_ihp.h" +#include "platoon_config_ihp.h" +#include +#include +#include +#include +#include +#include +// #include "TestHelpers.h" + +using namespace platoon_strategic_ihp; + +TEST(PlatoonManagerTest, test_construct) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + PlatoonManager pm_ = plugin.getHostPM(); + pm_.current_platoon_state = PlatoonState::LEADER; +} + +TEST(PlatoonManagerTest, test_ecef_encode) +{ + ros::Time::init(); + + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + + cav_msgs::LocationECEF ecef_point_test; + ecef_point_test.ecef_x = 1.0; + ecef_point_test.ecef_y = 2.0; + ecef_point_test.ecef_z = 3.0; + // Update ecef location + plugin.setHostECEF(ecef_point_test); + plugin.run_candidate_leader(); +} + + +TEST(PlatoonManagerTest, test_split) +{ + cav_msgs::MobilityOperation msg; + std::string strategyParams("INFO|REAR:1,LENGTH:2,SPEED:3,SIZE:4"); + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + std::vector rearVehicleBsmId_parsed; + boost::algorithm::split(rearVehicleBsmId_parsed, inputsParams[0], boost::is_any_of(":")); + std::string rearVehicleBsmId = rearVehicleBsmId_parsed[1]; + std::cout << "rearVehicleBsmId: " << rearVehicleBsmId << std::endl; + + std::vector rearVehicleDtd_parsed; + boost::algorithm::split(rearVehicleDtd_parsed, inputsParams[1], boost::is_any_of(":")); + double rearVehicleDtd = std::stod(rearVehicleDtd_parsed[1]); + std::cout << "rearVehicleDtd: " << rearVehicleDtd << std::endl; +} + + +// TEST(PlatoonManagerTest, test_states) +// { +// ros::Time::init(); + +// PlatoonPluginConfig config; +// std::shared_ptr wm = std::make_shared(); + +// PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); +// pm_.current_platoon_state = PlatoonState::LEADER; +// pm_.current_downtrack_distance_ = 20; + +// cav_msgs::MobilityRequest request; +// request.plan_type.type = 3; +// request.strategy_params = "SIZE:1,SPEED:0,DTD:11.5599"; + +// plugin.mob_req_cb(request); + +// EXPECT_EQ(pm_.current_platoon_state, PlatoonState::LEADERWAITING); +// } + +TEST(PlatoonManagerTest, test_compose) +{ + std::string OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,DTD:%2%,SPEED:%3%"; + double cmdSpeed = 1; + double current_speed = 2; + double current_downtrack = 4; + boost::format fmter(OPERATION_STATUS_PARAMS); + fmter %cmdSpeed; + fmter %current_downtrack; + fmter %current_speed; + std::string statusParams = fmter.str(); + + std::cout << "statusParams: " << statusParams << std::endl; +} + + +TEST(PlatoonStrategicIHPPlugin, mob_resp_cb) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + PlatoonManager pm_ = plugin.getHostPM(); + pm_.current_platoon_state = PlatoonState::FOLLOWER; + + plugin.onSpin(); + +} + +TEST(PlatoonStrategicIHPPlugin, platoon_info_pub) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + PlatoonManager pm_ = plugin.getHostPM(); + pm_.current_platoon_state = PlatoonState::LEADER; + + cav_msgs::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); + EXPECT_EQ(info_msg1.leader_id, "default_id"); + + pm_.current_platoon_state = PlatoonState::FOLLOWER; + pm_.isFollower = true; + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + std::vector cur_pl; + cur_pl.push_back(member); + pm_.platoon = cur_pl; + + cav_msgs::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); + EXPECT_EQ(info_msg2.leader_id, "1"); +} + +TEST(PlatoonStrategicIHPPlugin, test_follower) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + PlatoonManager pm_ = plugin.getHostPM(); + pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; + pm_.current_plan.valid = true; + EXPECT_EQ(pm_.isFollower, false); + + cav_msgs::MobilityResponse resp; + resp.header.plan_id = "resp"; + resp.is_accepted = true; + plugin.mob_resp_cb(resp); + EXPECT_EQ(pm_.current_platoon_state, PlatoonState::FOLLOWER); + EXPECT_EQ(pm_.isFollower, true); +} + +TEST(PlatoonStrategicIHPPlugin, test_get_leader) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + PlatoonManager pm_ = plugin.getHostPM(); + pm_.current_platoon_state = PlatoonState::FOLLOWER; + + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + std::vector cur_pl; + cur_pl.push_back(member); + + pm_.platoon = cur_pl; + + EXPECT_EQ(pm_.platoon.size(), 1); + + PlatoonMember member1 = pm_.platoon[0]; + + pm_.isFollower = true; + PlatoonMember platoon_leader = pm_.getDynamicLeader(); + + EXPECT_EQ(member1.staticId, "1"); + EXPECT_EQ(platoon_leader.staticId, "1"); +} + + +TEST(PlatoonManagerTest, test2) +{ + platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + std::vector cur_pl; + + cur_pl.push_back(*member); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = true; + pm.platoonSize = 1; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + std::string params = "CMDSPEED:11,DOWNTRACK:01,SPEED:11"; + + ros::Time::init(); + + pm.updatesOrAddMemberInfo("2", 2.0, 1.0, 2.5); + + EXPECT_EQ(2, pm.platoon.size()); + EXPECT_EQ("1", pm.platoon[0].staticId); +} + + +TEST(PlatoonManagerTest, test3) +{ + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + std::vector cur_pl; + + cur_pl.push_back(*member1); + cur_pl.push_back(*member2); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = false; + pm.platoonSize = 2; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + ros::Time::init(); + + int res = pm.getTotalPlatooningSize(); + + EXPECT_EQ(3, res); + +} + +TEST(PlatoonManagerTest, test4) +{ + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + std::vector cur_pl; + + cur_pl.push_back(*member1); + cur_pl.push_back(*member2); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = true; + pm.platoonSize = 2; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + ros::Time::init(); + + int res = pm.allPredecessorFollowing(); + + EXPECT_EQ(0, res); +} diff --git a/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp new file mode 100644 index 0000000000..c5e6ba1471 --- /dev/null +++ b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp @@ -0,0 +1,391 @@ + +/*------------------------------------------------------------------------------ +* 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. + +------------------------------------------------------------------------------*/ + +/* + * Developed by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + */ + +#include "platoon_manager_ihp.h" +#include "platoon_strategic_ihp.h" +#include "platoon_config_ihp.h" +#include +#include +#include +#include +#include +#include +#include +#include +// #include "TestHelpers.h" + +using namespace platoon_strategic_ihp; + +TEST(PlatoonManagerTestFrontJoin, test_construct_front) +{ + ros::Time::init(); + + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + // change state + plugin.setPMState(PlatoonState::LEADER); +} + +// UCLA: use "run_candidate_leader" to test ecef encoding +TEST(PlatoonManagerTestFrontJoin, test_ecef_encode_front) +{ + ros::Time::init(); + + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + cav_msgs::LocationECEF ecef_point_test; + ecef_point_test.ecef_x = 1.0; + ecef_point_test.ecef_y = 2.0; + ecef_point_test.ecef_z = 3.0; + // Update ecef location + plugin.setHostECEF(ecef_point_test); + plugin.run_candidate_leader(); +} + + +TEST(PlatoonManagerTestFrontJoin, test_split_front) +{ + cav_msgs::MobilityOperation msg; + std::string strategyParams("INFO|REAR:1,LENGTH:2,SPEED:3,SIZE:4"); + std::vector inputsParams; + boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); + std::vector rearVehicleBsmId_parsed; + boost::algorithm::split(rearVehicleBsmId_parsed, inputsParams[0], boost::is_any_of(":")); + std::string rearVehicleBsmId = rearVehicleBsmId_parsed[1]; + std::cout << "rearVehicleBsmId: " << rearVehicleBsmId << std::endl; + + std::vector rearVehicleDtd_parsed; + boost::algorithm::split(rearVehicleDtd_parsed, inputsParams[1], boost::is_any_of(":")); + double rearVehicleDtd = std::stod(rearVehicleDtd_parsed[1]); + std::cout << "rearVehicleDtd: " << rearVehicleDtd << std::endl; +} + + +TEST(PlatoonManagerTestFrontJoin, test_compose_front) +{ + std::string OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,DTD:%2%,SPEED:%3%"; + double cmdSpeed = 1; + double current_speed = 2; + double current_downtrack = 4; + boost::format fmter(OPERATION_STATUS_PARAMS); + fmter %cmdSpeed; + fmter %current_downtrack; + fmter %current_speed; + std::string statusParams = fmter.str(); + + std::cout << "statusParams: " << statusParams << std::endl; +} + + +TEST(PlatoonStrategicIHPPlugin, mob_resp_cb_front) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // change state + plugin.setPMState(PlatoonState::FOLLOWER); + + plugin.onSpin(); +} + +TEST(PlatoonStrategicIHPPlugin, platoon_info_pub_front) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + + // Use platoon manager setter to set state + plugin.setPMState(PlatoonState::LEADER); + // compose platoon info + cav_msgs::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); + // check platoon info (Host is a single ADS vehicle, so it's also the leader) + EXPECT_EQ(info_msg1.leader_id, "default_id"); + + // set host to follower + plugin.setPMState(PlatoonState::FOLLOWER); + // set plan valid + plugin.setPMValid(true); + // add member + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + std::vector cur_pl; + cur_pl.push_back(member); + // update platoon list + plugin.updatePlatoonList(cur_pl); + // compose platoon info + cav_msgs::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); + // check platoo info (when host is follower, the newly added member will be the leader) + EXPECT_EQ(info_msg2.leader_id, "1"); +} + +// ---------------------------------------- UCLA Tests for same-lane front join and cut-in front join --------------------------------------- +// UCLA: Use the transition "leader_aborting --> follower" to test follower functions +TEST(PlatoonStrategicIHPPlugin, test_follower_front) +{ + + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // change state + plugin.setPMState(PlatoonState::LEADERABORTING); + // set plan valid + plugin.setPMValid(true); + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); + EXPECT_EQ(pm_.isFollower, false); + + cav_msgs::MobilityResponse resp; + resp.is_accepted = true; + // sender id need to match with default value + resp.header.sender_id = pm_.targetLeaderId; + // plan_id need to match with default value, which is "" + resp.header.plan_id = pm_.current_plan.planId; + plugin.mob_resp_cb(resp); + + // Use Getter to retrieve an updated Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); + EXPECT_EQ(pm1_.current_platoon_state, PlatoonState::FOLLOWER); + EXPECT_EQ(pm1_.isFollower, true); +} + +// UCLA: Cut-in front test for platoon leader, Test the transition "lead_with_operation --> leader_aborting" +TEST(PlatoonStrategicIHPPlugin, cutin_test_leader_2) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + + // change state + plugin.setPMState(PlatoonState::LEADWITHOPERATION); + // set plan valid + plugin.setPMValid(true); + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); + EXPECT_EQ(pm_.isFollower, false); + + // compose dummy request msg + cav_msgs::MobilityRequest msg; + msg.header.plan_id = "req"; + msg.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + + // compose dummy response msg + MobilityRequestResponse resp = plugin.handle_mob_req(msg); + + // Use Getter to update Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); + + // run test and compare result + EXPECT_EQ(pm1_.current_platoon_state, PlatoonState::LEADERABORTING); + EXPECT_EQ(resp, MobilityRequestResponse::ACK); +} + +//UCLA: Cut-in front test for joining vheicle. Test the transition "leader --> prepare to jion" +TEST(PlatoonStrategicIHPPlugin, cutin_test_join_1) +{ + ros::Time::init(); + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); + plugin.setPMState(PlatoonState::LEADER); + plugin.setPMValid(true); + EXPECT_EQ(pm_.isFollower, false); + + // compose dummy response msg + cav_msgs::MobilityResponse resp; + + // compose plan ID and leader ID + resp.header.plan_id = pm_.current_plan.planId; + resp.header.sender_id = pm_.current_plan.peerId; + // plan type and acceptance + resp.plan_type.type = cav_msgs::PlanType::CUT_IN_FROM_SIDE; + resp.is_accepted = true; + + // send dummy reponse + plugin.mob_resp_cb(resp); + + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); + EXPECT_EQ(pm1_.current_platoon_state, PlatoonState::PREPARETOJOIN); + +} + +//UCLA: Cut-in front test for joining vheicle. Test the transition "prepare to jion --> candidate leader" +TEST(PlatoonStrategicIHPPlugin, cutin_test_join_2) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // change state + plugin.setPMState(PlatoonState::PREPARETOJOIN); + // set plan valid + plugin.setPMValid(true); + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); + EXPECT_EQ(pm_.isFollower, false); + + // compose dummy response msg + cav_msgs::MobilityResponse resp; + // sender id need to match with default value + resp.header.sender_id = pm_.targetLeaderId; + // plan_id need to match with default value, which is "" + resp.header.plan_id = pm_.current_plan.planId; + resp.is_accepted = true; + resp.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + + // send dummy reponse + plugin.mob_resp_cb(resp); + + // run test and compare result + // Use Getter to update Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); + EXPECT_EQ(pm1_.current_platoon_state, PlatoonState::CANDIDATELEADER); +} + +// ------------------------------------------------------------------------------------------------------------------------------------------- + +// test get leader function +TEST(PlatoonStrategicIHPPlugin, test_get_leader_front) +{ + PlatoonPluginConfig config; + std::shared_ptr wm = std::make_shared(); + + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + // change state + plugin.setPMState(PlatoonState::FOLLOWER); + + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + std::vector cur_pl; + cur_pl.push_back(member); + + // update platoon list + plugin.updatePlatoonList(cur_pl); + + // Use Getter to retrieve host Platoon Manager class + platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); + // check platoon size + EXPECT_EQ(pm_.platoon.size(), 1); + + PlatoonMember member1 = pm_.platoon[0]; + //check member info + EXPECT_EQ(member1.staticId, "1"); + + // update PM status + plugin.setToFollower(); + // update PM + platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); + PlatoonMember platoon_leader = pm1_.getDynamicLeader(); + // check dynamic leader info + EXPECT_EQ(platoon_leader.staticId, "1"); + +} + +// test platoon size and get leader platoon ID +TEST(PlatoonManagerTestFrontJoin, test2_front) +{ + platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); // dtd: 0.1m, smaller downtrack, at back. + std::vector cur_pl; + + cur_pl.push_back(*member); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = true; + pm.platoonSize = 1; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + std::string params = "CMDSPEED:11,DOWNTRACK:01,SPEED:11"; + + ros::Time::init(); + + pm.updatesOrAddMemberInfo("2", 2.0, 1.0, 2.5); // dtd: 1.0m, larger down track, in front. + + EXPECT_EQ(2, pm.platoon.size()); + EXPECT_EQ("2", pm.platoon[0].staticId); // sorted by dtd distance, larger downtrack means vehicle at front, hence ranked higher. +} + +// add 2 member to platoon, test size +TEST(PlatoonManagerTestFrontJoin, test3_front) +{ + // note: platoon list now include leading vehicle. So platoon_size = platoon.size() , here expect platoon_size == 2. + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + std::vector cur_pl; + + cur_pl.push_back(*member1); + cur_pl.push_back(*member2); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = false; + pm.platoonSize = 2; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + ros::Time::init(); + + int res = pm.getTotalPlatooningSize(); + + EXPECT_EQ(2, res); +} + +// test APF for 3 vehicles +TEST(PlatoonManagerTestFrontJoin, test4_front) +{ + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + std::vector cur_pl; + + cur_pl.push_back(*member1); + cur_pl.push_back(*member2); + + platoon_strategic_ihp::PlatoonManager pm; + pm.platoon = cur_pl; + + pm.isFollower = true; + pm.platoonSize = 2; + pm.leaderID = "0"; + pm.currentPlatoonID = "a"; + + ros::Time::init(); + + int res = pm.allPredecessorFollowing(); + + EXPECT_EQ(0, res); +} diff --git a/platooning_tactical_plugin/CMakeLists.txt b/platooning_tactical_plugin/CMakeLists.txt index 3792335230..a8ff05dba0 100644 --- a/platooning_tactical_plugin/CMakeLists.txt +++ b/platooning_tactical_plugin/CMakeLists.txt @@ -32,6 +32,7 @@ set( CATKIN_DEPS tf tf2 tf2_geometry_msgs + basic_autonomy ) ## Find catkin macros and libraries @@ -68,7 +69,6 @@ add_executable( ${PROJECT_NAME} add_library(platooning_tactical_plugin_library src/platooning_tactical_plugin.cpp - src/smoothing/BSpline.cpp ) target_link_libraries(platooning_tactical_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(platooning_tactical_plugin_library ${catkin_EXPORTED_TARGETS}) @@ -102,6 +102,5 @@ install(DIRECTORY launch config catkin_add_gmock(${PROJECT_NAME}-test test/test_platooning_tactical_plugin_planning.cpp - test/test_platooning_tactical_plugin.cpp ) target_link_libraries(${PROJECT_NAME}-test platooning_tactical_plugin_library ${catkin_LIBRARIES}) diff --git a/platooning_tactical_plugin/config/parameters.yaml b/platooning_tactical_plugin/config/parameters.yaml index 4393913cdc..6575865af6 100644 --- a/platooning_tactical_plugin/config/parameters.yaml +++ b/platooning_tactical_plugin/config/parameters.yaml @@ -1,6 +1,6 @@ # Float: The length of the trajectory in time domain # Unit: s -trajectory_time_length: 6.0 # Trajectory length in seconds +trajectory_time_length: 10.0 # Trajectory length in seconds # Float: Curve re-sampling step size in m # Unit: m curve_resample_step_size: 1.0 diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/log/log.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/log/log.h deleted file mode 100644 index 8d03410515..0000000000 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/log/log.h +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (C) 2019-2020 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 - -namespace platooning_tactical_plugin -{ -namespace log -{ -/** - * \brief Helper function to convert a lanelet::BasicPoint2d to a string - */ -std::string basicPointToStream(lanelet::BasicPoint2d point) -{ - std::ostringstream out; - out << point.x() << ", " << point.y(); - return out.str(); -} -/** - * \brief Helper function to convert a PointSpeedPair to a string - */ -std::string pointSpeedPairToStream(PointSpeedPair point) -{ - std::ostringstream out; - out << "Point: " << basicPointToStream(point.point) << " Speed: " << point.speed; - return out.str(); -} - -/** - * \brief Print a ROS_DEBUG_STREAM for each value in values where the printed value is a string returned by func - */ -template -void printDebugPerLine(const std::vector& values, std::function func) -{ - for (const auto& value : values) - { - ROS_DEBUG_STREAM(func(value)); - } -} - -/** - * \brief Print a ROS_DEBUG_STREAM for each value in values where the printed value is a string returned by free_func - */ -template -void printDebugPerLine(const std::vector& values, std::string (*free_func)(T)) -{ - auto function = static_cast>(free_func); - printDebugPerLine(values, function); -} - -/** - * \brief Print a ROS_DEBUG_STREAM for each value in values where the printed value is << prefix << value - */ -void printDoublesPerLineWithPrefix(const std::string& prefix, const std::vector& values) -{ - for (const auto& value : values) - { - ROS_DEBUG_STREAM(prefix << value); - } -} - -} // namespace log -} // namespace platooning_tactical_plugin diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h index 3fa287600e..8b767c0397 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h @@ -28,9 +28,9 @@ #include #include #include -#include #include #include +#include #include "platooning_tactical_plugin_config.h" @@ -81,175 +81,8 @@ class PlatooningTacticalPlugin */ bool onSpin(); - /** - * \brief Applies the provided speed limits to the provided speeds such that each element is capped at its corresponding speed limit if needed - * - * \param speeds The speeds to limit - * \param speed_limits The speed limits to apply. Must have the same size as speeds - * - * \return The capped speed limits. Has the same size as speeds - */ - std::vector apply_speed_limits(const std::vector speeds, const std::vector speed_limits); - - /** - * \brief Returns a 2D coordinate frame which is located at p1 and oriented so p2 lies on the +X axis - * - * \param p1 The origin point for the frame in the parent frame - * \param p2 A point in the parent frame that will define the +X axis relative to p1 - * - * \return A 2D coordinate frame transform - */ - Eigen::Isometry2d compute_heading_frame(const lanelet::BasicPoint2d& p1, const lanelet::BasicPoint2d& p2); - - /** - * \brief Reduces the input points to only those points that fit within the provided time boundary - * - * \param points The input point speed pairs to reduce - * \param time_span The time span in seconds which the output points will fit within - * - * \return The subset of points that fit within time_span - */ - std::vector constrain_to_time_boundary(const std::vector& points, double time_span); - - /** - * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of trajectory points for trajectory planning - * - * \param points The set of points that define the current lane the vehicle is in and are defined based on the request planning maneuvers. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. - * \param state The current state of the vehicle - * \param state_time The abosolute time which the provided vehicle state corresponds to - * - * \return A list of trajectory points to send to the carma planning stack - */ - std::vector - compose_trajectory_from_centerline(const std::vector& points, const cav_msgs::VehicleState& state, const ros::Time& state_time); - - /** - * \brief Method combines input points, times, orientations, and an absolute start time to form a valid carma platform trajectory - * - * NOTE: All input vectors must be the same size. The output vector will take this size. - * - * \param points The points in the map frame that the trajectory will follow. Units m - * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s - * \param yaws The orientation the vehicle should achieve at each point. Units radians - * \param startTime The absolute start time which will be used to update the input relative times. Units s - * - * \return A list of trajectory points built from the provided inputs. - */ - std::vector trajectory_from_points_times_orientations( - const std::vector& points, const std::vector& times, - const std::vector& yaws, ros::Time startTime); - - /** - * \brief Converts a set of requested Platooning maneuvers to point speed limit pairs. - * - * \param maneuvers The list of maneuvers to convert - * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. - * - * \param wm Pointer to intialized world model for semantic map access - * - * \return List of centerline points paired with speed limits - */ - std::vector maneuvers_to_points(const std::vector& maneuvers, - double max_starting_downtrack, - const carma_wm::WorldModelConstPtr& wm); - - /** - * \brief Returns the nearest point to the provided vehicle pose in the provided list - * - * \param points The points to evaluate - * \param state The current vehicle state - * - * \return index of nearest point in points - */ - int getNearestPointIndex(const std::vector& points, const cav_msgs::VehicleState& state); - - /** - * \brief Helper method to split a list of PointSpeedPair into separate point and speed lists - */ - void splitPointSpeedPairs(const std::vector& points, std::vector* basic_points, - std::vector* speeds); - - /** - * \brief Computes a spline based on the provided points - * - * \param basic_points The points to use for fitting the spline - * - * \return A spline which has been fit to the provided points - */ - std::unique_ptr compute_fit(const std::vector& basic_points); - - /** - * \brief Returns the speeds of points closest to the lookahead distance. - * - * \param points The points in the map frame that the trajectory will follow. Units m - * \param speeds Speeds assigned to points that trajectory will follow. Unit m/s - * \param lookahead The lookahead distance to obtain future points' speed. Unit m - * - * \return A vector of speed values shifted by the lookahead distance. - */ - - std::vector get_lookahead_speed(const std::vector& points, const std::vector& speeds, const double& lookahead); - - /** - * \brief Computes the lookahead distance based on the input velocity - * - * \param velocity vehicle velocity in m/s. - * - * \return lookahead distance in m. - */ - double get_adaptive_lookahead(double velocity); - - /** - * \brief Returns the min, and its idx, from the vector of values, excluding given set of values - * - * \param values vector of values - * - * \param excluded set of excluded values - * - * \return minimum value and its idx - */ - std::pair min_with_exclusions(const std::vector& values, const std::unordered_set& excluded) const; - - /** - * \brief Applies the longitudinal acceleration limit to each point's speed - * - * \param downtracks downtrack distances corresponding to each speed - * \param curv_speeds vehicle velocity in m/s. - * \param accel_limit vehicle longitudinal acceleration in m/s^2. - * - * \return optimized speeds for each dowtrack points that satisfies longitudinal acceleration - */ - std::vector optimize_speed(const std::vector& downtracks, const std::vector& curv_speeds, double accel_limit); - - int get_nearest_index_by_downtrack(const std::vector& points, - const cav_msgs::VehicleState& state) const; - - /** - * \brief Given the curvature fit, computes the curvature at the given step along the curve - * - * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature - * - * \param fit_curve curvature fit - * - * \return Curvature (k = 1/r, 1/meter) - */ - double compute_curvature_at(const smoothing::SplineI& fit_curve, double step_along_the_curve) const; - - /** - * \brief Attaches back_distance length of points in front of future points - * - * \param points all point speed pairs - * \param nearest_pt_index idx of nearest point to the vehicle - * \param future_points future points before which to attach the points - * \param back_distance number of back distance in meters - * - * \return point speed pairs with back distance length of points in front of future points - */ - std::vector attach_back_points(const std::vector& points, const int nearest_pt_index, - std::vector future_points, double back_distance) const; + cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later private: carma_wm::WorldModelConstPtr wm_; @@ -258,6 +91,7 @@ class PlatooningTacticalPlugin cav_msgs::Plugin plugin_discovery_msg_; carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; + DebugPublisher debug_publisher_; cav_msgs::VehicleState ending_state_before_buffer; //state before applying extra points for curvature calculation that are removed later }; diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h index 4bbc6cc16b..8785adc9c9 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h @@ -40,6 +40,7 @@ struct PlatooningTacticalPluginConfig bool enable_object_avoidance = true; // Activate object avoidance logic bool publish_debug = false; // True if debug publishing will be enabled double buffer_ending_downtrack = 20.0; + std::string desired_controller_plugin = "PlatooningControlPlugin"; ////The desired controller plugin for the platooning trajectory friend std::ostream& operator<<(std::ostream& output, const PlatooningTacticalPluginConfig& c) { @@ -59,6 +60,7 @@ struct PlatooningTacticalPluginConfig << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl << "publish_debug: " << c.publish_debug << std::endl << "buffer_ending_downtrack: " << c.buffer_ending_downtrack << std::endl + << "desired_controller_plugin: " << c.desired_controller_plugin << std::endl << "}" << std::endl; return output; } diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/BSpline.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/BSpline.h deleted file mode 100644 index f42a078444..0000000000 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/BSpline.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2020 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 - -namespace platooning_tactical_plugin -{ -namespace smoothing -{ - - typedef Eigen::Spline Spline2d; -/** - * \brief Realization of SplineI that uses the Eigen::Splines library for interpolation - */ -class BSpline : public SplineI -{ -public: - ~BSpline(){}; - void setPoints(std::vector points) override; - lanelet::BasicPoint2d operator()(double t) const override; - lanelet::BasicPoint2d first_deriv(double t) const override; - lanelet::BasicPoint2d second_deriv(double t) const override; - -private: - Spline2d spline_; -}; -}; // namespace smoothing -}; // namespace platooning_tactical_plugin diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/SplineI.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/SplineI.h deleted file mode 100644 index e72f090ea8..0000000000 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/SplineI.h +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2020 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 - -namespace platooning_tactical_plugin -{ -namespace smoothing -{ -/** - * \brief Interface to a spline interpolator that can be used to smoothly interpolate between points - */ -class SplineI -{ -public: - /** - * @brief Virtual destructor to ensure delete safety for pointers to implementing classes - * - */ - virtual ~SplineI(){}; - - /** - * \brief Set key points which the spline will interpolate between - * - * \param points The key points - */ - virtual void setPoints(std::vector points) = 0; - - /** - * \brief Get the BasicPoint2d coordinate along the curve at t-th step. - * - * \param t The t-th step to solve the spline at, where t is from 0 (beginning of curve) to 1 (end of curve) - * - * \return lanelet::BasicPoint2d with x, y that matches the t-th step along the curve - */ - virtual lanelet::BasicPoint2d operator()(double t) const = 0; - - /** - * \brief Get the BasicPoint2d representing the first_deriv along the curve at t-th step. - * - * \param t The t-th step to solve the spline at, where t is from 0 (beginning of curve) to 1 (end of curve) - * - * \return lanelet::BasicPoint2d with x, y that matches the first_deriv at t-th step along the curve. This is not partial derivatives - */ - virtual lanelet::BasicPoint2d first_deriv(double x) const = 0; - - /** - * \brief Get the BasicPoint2d representing the first_deriv along the curve at t-th step. - * - * \param t The t-th step to solve the spline at, where t is from 0 (beginning of curve) to 1 (end of curve) - * - * \return lanelet::BasicPoint2d with x, y that matches the second_deriv at t-th step along the curve. This is not partial derivatives - */ - virtual lanelet::BasicPoint2d second_deriv(double x) const = 0; -}; -}; // namespace smoothing -}; // namespace platooning_tactical_plugin diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/filters.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/filters.h deleted file mode 100644 index 6f3f6b7f38..0000000000 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/smoothing/filters.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2020 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 -namespace platooning_tactical_plugin -{ -namespace smoothing -{ -/** - * \brief Extremely simplie moving average filter - * - * \param input The points to be filtered - * \param window_size The number of points to use in the moving window for averaging - * - * \return The filterted points - */ -std::vector moving_average_filter(const std::vector input, int window_size) -{ - int i = 0; - double average; - std::deque samples; - std::vector output; - output.reserve(input.size()); - for (auto value : input) - { - if (i < window_size) - { - samples.push_back(value); - } - else - { - samples.pop_front(); - samples.push_back(value); - } - - double total = 0; - for (auto s : samples) - { - total += s; - } - - double average = total / samples.size(); - output.push_back(average); - i++; - } - - return output; -} -}; // namespace smoothing -}; // namespace platooning_tactical_plugin \ No newline at end of file diff --git a/platooning_tactical_plugin/package.xml b/platooning_tactical_plugin/package.xml index e4649182c3..ab773a83c5 100644 --- a/platooning_tactical_plugin/package.xml +++ b/platooning_tactical_plugin/package.xml @@ -37,5 +37,6 @@ The platooning_tactical_plugin converts maneuvers from platooning strategic plug tf2 tf2_geometry_msgs trajectory_utils +basic_autonomy carma_cmake_common \ No newline at end of file diff --git a/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp index 2f7af80e8e..6def33817b 100644 --- a/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp +++ b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp @@ -30,10 +30,6 @@ #include #include #include -#include -#include -#include -#include #include using oss = std::ostringstream; @@ -84,11 +80,24 @@ bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryReques } } + basic_autonomy:: waypoint_generation::DetailedTrajConfig wpg_detail_config; + basic_autonomy:: waypoint_generation::GeneralTrajConfig wpg_general_config; - - - - auto points_and_target_speeds = maneuvers_to_points(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance), wm_); // Convert maneuvers to points + wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("platooning", + 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_.max_accel * config_.max_accel_multiplier, + config_.lateral_accel_limit * config_.lat_accel_multiplier, + config_.speed_moving_average_window_size, + config_.curvature_moving_average_window_size, config_.back_distance, + config_.buffer_ending_downtrack, + config_.desired_controller_plugin); + + auto points_and_target_speeds = basic_autonomy::waypoint_generation::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); ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size()); @@ -98,11 +107,17 @@ bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryReques original_trajectory.header.frame_id = "map"; original_trajectory.header.stamp = ros::Time::now(); original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); - - original_trajectory.trajectory_points = compose_trajectory_from_centerline(points_and_target_speeds, req.vehicle_state, req.header.stamp); // Compute the trajectory + original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req.vehicle_state, req.header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + wpg_detail_config); // Compute the trajectory original_trajectory.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, config_.minimum_speed); - + resp.trajectory_plan = original_trajectory; + + if (config_.publish_debug) { // Publish the debug message if in debug logging mode + debug_msg_.trajectory_plan = resp.trajectory_plan; + debug_publisher_(debug_msg_); + } resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); @@ -115,659 +130,4 @@ bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryReques } -std::vector PlatooningTacticalPlugin::apply_speed_limits(const std::vector speeds, - const std::vector speed_limits) -{ - ROS_DEBUG_STREAM("Speeds list size: " << speeds.size()); - ROS_DEBUG_STREAM("SpeedLimits list size: " << speed_limits.size()); - - if (speeds.size() != speed_limits.size()) - { - throw std::invalid_argument("Speeds and speed limit lists not same size"); - } - std::vector out; - for (size_t i = 0; i < speeds.size(); i++) - { - out.push_back(std::min(speeds[i], speed_limits[i])); - } - - return out; -} - -Eigen::Isometry2d PlatooningTacticalPlugin::compute_heading_frame(const lanelet::BasicPoint2d& p1, - const lanelet::BasicPoint2d& p2) -{ - Eigen::Rotation2Dd yaw(atan2(p2.y() - p1.y(), p2.x() - p1.x())); - - return carma_wm::geometry::build2dEigenTransform(p1, yaw); -} - -std::pair PlatooningTacticalPlugin::min_with_exclusions(const std::vector& values, const std::unordered_set& excluded) const { - double min = std::numeric_limits::max(); - size_t best_idx = -1; - for (size_t i = 0; i < values.size(); i++) { - if (excluded.find(i) != excluded.end()) { - continue; - } - - if (values[i] < min) { - min = values[i]; - best_idx = i; - } - } - - return std::make_pair(min, best_idx); -} - -std::vector PlatooningTacticalPlugin::optimize_speed(const std::vector& downtracks, const std::vector& curv_speeds, double accel_limit) -{ - if (downtracks.size() != curv_speeds.size()) - { - throw std::invalid_argument("Downtracks and speeds do not have the same size"); - } - - if (accel_limit <= 0) - { - throw std::invalid_argument("Accel limits should be positive"); - } - - bool optimize = true; - std::unordered_set visited_idx; - visited_idx.reserve(curv_speeds.size()); - - std::vector output = curv_speeds; - - while (optimize) - { - auto min_pair = min_with_exclusions(curv_speeds, visited_idx); - size_t min_idx = std::get<1>(min_pair); - if (min_idx == -1) { - break; - } - - visited_idx.insert(min_idx); // Mark this point as visited - double v_i = std::get<0>(min_pair); - double x_i = downtracks[min_idx]; - for (int i = min_idx - 1; i > 0; i--) { // NOTE: Do not use size_t for i type here as -- with > 0 will result in overflow - // First point's speed is left unchanged as it is current speed of the vehicle - double v_f = curv_speeds[i]; - double dv = v_f - v_i; - - double x_f = downtracks[i]; - double dx = x_f - x_i; - - if(dv > 0) { - v_f = std::min(v_f, sqrt(v_i * v_i - 2 * accel_limit * dx)); // inverting accel as we are only visiting deceleration case - visited_idx.insert(i); - } else if (dv < 0) { - break; - } - output[i] = v_f; - v_i = v_f; - x_i = x_f; - } - } - - log::printDoublesPerLineWithPrefix("only_reverse[i]: ", output); - - output = trajectory_utils::apply_accel_limits_by_distance(downtracks, output, accel_limit, accel_limit); - log::printDoublesPerLineWithPrefix("after_forward[i]: ", output); - - return output; -} - - -std::vector PlatooningTacticalPlugin::constrain_to_time_boundary(const std::vector& points, - double time_span) -{ - std::vector basic_points; - std::vector speeds; - splitPointSpeedPairs(points, &basic_points, &speeds); - - std::vector downtracks = carma_wm::geometry::compute_arc_lengths(basic_points); - - size_t time_boundary_exclusive_index = - trajectory_utils::time_boundary_index(downtracks, speeds, config_.trajectory_time_length); - - if (time_boundary_exclusive_index == 0) - { - throw std::invalid_argument("No points to fit in timespan"); - } - - std::vector time_bound_points; - time_bound_points.reserve(time_boundary_exclusive_index); - - if (time_boundary_exclusive_index == points.size()) - { - time_bound_points.insert(time_bound_points.end(), points.begin(), - points.end()); // All points fit within time boundary - } - else - { - time_bound_points.insert(time_bound_points.end(), points.begin(), - points.begin() + time_boundary_exclusive_index - 1); // Limit points by time boundary - } - - return time_bound_points; -} - - -std::vector PlatooningTacticalPlugin::trajectory_from_points_times_orientations( - const std::vector& points, const std::vector& times, const std::vector& yaws, - ros::Time startTime) -{ - if (points.size() != times.size() || points.size() != yaws.size()) - { - throw std::invalid_argument("All input vectors must have the same size"); - } - - std::vector traj; - traj.reserve(points.size()); - - for (int i = 0; i < points.size(); i++) - { - cav_msgs::TrajectoryPlanPoint tpp; - ros::Duration relative_time(times[i]); - tpp.target_time = startTime + relative_time; - tpp.x = points[i].x(); - tpp.y = points[i].y(); - tpp.yaw = yaws[i]; - - tpp.controller_plugin_name = "PlatooningControlPlugin"; - tpp.planner_plugin_name = plugin_discovery_msg_.name; - - traj.push_back(tpp); - } - - return traj; -} - -std::vector PlatooningTacticalPlugin::maneuvers_to_points(const std::vector& maneuvers, - double max_starting_downtrack, - const carma_wm::WorldModelConstPtr& wm) -{ - std::vector points_and_target_speeds; - std::unordered_set visited_lanelets; - - bool first = true; - ROS_DEBUG_STREAM("VehDowntrack: " << max_starting_downtrack); - for (const auto& manuever : maneuvers) - { - if (manuever.type != cav_msgs::Maneuver::LANE_FOLLOWING) - { - throw std::invalid_argument("Platooning tactical plugin does not support this maneuver type"); - } - - if (manuever.lane_following_maneuver.parameters.negotiation_type == cav_msgs::ManeuverParameters::NO_NEGOTIATION) - { - throw std::invalid_argument("NO_NEGOTIATION maneuver is not supported by this plugin"); - } - - - // if (manuever.lane_following_maneuver.parameters.negotiation_type == cav_msgs::ManeuverParameters::PLATOONING) - // { - // // TODO: start and end speed are floats, sp compare them with epsilon - // // if ( manuever.lane_following_maneuver.start_speed != manuever.lane_following_maneuver.end_speed) - // // { - // // throw std::invalid_argument("Invalid Platooning Maneuver"); - // // } - // } - - cav_msgs::LaneFollowingManeuver lane_following_maneuver = manuever.lane_following_maneuver; - - double starting_downtrack = lane_following_maneuver.start_dist; - if (first) - { - if (starting_downtrack > max_starting_downtrack) - { - starting_downtrack = max_starting_downtrack; - } - first = false; - } - - ROS_DEBUG_STREAM("Used downtrack: " << starting_downtrack); - - auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist, true, true); - - if (lanelets.empty()) - { - ROS_ERROR_STREAM("Detected no lanelets between starting_downtrack: " << starting_downtrack << ", and lane_following_maneuver.end_dist: " << lane_following_maneuver.end_dist); - throw std::invalid_argument("Detected no lanelets between starting_downtrack and end_dist"); - } - - - ROS_DEBUG_STREAM("Maneuver"); - lanelet::BasicLineString2d downsampled_centerline; - downsampled_centerline.reserve(200); - - - // getLaneletsBetween is inclusive lanelets between its two boundaries - // which may return lanechange lanelets, so - // exclude lanechanges and plan for only the straight part - int curr_idx = 0; - auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]); - lanelet::ConstLanelets straight_lanelets; - - if (lanelets.size() <= 1) // no lane change anyways if only size 1 - { - ROS_DEBUG_STREAM("Detected one straight lanelet Id: " << lanelets[curr_idx].id()); - straight_lanelets = lanelets; - } - else - { - // skip all lanechanges until lane follow starts - while (curr_idx + 1 < lanelets.size() && - std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) == following_lanelets.end()) - { - ROS_DEBUG_STREAM("As there were no directly following lanelets after this, skipping lanelet id: " << lanelets[curr_idx].id()); - curr_idx ++; - following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]); - } - - ROS_DEBUG_STREAM("Added lanelet Id for lane follow: " << lanelets[curr_idx].id()); - // guaranteed to have at least one "straight" lanelet (e.g the last one in the list) - straight_lanelets.push_back(lanelets[curr_idx]); - - // add all lanelets on the straight road until next lanechange - while (curr_idx + 1 < lanelets.size() && - std::find(following_lanelets.begin(),following_lanelets.end(), lanelets[curr_idx + 1]) != following_lanelets.end()) - { - curr_idx++; - ROS_DEBUG_STREAM("Added lanelet Id forlane follow: " << lanelets[curr_idx].id()); - straight_lanelets.push_back(lanelets[curr_idx]); - following_lanelets = wm->getMapRoutingGraph()->following(lanelets[curr_idx]); - } - } - - for (auto l : straight_lanelets) - { - ROS_DEBUG_STREAM("Processing lanelet ID: " << l.id()); - if (visited_lanelets.find(l.id()) == visited_lanelets.end()) - { - - bool is_turn = false; - if(l.hasAttribute("turn_direction")) { - std::string turn_direction = l.attribute("turn_direction").value(); - is_turn = turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0; - } - - lanelet::BasicLineString2d centerline = l.centerline2d().basicLineString(); - lanelet::BasicLineString2d downsampled_points; - if (is_turn) { - downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.turn_downsample_ratio); - } else { - downsampled_points = carma_utils::containers::downsample_vector(centerline, config_.default_downsample_ratio); - } - downsampled_centerline = carma_wm::geometry::concatenate_line_strings(downsampled_centerline, downsampled_points); - visited_lanelets.insert(l.id()); - } - } - - first = true; - for (auto p : downsampled_centerline) - { - if (first && points_and_target_speeds.size() != 0) - { - first = false; - continue; // Skip the first point if we have already added points from a previous maneuver to avoid duplicates - } - PointSpeedPair pair; - pair.point = p; - pair.speed = lane_following_maneuver.end_speed; - points_and_target_speeds.push_back(pair); - } - } - - // Here we are limiting the trajectory length to the given length by maneuver end dist as opposed to the end of lanelets involved. - double starting_route_downtrack = wm_->routeTrackPos(points_and_target_speeds.front().point).downtrack; - double ending_downtrack = maneuvers.back().lane_following_maneuver.end_dist; - - if(ending_downtrack + config_.buffer_ending_downtrack < wm_->getRouteEndTrackPos().downtrack){ - ending_downtrack = ending_downtrack + config_.buffer_ending_downtrack; - } - else - { - ending_downtrack = wm_->getRouteEndTrackPos().downtrack; - } - - size_t max_i = points_and_target_speeds.size() - 1; - size_t unbuffered_idx = points_and_target_speeds.size() - 1; - bool found_unbuffered_idx = false; - double dist_accumulator = starting_route_downtrack; - lanelet::BasicPoint2d prev_point; - - for (int i = 0; i < points_and_target_speeds.size(); i ++) { - auto current_point = points_and_target_speeds[i].point; - if (i == 0) { - prev_point = current_point; - continue; - } - - double delta_d = lanelet::geometry::distance2d(prev_point, current_point); - ROS_DEBUG_STREAM("Index i: " << i << ", delta_d: " << delta_d << ", dist_accumulator:" << dist_accumulator <<", current_point.x():" << current_point.x() << - "current_point.y():" << current_point.y()); - dist_accumulator += delta_d; - if (dist_accumulator > maneuvers.back().lane_following_maneuver.end_dist && !found_unbuffered_idx) - { - unbuffered_idx = i - 1; - ROS_DEBUG_STREAM("Found index unbuffered_idx at: " << unbuffered_idx); - found_unbuffered_idx = true; - } - if (dist_accumulator > ending_downtrack) { - max_i = i - 1; - ROS_DEBUG_STREAM("Max_i breaking at: i: " << i << ", max_i: " << max_i); - break; - } - prev_point = current_point; - } - ending_state_before_buffer.x_pos_global = points_and_target_speeds[unbuffered_idx].point.x(); - ending_state_before_buffer.y_pos_global = points_and_target_speeds[unbuffered_idx].point.y(); - ROS_DEBUG_STREAM("Here ending_state_before_buffer.x_pos_global: " << ending_state_before_buffer.x_pos_global << - ", and y_pos_global" << ending_state_before_buffer.y_pos_global); - - std::vector constrained_points(points_and_target_speeds.begin(), points_and_target_speeds.begin() + max_i); - return constrained_points; -} - -int PlatooningTacticalPlugin::getNearestPointIndex(const std::vector& points, - const cav_msgs::VehicleState& state) -{ - lanelet::BasicPoint2d veh_point(state.x_pos_global, state.y_pos_global); - ROS_DEBUG_STREAM("veh_point: " << veh_point.x() << ", " << veh_point.y()); - double min_distance = std::numeric_limits::max(); - int i = 0; - int best_index = 0; - for (const auto& p : points) - { - double distance = lanelet::geometry::distance2d(p.point, veh_point); - ROS_DEBUG_STREAM("distance: " << distance); - ROS_DEBUG_STREAM("p: " << p.point.x() << ", " << p.point.y()); - if (distance < min_distance) - { - best_index = i; - min_distance = distance; - } - i++; - } - - return best_index; -} - - -void PlatooningTacticalPlugin::splitPointSpeedPairs(const std::vector& points, - std::vector* basic_points, - std::vector* speeds) -{ - basic_points->reserve(points.size()); - speeds->reserve(points.size()); - - for (const auto& p : points) - { - basic_points->push_back(p.point); - speeds->push_back(p.speed); - } -} - -std::unique_ptr -PlatooningTacticalPlugin::compute_fit(const std::vector& basic_points) -{ - if (basic_points.size() < 4) - { - ROS_WARN_STREAM("Insufficient Spline Points"); - return nullptr; - } - - std::unique_ptr spl = std::make_unique(); - spl->setPoints(basic_points); - - return spl; -} - -std::vector PlatooningTacticalPlugin::compose_trajectory_from_centerline( - const std::vector& points, const cav_msgs::VehicleState& state, const ros::Time& state_time) -{ - ROS_DEBUG_STREAM("VehicleState: " - << " x: " << state.x_pos_global << " y: " << state.y_pos_global << " yaw: " << state.orientation - << " speed: " << state.longitudinal_vel); - - ROS_DEBUG_STREAM("points size: " << points.size()); - - log::printDebugPerLine(points, &log::pointSpeedPairToStream); - - int nearest_pt_index = getNearestPointIndex(points, state); - - ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index); - - std::vector future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position - - auto time_bound_points = constrain_to_time_boundary(future_points, config_.trajectory_time_length); - - ROS_DEBUG_STREAM("time_bound_points size: " << time_bound_points.size()); - - log::printDebugPerLine(time_bound_points, &log::pointSpeedPairToStream); - - ROS_DEBUG("Got basic points "); - - std::vector back_and_future = attach_back_points(points,nearest_pt_index,time_bound_points, config_.back_distance); - - ROS_DEBUG_STREAM("Got back_and_future points with size" < speed_limits; - std::vector curve_points; - - splitPointSpeedPairs(back_and_future, &curve_points, &speed_limits); - - std::unique_ptr fit_curve = compute_fit(curve_points); // Compute splines based on curve points - if (!fit_curve) - { - throw std::invalid_argument("Could not fit a spline curve along the given trajectory!"); - } - - ROS_DEBUG("Got fit"); - - ROS_DEBUG_STREAM("speed_limits.size() " << speed_limits.size()); - - std::vector all_sampling_points; - all_sampling_points.reserve(1 + curve_points.size() * 2); - - std::vector distributed_speed_limits; - distributed_speed_limits.reserve(1 + curve_points.size() * 2); - - // compute total length of the trajectory to get correct number of points - // we expect using curve_resample_step_size - std::vector downtracks_raw = carma_wm::geometry::compute_arc_lengths(curve_points); - - int total_step_along_curve = static_cast(downtracks_raw.back() / config_.curve_resample_step_size); - - int current_speed_index = 0; - size_t total_point_size = curve_points.size(); - - double step_threshold_for_next_speed = (double)total_step_along_curve / (double)total_point_size; - double scaled_steps_along_curve = 0.0; // from 0 (start) to 1 (end) for the whole trajectory - std::vector better_curvature; - better_curvature.reserve(1 + curve_points.size() * 2); - - for (size_t steps_along_curve = 0; steps_along_curve < total_step_along_curve; steps_along_curve++) // Resample curve at tighter resolution - { - lanelet::BasicPoint2d p = (*fit_curve)(scaled_steps_along_curve); - - all_sampling_points.push_back(p); - double c = compute_curvature_at((*fit_curve), scaled_steps_along_curve); - better_curvature.push_back(c); - if ((double)steps_along_curve > step_threshold_for_next_speed) - { - step_threshold_for_next_speed += (double)total_step_along_curve / (double) total_point_size; - current_speed_index ++; - } - distributed_speed_limits.push_back(speed_limits[current_speed_index]); // Identify speed limits for resampled points - scaled_steps_along_curve += 1.0/total_step_along_curve; //adding steps_along_curve_step_size - } - - ROS_DEBUG_STREAM("Got sampled points with size:" << all_sampling_points.size()); - log::printDebugPerLine(all_sampling_points, &log::basicPointToStream); - - std::vector final_yaw_values = carma_wm::geometry::compute_tangent_orientations(all_sampling_points); - - log::printDoublesPerLineWithPrefix("raw_curvatures[i]: ", better_curvature); - - std::vector curvatures = smoothing::moving_average_filter(better_curvature, config_.curvature_moving_average_window_size); - - std::vector ideal_speeds = - trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit); - - log::printDoublesPerLineWithPrefix("curvatures[i]: ", curvatures); - log::printDoublesPerLineWithPrefix("ideal_speeds: ", ideal_speeds); - log::printDoublesPerLineWithPrefix("final_yaw_values[i]: ", final_yaw_values); - - std::vector constrained_speed_limits = apply_speed_limits(ideal_speeds, distributed_speed_limits); - log::printDoublesPerLineWithPrefix("constrained_speed_limits: ", constrained_speed_limits); - - ROS_DEBUG("Processed all points in computed fit"); - - if (all_sampling_points.size() == 0) - { - ROS_WARN_STREAM("No trajectory points could be generated"); - return {}; - } - - // Add current vehicle point to front of the trajectory - - nearest_pt_index = get_nearest_index_by_downtrack(all_sampling_points, state); - ROS_DEBUG_STREAM("Current state's nearest_pt_index: " << nearest_pt_index); - ROS_DEBUG_STREAM("Curvature right now: " << better_curvature[nearest_pt_index] << ", at state x: " << state.x_pos_global << ", state y: " << state.y_pos_global); - ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[nearest_pt_index].x() << ", y:" << all_sampling_points[nearest_pt_index].y()); - - int buffer_pt_index = get_nearest_index_by_downtrack(all_sampling_points, ending_state_before_buffer); - ROS_DEBUG_STREAM("Ending state's index before applying buffer (buffer_pt_index): " << buffer_pt_index); - ROS_DEBUG_STREAM("Corresponding to point: x: " << all_sampling_points[buffer_pt_index].x() << ", y:" << all_sampling_points[buffer_pt_index].y()); - - if (nearest_pt_index + 1 >= buffer_pt_index) - { - ROS_WARN_STREAM("Current state is at or passed the planned end distance. Couldn't generate trajectory"); - return {}; - } - - //drop buffer points here - std::vector future_basic_points(all_sampling_points.begin() + nearest_pt_index + 1, - all_sampling_points.begin()+ buffer_pt_index); // Points in front of current vehicle position - - std::vector future_speeds(constrained_speed_limits.begin() + nearest_pt_index + 1, - constrained_speed_limits.begin() + buffer_pt_index); // Points in front of current vehicle position - std::vector future_yaw(final_yaw_values.begin() + nearest_pt_index + 1, - final_yaw_values.begin() + buffer_pt_index); // Points in front of current vehicle position - std::vector final_actual_speeds = future_speeds; - all_sampling_points = future_basic_points; - final_yaw_values = future_yaw; - ROS_DEBUG_STREAM("Trimmed future points to size: " << future_basic_points.size() ); - - lanelet::BasicPoint2d cur_veh_point(state.x_pos_global, state.y_pos_global); - - all_sampling_points.insert(all_sampling_points.begin(), - cur_veh_point); // Add current vehicle position to front of sample points - - final_actual_speeds.insert(final_actual_speeds.begin(), state.longitudinal_vel); - - final_yaw_values.insert(final_yaw_values.begin(), state.orientation); - - // Compute points to local downtracks - std::vector downtracks = carma_wm::geometry::compute_arc_lengths(all_sampling_points); - - // Apply accel limits - final_actual_speeds = optimize_speed(downtracks, final_actual_speeds, config_.max_accel); - - log::printDoublesPerLineWithPrefix("postAccel[i]: ", final_actual_speeds); - - final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, config_.speed_moving_average_window_size); - log::printDoublesPerLineWithPrefix("post_average[i]: ", final_actual_speeds); - - for (auto& s : final_actual_speeds) // Limit minimum speed. TODO how to handle stopping? - { - s = std::max(s, config_.minimum_speed); - } - - log::printDoublesPerLineWithPrefix("post_min_speed[i]: ", final_actual_speeds); - - // Convert speeds to times - std::vector times; - trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, ×); - - log::printDoublesPerLineWithPrefix("times[i]: ", times); - - // Build trajectory points - std::vector traj_points = - trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time); - - if (config_.publish_debug) { - carma_debug_msgs::TrajectoryCurvatureSpeeds msg; - msg.velocity_profile = final_actual_speeds; - msg.relative_downtrack = downtracks; - msg.tangent_headings = final_yaw_values; - std::vector aligned_speed_limits(constrained_speed_limits.begin() + nearest_pt_index, - constrained_speed_limits.end()); - msg.speed_limits = aligned_speed_limits; - std::vector aligned_curvatures(curvatures.begin() + nearest_pt_index, - curvatures.end()); - msg.curvatures = aligned_curvatures; - msg.lat_accel_limit = config_.lateral_accel_limit; - msg.lon_accel_limit = config_.max_accel; - msg.starting_state = state; - debug_msg_ = msg; - } - return traj_points; -} - -int PlatooningTacticalPlugin::get_nearest_index_by_downtrack(const std::vector& points, - const cav_msgs::VehicleState& state) const -{ - lanelet::BasicPoint2d veh_point(state.x_pos_global, state.y_pos_global); - double min_distance = std::numeric_limits::max(); - int i = 0; - int best_index = 0; - for (const auto& p : points) - { - double distance = lanelet::geometry::distance2d(p, veh_point); - if (distance < min_distance) - { - best_index = i; - min_distance = distance; - } - i++; - } - - return best_index; -} - -double PlatooningTacticalPlugin::compute_curvature_at(const smoothing::SplineI& fit_curve, double step_along_the_curve) const -{ - lanelet::BasicPoint2d f_prime_pt = fit_curve.first_deriv(step_along_the_curve); - lanelet::BasicPoint2d f_prime_prime_pt = fit_curve.second_deriv(step_along_the_curve); - // Convert to 3d vector to do 3d vector operations like cross. - Eigen::Vector3d f_prime = {f_prime_pt.x(), f_prime_pt.y(), 0}; - Eigen::Vector3d f_prime_prime = {f_prime_prime_pt.x(), f_prime_prime_pt.y(), 0}; - return (f_prime.cross(f_prime_prime)).norm()/(pow(f_prime.norm(),3)); -} - -std::vector PlatooningTacticalPlugin::attach_back_points(const std::vector& points, - const int nearest_pt_index, std::vector future_points, double back_distance) const -{ - std::vector back_and_future; - back_and_future.reserve(points.size()); - double total_dist = 0; - int min_i = 0; - for (int i = nearest_pt_index; i > 0; --i) { - min_i = i; - total_dist += lanelet::geometry::distance2d(points[i].point, points[i-1].point); - - if (total_dist > back_distance) { - break; - } - } - - back_and_future.insert(back_and_future.end(), points.begin() + min_i, points.begin() + nearest_pt_index + 1); - back_and_future.insert(back_and_future.end(), future_points.begin(), future_points.end()); - return back_and_future; -} - - } // namespace platooning_tactical_plugin diff --git a/platooning_tactical_plugin/src/smoothing/BSpline.cpp b/platooning_tactical_plugin/src/smoothing/BSpline.cpp deleted file mode 100644 index da5487ca45..0000000000 --- a/platooning_tactical_plugin/src/smoothing/BSpline.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include - -namespace platooning_tactical_plugin -{ -namespace smoothing -{ -void BSpline::setPoints(std::vector points) -{ - Eigen::MatrixXd matrix_points(2, points.size()); - int row_index = 0; - for(auto const point : points){ - matrix_points.col(row_index) << point.x(), point.y(); - row_index++; - } - spline_ = Eigen::SplineFitting::Interpolate(matrix_points,3 ); -} -lanelet::BasicPoint2d BSpline::operator()(double t) const -{ - Eigen::VectorXd values = spline_(t); - lanelet::BasicPoint2d pt = {values.x(), values.y()}; - return pt; -} - -lanelet::BasicPoint2d BSpline::first_deriv(double t) const { - Eigen::Array2Xd v = spline_.derivatives(t, 1); - lanelet::BasicPoint2d output = {v(2), v(3)}; - return output; -} - -lanelet::BasicPoint2d BSpline::second_deriv(double t) const { - Eigen::Array2Xd v = spline_.derivatives(t, 2); - lanelet::BasicPoint2d output = {v(4), v(5)}; - return output; -} - -}; // namespace smoothing -}; // namespace platooning_tactical_plugin \ No newline at end of file diff --git a/platooning_tactical_plugin/test/test_platooning_tactical_plugin.cpp b/platooning_tactical_plugin/test/test_platooning_tactical_plugin.cpp deleted file mode 100644 index 3f5d67f7bf..0000000000 --- a/platooning_tactical_plugin/test/test_platooning_tactical_plugin.cpp +++ /dev/null @@ -1,586 +0,0 @@ -/* - * Copyright (C) 2019-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 - -using namespace platooning_tactical_plugin; -// Test to ensure Eigen::Isometry2d behaves like tf2::Transform -TEST(PlatooningTacticalPluginTest, validate_eigen) -{ - Eigen::Rotation2Dd frame_rot(M_PI_2); - lanelet::BasicPoint2d origin(1, 1); - Eigen::Isometry2d B_in_A = carma_wm::geometry::build2dEigenTransform(origin, frame_rot); - - Eigen::Rotation2Dd new_rot(B_in_A.rotation()); - - ASSERT_EQ(2, B_in_A.translation().size()); - ASSERT_NEAR(1.0, B_in_A.translation()[0], 0.000000001); - ASSERT_NEAR(1.0, B_in_A.translation()[1], 0.000000001); - ASSERT_NEAR(M_PI_2, new_rot.smallestAngle(), 0.000000001); - - lanelet::BasicPoint2d p_in_B(0.5, -1); - lanelet::BasicPoint2d p_in_A = B_in_A * p_in_B; - - ASSERT_NEAR(2.0, p_in_A.x(), 0.000000001); - ASSERT_NEAR(1.5, p_in_A.y(), 0.000000001); - - Eigen::Rotation2Dd zero_rot(0.0); - Eigen::Isometry2d P_in_B_as_tf = carma_wm::geometry::build2dEigenTransform(p_in_B, zero_rot); - Eigen::Isometry2d P_in_A = B_in_A * P_in_B_as_tf; - Eigen::Rotation2Dd P_in_A_rot(P_in_A.rotation()); - - ASSERT_EQ(2, P_in_A.translation().size()); - ASSERT_NEAR(2.0, P_in_A.translation()[0], 0.000000001); - ASSERT_NEAR(1.5, P_in_A.translation()[1], 0.000000001); - ASSERT_NEAR(M_PI_2, P_in_A_rot.smallestAngle(), 0.000000001); -} - - -TEST(PlatooningTacticalPluginTest, trajectory_from_points_times_orientations) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - lanelet::BasicPoint2d p1(0.0, 0.0); - lanelet::BasicPoint2d p2(2.0, 0.0); - lanelet::BasicPoint2d p3(4.5, 0.0); - lanelet::BasicPoint2d p4(7.0, 3.0); - - std::vector points = { p1, p2, p3, p4 }; - - std::vector times = { 0, 2, 4, 8 }; - std::vector yaws = { 0.2, 0.5, 0.6, 1.0 }; - ros::Time startTime(1.0); - std::vector traj_points = - plugin.trajectory_from_points_times_orientations(points, times, yaws, startTime); - - ASSERT_EQ(4, traj_points.size()); - ASSERT_NEAR(1.0, traj_points[0].target_time.toSec(), 0.0000001); - ASSERT_NEAR(3.0, traj_points[1].target_time.toSec(), 0.0000001); - ASSERT_NEAR(5.0, traj_points[2].target_time.toSec(), 0.0000001); - ASSERT_NEAR(9.0, traj_points[3].target_time.toSec(), 0.0000001); - - ASSERT_NEAR(0.0, traj_points[0].x, 0.0000001); - ASSERT_NEAR(2.0, traj_points[1].x, 0.0000001); - ASSERT_NEAR(4.5, traj_points[2].x, 0.0000001); - ASSERT_NEAR(7.0, traj_points[3].x, 0.0000001); - - ASSERT_NEAR(0.0, traj_points[0].y, 0.0000001); - ASSERT_NEAR(0.0, traj_points[1].y, 0.0000001); - ASSERT_NEAR(0.0, traj_points[2].y, 0.0000001); - ASSERT_NEAR(3.0, traj_points[3].y, 0.0000001); - - ASSERT_NEAR(0.2, traj_points[0].yaw, 0.0000001); - ASSERT_NEAR(0.5, traj_points[1].yaw, 0.0000001); - ASSERT_NEAR(0.6, traj_points[2].yaw, 0.0000001); - ASSERT_NEAR(1.0, traj_points[3].yaw, 0.0000001); - - std::string controller_plugin = "default"; - ASSERT_EQ(0, traj_points[0].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[1].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[2].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[3].controller_plugin_name.compare(controller_plugin)); - - std::string expected_plugin_name = "PlatooningTacticalPlugin"; - ASSERT_EQ(0, traj_points[0].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[1].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[2].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[3].planner_plugin_name.compare(expected_plugin_name)); -} - -TEST(PlatooningTacticalPluginTest, constrain_to_time_boundary) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 0); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(6, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(7, 0); - points.push_back(p); - - - std::vector time_bound_points = plugin.constrain_to_time_boundary(points, 5.0); - - ASSERT_EQ(6, time_bound_points.size()); - ASSERT_NEAR(0.0, time_bound_points[0].point.x(), 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[1].point.x(), 0.0000001); - ASSERT_NEAR(2.0, time_bound_points[2].point.x(), 0.0000001); - ASSERT_NEAR(3.0, time_bound_points[3].point.x(), 0.0000001); - ASSERT_NEAR(4.0, time_bound_points[4].point.x(), 0.0000001); - ASSERT_NEAR(5.0, time_bound_points[5].point.x(), 0.0000001); - - ASSERT_NEAR(0.0, time_bound_points[0].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[1].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[2].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[3].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[4].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[5].point.y(), 0.0000001); - - ASSERT_NEAR(1.0, time_bound_points[0].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[1].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[2].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[3].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[4].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[5].speed, 0.0000001); -} - - -TEST(PlatooningTacticalPluginTest, getNearestPointIndex) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 0); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 1); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 4); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 5); - points.push_back(p); - p.point = lanelet::BasicPoint2d(6, 6); - points.push_back(p); - p.point = lanelet::BasicPoint2d(7, 7); - points.push_back(p); - - cav_msgs::VehicleState state; - state.x_pos_global = 3.3; - state.y_pos_global = 3.3; - - ASSERT_EQ(3, plugin.getNearestPointIndex(points, state)); -} - - -TEST(PlatooningTacticalPluginTest, splitPointSpeedPairs) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 1); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 4); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 5); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 6); - points.push_back(p); - - std::vector basic_points; - std::vector speeds; - - plugin.splitPointSpeedPairs(points, &basic_points, &speeds); - - ASSERT_EQ(points.size(), basic_points.size()); - ASSERT_NEAR(0.0, basic_points[0].x(), 0.0000001); - ASSERT_NEAR(1.0, basic_points[1].x(), 0.0000001); - ASSERT_NEAR(2.0, basic_points[2].x(), 0.0000001); - ASSERT_NEAR(3.0, basic_points[3].x(), 0.0000001); - ASSERT_NEAR(4.0, basic_points[4].x(), 0.0000001); - ASSERT_NEAR(5.0, basic_points[5].x(), 0.0000001); - - ASSERT_NEAR(1.0, basic_points[0].y(), 0.0000001); - ASSERT_NEAR(2.0, basic_points[1].y(), 0.0000001); - ASSERT_NEAR(3.0, basic_points[2].y(), 0.0000001); - ASSERT_NEAR(4.0, basic_points[3].y(), 0.0000001); - ASSERT_NEAR(5.0, basic_points[4].y(), 0.0000001); - ASSERT_NEAR(6.0, basic_points[5].y(), 0.0000001); - - ASSERT_NEAR(1.0, speeds[0], 0.0000001); - ASSERT_NEAR(1.0, speeds[1], 0.0000001); - ASSERT_NEAR(1.0, speeds[2], 0.0000001); - ASSERT_NEAR(1.0, speeds[3], 0.0000001); - ASSERT_NEAR(1.0, speeds[4], 0.0000001); - ASSERT_NEAR(1.0, speeds[5], 0.0000001); -} - -TEST(PlatooningTacticalPluginTest, compute_fit) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - /////////////////////// - // Check straight line - /////////////////////// - std::vector points; - auto p = lanelet::BasicPoint2d(20, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(21, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(22, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(23, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(24, 30); - points.push_back(p); - std::unique_ptr fit_curve = plugin.compute_fit(points); - std::vector spline_points; - // Following logic is written for BSpline library. Switch with appropriate call of the new library if different. - float parameter = 0.0; - for(int i=0; i< points.size(); i++){ - lanelet::BasicPoint2d pt = (*fit_curve)(parameter); - - // Uncomment to print and check if this generated map matches with the original one above - // ROS_INFO_STREAM("BSpline point: x: " << values.x() << "y: " << values.y()); - spline_points.push_back(pt); - parameter += 1.0/(points.size()*1.0); - } - - ASSERT_EQ(spline_points.size(), points.size()); - int error_count = 0; - - tf::Vector3 original_vector_1(points[1].x() - points[0].x(), - points[1].y() - points[0].y(), 0); - original_vector_1.setZ(0); - tf::Vector3 spline_vector_1(spline_points[1].x() - spline_points[0].x(), - spline_points[1].y() - spline_points[0].y(), 0); - spline_vector_1.setZ(0); - tf::Vector3 original_vector_2(points[2].x() - points[1].x(), - points[2].y() - points[1].y(), 0); - original_vector_2.setZ(0); - tf::Vector3 spline_vector_2(spline_points[2].x() - spline_points[1].x(), - spline_points[2].y() - spline_points[1].y(), 0); - spline_vector_2.setZ(0); - double angle_in_rad_1 = std::fabs(tf::tfAngle(original_vector_1, spline_vector_1)); - double angle_in_rad_2 = std::fabs(tf::tfAngle(original_vector_2, spline_vector_2)); - - ASSERT_NEAR(angle_in_rad_1, 0.0, 0.0001); - ASSERT_NEAR(angle_in_rad_2, 0.0, 0.0001); - - /////////////////////// - // S curve - /////////////////////// - points = {}; - lanelet::BasicPoint2d po1(3,4); - points.push_back( po1); - lanelet::BasicPoint2d po2(5,4); - points.push_back( po2); - lanelet::BasicPoint2d po3(8,9); - points.push_back( po3); - lanelet::BasicPoint2d po4(8,23); - points.push_back( po4); - lanelet::BasicPoint2d po5(3.5,25); - points.push_back( po5); - lanelet::BasicPoint2d po6(3,25); - points.push_back( po6); - lanelet::BasicPoint2d po7(2.5,26); - points.push_back( po7); - lanelet::BasicPoint2d po8(2.25,27); - points.push_back( po8); - lanelet::BasicPoint2d po9(2.0,28); - points.push_back( po9); - lanelet::BasicPoint2d po10(1.5,30); - points.push_back(po10); - lanelet::BasicPoint2d po11(1.0,32); - points.push_back(po11); - lanelet::BasicPoint2d po12(1.25,34); - points.push_back(po12); - lanelet::BasicPoint2d po13(2.0,35); - points.push_back(po13); - lanelet::BasicPoint2d po14(4.0,35); - points.push_back(po14); - lanelet::BasicPoint2d po15(5.0,35.5); - points.push_back(po15); - lanelet::BasicPoint2d po16(6.0,36); - points.push_back(po16); - lanelet::BasicPoint2d po17(7.0,50); - points.push_back(po17); - lanelet::BasicPoint2d po18(6.5,48); - points.push_back(po18); - lanelet::BasicPoint2d po19(4.0,43); - points.push_back(po19); - - // As different libraries may fit S curves differently, we are only checking if we can get any fit here. - ASSERT_NO_THROW(plugin.compute_fit(points)); - - std::unique_ptr fit_s_curve = plugin.compute_fit(points); - - ASSERT_TRUE(!!fit_s_curve); -} - -TEST(PlatooningTacticalPluginTest, optimize_speed) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector downtracks, curv_speeds; - downtracks.push_back(0); - downtracks.push_back(2); - downtracks.push_back(4); - downtracks.push_back(6); - downtracks.push_back(8); - downtracks.push_back(10); - downtracks.push_back(12); - downtracks.push_back(14); - downtracks.push_back(16); - - config.max_accel = 2.0; - double max_accel = config.max_accel; - - ASSERT_THROW(plugin.optimize_speed(downtracks, curv_speeds, max_accel), std::invalid_argument); - - curv_speeds.push_back(1); - curv_speeds.push_back(3); - curv_speeds.push_back(4); - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(0); - curv_speeds.push_back(3); - curv_speeds.push_back(3); - curv_speeds.push_back(6); - - ASSERT_THROW(plugin.optimize_speed(downtracks, curv_speeds, -10), std::invalid_argument); - - std::vector expected_results; - expected_results.push_back(1); - expected_results.push_back(3); - expected_results.push_back(4); - expected_results.push_back(3); - expected_results.push_back(1); - expected_results.push_back(0); - expected_results.push_back(2.82843); - expected_results.push_back(3); - expected_results.push_back(4.12311); - auto test_results = plugin.optimize_speed(downtracks, curv_speeds, max_accel); - - ASSERT_NEAR(expected_results[0], test_results[0], 0.001); - ASSERT_NEAR(expected_results[1], test_results[1], 0.001); - ASSERT_NEAR(expected_results[2], test_results[2], 0.001); - ASSERT_NEAR(expected_results[3], test_results[3], 0.001); - ASSERT_NEAR(expected_results[4], test_results[4], 0.001); - ASSERT_NEAR(expected_results[5], test_results[5], 0.001); - ASSERT_NEAR(expected_results[6], test_results[6], 0.001); - ASSERT_NEAR(expected_results[7], test_results[7], 0.001); - ASSERT_NEAR(expected_results[8], test_results[8], 0.001); - - // Check if the first speed is same - curv_speeds = {}; - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(3); - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(0); - curv_speeds.push_back(3); - curv_speeds.push_back(3); - curv_speeds.push_back(6); - - expected_results = {}; - expected_results.push_back(4); - expected_results.push_back(2.82847); - expected_results.push_back(3); - expected_results.push_back(3); - expected_results.push_back(1); - expected_results.push_back(0); - expected_results.push_back(2.82843); - expected_results.push_back(3); - expected_results.push_back(4.12311); - - test_results = plugin.optimize_speed(downtracks, curv_speeds, max_accel); - - ASSERT_NEAR(expected_results[0], test_results[0], 0.001); - ASSERT_NEAR(expected_results[1], test_results[1], 0.001); - ASSERT_NEAR(expected_results[2], test_results[2], 0.001); - ASSERT_NEAR(expected_results[3], test_results[3], 0.001); - ASSERT_NEAR(expected_results[4], test_results[4], 0.001); - ASSERT_NEAR(expected_results[5], test_results[5], 0.001); - ASSERT_NEAR(expected_results[6], test_results[6], 0.001); - ASSERT_NEAR(expected_results[7], test_results[7], 0.001); - ASSERT_NEAR(expected_results[8], test_results[8], 0.001); - -} - -TEST(PlatooningTacticalPluginTest, compute_curvature_at) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - /////////////////////// - // Check straight line - /////////////////////// - std::vector points; - auto p = lanelet::BasicPoint2d(20, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(21, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(22, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(23, 30); - points.push_back(p); - std::unique_ptr fit_curve = plugin.compute_fit(points); - - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.0), 0, 0.001); // check start - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 1.0), 0, 0.001); // check end - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.23), 0, 0.001); // check random 1 - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.97), 0, 0.001); // check random 2 - - /////////////////////// - // Circle (0,0 centered, R radius) - /////////////////////// - points = {}; - std::vector x,y; - double x_ = 0.0; - double radius = 20; - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); //y- - x_ += radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(sqrt(pow(radius,2) - pow(x_,2))); //y+ - x_ -= radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(sqrt(pow(radius,2) - pow(x_,2))); //y+ - x_ -= radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); //y- - x_ += radius/(double)10; - } - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); // to close the loop with redundant first point - - for (auto i = 0; i < y.size(); i ++) - { - points.push_back({x[i],y[i]}); - } - - std::unique_ptr fit_circle = plugin.compute_fit(points); - double param = 0.0; - for (int i = 0 ; i < 40; i ++) - { - auto pt = (*fit_circle)(param); - param += 1.0/40.0; - } - auto pt = (*fit_circle)(param); - - double circle_param = 0.0; - for ( auto i= 0; i < 50; i++) - { - circle_param += 0.02; - } - - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.0), 1.0/radius, 0.005); // check start curvature 1/r - // check curvature is consistent - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.42), plugin.compute_curvature_at((*fit_circle), 0.85), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.0), plugin.compute_curvature_at((*fit_circle), 1.0), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.23), plugin.compute_curvature_at((*fit_circle), 0.99), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.12), plugin.compute_curvature_at((*fit_circle), 0.76), 0.005); -} - -TEST(PlatooningTacticalPluginTest, attach_back_points) -{ - PlatooningTacticalPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - std::vector future_points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 1); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 4); - future_points.push_back(p); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 5); - future_points.push_back(p); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 6); - future_points.push_back(p); - points.push_back(p); - - int nearest_pt_index = 2; - - auto result = plugin.attach_back_points(points, nearest_pt_index, future_points, 1.5); - - ASSERT_EQ(points.size() -1, result.size()); - ASSERT_NEAR(1.0, result[0].point.x(), 0.0000001); - ASSERT_NEAR(2.0, result[1].point.x(), 0.0000001); - ASSERT_NEAR(3.0, result[2].point.x(), 0.0000001); - ASSERT_NEAR(4.0, result[3].point.x(), 0.0000001); - ASSERT_NEAR(5.0, result[4].point.x(), 0.0000001); - - ASSERT_NEAR(2.0, result[0].point.y(), 0.0000001); - ASSERT_NEAR(3.0, result[1].point.y(), 0.0000001); - ASSERT_NEAR(4.0, result[2].point.y(), 0.0000001); - ASSERT_NEAR(5.0, result[3].point.y(), 0.0000001); - ASSERT_NEAR(6.0, result[4].point.y(), 0.0000001); - -} diff --git a/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp b/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp index 8636595ee8..5fd989c049 100644 --- a/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp +++ b/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp @@ -47,7 +47,7 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallback) PlatooningTacticalPluginConfig config; config.default_downsample_ratio = 1; std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); + PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) { ROS_DEBUG_STREAM(msg); }); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); @@ -116,7 +116,7 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallbackexception) PlatooningTacticalPluginConfig config; config.default_downsample_ratio = 1; std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) {}); + PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) { ROS_DEBUG_STREAM(msg); }); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); @@ -180,7 +180,7 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallbackexception) } -}; // namespace platooning_tactical_plugin +} // namespace platooning_tactical_plugin // Run all the tests int main(int argc, char **argv) diff --git a/port_drayage_plugin/src/port_drayage_plugin.cpp b/port_drayage_plugin/src/port_drayage_plugin.cpp index 36947048e5..50546a0f4f 100644 --- a/port_drayage_plugin/src/port_drayage_plugin.cpp +++ b/port_drayage_plugin/src/port_drayage_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-2022 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 @@ -127,7 +127,7 @@ namespace port_drayage_plugin bool PortDrayagePlugin::call_set_active_route_client(cav_srvs::SetActiveRoute req){ if(_set_active_route_client.call(req)){ - if(req.response.errorStatus == cav_srvs::SetActiveRouteResponse::NO_ERROR){ + if(req.response.error_status == cav_srvs::SetActiveRouteResponse::NO_ERROR){ ROS_DEBUG_STREAM("Route Generation succeeded for Set Active Route service call."); return true; } diff --git a/port_drayage_plugin/src/port_drayage_worker.cpp b/port_drayage_plugin/src/port_drayage_worker.cpp index d6b288b8c1..1934e3b2ab 100644 --- a/port_drayage_plugin/src/port_drayage_worker.cpp +++ b/port_drayage_plugin/src/port_drayage_worker.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-2022 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 @@ -113,7 +113,7 @@ namespace port_drayage_plugin cav_srvs::SetActiveRoute route_req; if (dest_latitude && dest_longitude) { route_req.request.choice = cav_srvs::SetActiveRouteRequest::DESTINATION_POINTS_ARRAY; - route_req.request.routeID = _latest_mobility_operation_msg.operation; + route_req.request.route_id = _latest_mobility_operation_msg.operation; cav_msgs::Position3D destination_point; destination_point.latitude = *_latest_mobility_operation_msg.dest_latitude; diff --git a/port_drayage_plugin/test/test_port_drayage_plugin.cpp b/port_drayage_plugin/test/test_port_drayage_plugin.cpp index 64813e2288..48c46897a8 100644 --- a/port_drayage_plugin/test/test_port_drayage_plugin.cpp +++ b/port_drayage_plugin/test/test_port_drayage_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * Copyright (C) 2019-2022 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 @@ -507,7 +507,7 @@ TEST(PortDrayageTest, testComposeSetActiveRouteRequest) // Verify the results of PortDrayageWorker's compose_set_active_route_request() method cav_srvs::SetActiveRoute route_req = pdw.compose_set_active_route_request(*pdw._latest_mobility_operation_msg.dest_latitude, *pdw._latest_mobility_operation_msg.dest_longitude); ASSERT_EQ(cav_srvs::SetActiveRouteRequest::DESTINATION_POINTS_ARRAY, route_req.request.choice); - ASSERT_EQ("MOVING_TO_LOADING_AREA", route_req.request.routeID); + ASSERT_EQ("MOVING_TO_LOADING_AREA", route_req.request.route_id); ASSERT_EQ(1, route_req.request.destination_points.size()); ASSERT_EQ(38.9550038, route_req.request.destination_points[0].latitude); ASSERT_EQ(-77.1481983, route_req.request.destination_points[0].longitude); diff --git a/roadway_objects/include/roadway_objects/roadway_objects_node.hpp b/roadway_objects/include/roadway_objects/roadway_objects_node.hpp index f3c02054a9..86b8190cd8 100644 --- a/roadway_objects/include/roadway_objects/roadway_objects_node.hpp +++ b/roadway_objects/include/roadway_objects/roadway_objects_node.hpp @@ -47,10 +47,10 @@ namespace roadway_objects carma_ros2_utils::PubPtr roadway_obs_pub_; // World Model Listener. Must be declared before object_worker_ for proper initialization. - carma_wm::WMListener wm_listener_; + std::shared_ptr wm_listener_; // Worker class object - RoadwayObjectsWorker object_worker_; + std::shared_ptr object_worker_; public: /** diff --git a/roadway_objects/src/roadway_objects_node.cpp b/roadway_objects/src/roadway_objects_node.cpp index 4d03edca22..f58a4c870f 100644 --- a/roadway_objects/src/roadway_objects_node.cpp +++ b/roadway_objects/src/roadway_objects_node.cpp @@ -20,24 +20,26 @@ namespace roadway_objects namespace std_ph = std::placeholders; RoadwayObjectsNode::RoadwayObjectsNode(const rclcpp::NodeOptions &options) - : carma_ros2_utils::CarmaLifecycleNode(options), - wm_listener_(this->get_node_base_interface(), this->get_node_logging_interface(), - this->get_node_topics_interface(), this->get_node_parameters_interface()), - object_worker_(wm_listener_.getWorldModel(), std::bind(&RoadwayObjectsNode::publishObstacles, this, std_ph::_1), get_node_logging_interface()) + : carma_ros2_utils::CarmaLifecycleNode(options) { } carma_ros2_utils::CallbackReturn RoadwayObjectsNode::handle_on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO_STREAM(get_logger(), "RoadwayObjectsNode trying to configure"); - - // Setup subscribers - external_objects_sub_ = create_subscription("external_objects", 10, - std::bind(&RoadwayObjectsWorker::externalObjectsCallback, &object_worker_, std_ph::_1)); + wm_listener_ = std::make_shared(get_node_base_interface(), get_node_logging_interface(), + get_node_topics_interface(), get_node_parameters_interface()); + + object_worker_ = std::make_shared(wm_listener_->getWorldModel(), std::bind(&RoadwayObjectsNode::publishObstacles, this, std_ph::_1), get_node_logging_interface()); + // Setup publishers roadway_obs_pub_ = create_publisher("roadway_objects", 10); + // Setup subscribers + external_objects_sub_ = create_subscription("external_objects", 10, + std::bind(&RoadwayObjectsWorker::externalObjectsCallback, object_worker_.get(), std_ph::_1)); + // Return success if everthing initialized successfully return CallbackReturn::SUCCESS; } diff --git a/route/CMakeLists.txt b/route/CMakeLists.txt old mode 100755 new mode 100644 index dd93ef976c..3ba27f0fd6 --- a/route/CMakeLists.txt +++ b/route/CMakeLists.txt @@ -1,5 +1,5 @@ -# -# Copyright (C) 2018-2021 LEIDOS. + +# Copyright (C) 2018-2022 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 @@ -12,91 +12,82 @@ # 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 2.8.3) +cmake_minimum_required(VERSION 3.5) project(route) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - cav_srvs - carma_utils - autoware_msgs - wgs84_utils_ros1 - carma_wm - tf - tf2 - tf2_geometry_msgs -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) +# Name build targets +set(node_exec route_node_exec) +set(node_lib route_node_lib) +set(generator_worker_lib route_worker_lib) +set(state_worker_lib route_state_lib) -################################### -## catkin specific configuration ## -################################### +# Includes +include_directories( + include +) -catkin_package( - CATKIN_DEPENDS roscpp std_msgs cav_srvs carma_utils autoware_msgs wgs84_utils_ros1 carma_wm tf tf2 tf2_geometry_msgs +# Build +ament_auto_add_library(${generator_worker_lib} + src/route_generator_worker.cpp ) -########### -## Build ## -########### +ament_auto_add_library(${state_worker_lib} + src/route_state_worker.cpp +) -include_directories( - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - include +ament_auto_add_library(${node_lib} SHARED + src/route_node.cpp +) + +ament_auto_add_executable(${node_exec} + src/main.cpp ) +# Register component +rclcpp_components_register_nodes(${node_lib} "route::Route") -add_executable( ${PROJECT_NAME} - src/main.cpp - src/route.cpp - src/route_generator_worker.cpp - src/route_state_worker.cpp) -add_library(route_library src/route_generator_worker.cpp src/route_state_worker.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} route_library) -target_link_libraries(route_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -add_dependencies(route_library ${catkin_EXPORTED_TARGETS}) - -############# -## Install ## -############# - -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_lib} + ${generator_worker_lib} ) -## Install C++ -install(TARGETS ${PROJECT_NAME} route_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_link_libraries(${node_lib} + ${state_worker_lib} ) -## Install Other Resources -install(DIRECTORY launch -config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +target_link_libraries(${node_exec} + ${node_lib} ) -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test - test/test_route_generator.cpp - test/test_route_state.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_route + test/test_route_generator.cpp + test/test_route_state.cpp + ) + + ament_target_dependencies(test_route ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_route ${node_lib}) + +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch ) -target_link_libraries(${PROJECT_NAME}-test route_library ${catkin_LIBRARIES}) diff --git a/route/README.md b/route/README.md new file mode 100644 index 0000000000..4bf5befccf --- /dev/null +++ b/route/README.md @@ -0,0 +1,9 @@ +# route + +The Route package is one of the nodes that is part of the CARMA Platform Guidance system, and it provides the following functionality: + +'Route Generation’: This provides the list of available routes and provides vehicle travel route description and management. It is a standalone ROS node in the CARMA 3 platform. For development and research purpose, it either loads a destination from a file system, or receives destination information from a ROS service call (which is useful when a route is received from infrastructure), then utilizes the Lanelet2 library routing module to generate a route and exposes it to the rest of the vehicle system. It tracks the progress of the current route, and handles re-routing if an active route is invalidated. + +‘Route State Management’: This provides the current state of route following, including tracking and publishing vehicle cross track and down track distances along the active route. + +Link to detailed design document on Confluence: [Click Here](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/1324122258/Detailed+Design+-+Route) \ No newline at end of file diff --git a/route/config/route_config_params.yaml b/route/config/parameters.yaml old mode 100755 new mode 100644 similarity index 74% rename from route/config/route_config_params.yaml rename to route/config/parameters.yaml index 37e253197b..8977340248 --- a/route/config/route_config_params.yaml +++ b/route/config/parameters.yaml @@ -1,10 +1,14 @@ +# Double: Spin rate for the Route node +# Units: Hz +route_spin_rate: 10.0 + # Double: max cross track tolerance error # Units: meter max_crosstrack_error: 2.0 # Double: downtrack_range tracks how far from the end a stop is accepted as a completed route # Units: meter -destination_downtrack_range: 10 +destination_downtrack_range: 10.0 # Int: Maximum number of consecutive timesteps outside of the route allowable before triggering LEFT_ROUTE cte_max_count: 4 diff --git a/route/include/route.h b/route/include/route.h deleted file mode 100755 index 3f1de37357..0000000000 --- a/route/include/route.h +++ /dev/null @@ -1,84 +0,0 @@ -#pragma once - -/* - * 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. - */ - -#include -#include -#include -#include -#include - -#include "route_generator_worker.h" - -namespace route { - - /** - * The route package provides the following functionality: - * - Route generation which provides the list of available routes and provides vehicle travel route description and management. - * - Route state management which provides the current state of the route following, - * including tracking vehicle cross track and down track distances along the active route - */ - class Route - { - - public: - - /** - * \brief Default constructor - */ - Route() = default; - - /** - * \brief General starting point to run this node - */ - void run(); - - private: - - // public and private node handles - std::shared_ptr nh_, pnh_; - - // wm listener and pointer to the actual wm object - carma_wm::WMListener wml_; - carma_wm::WorldModelConstPtr wm_; - - // route generator worker - RouteGeneratorWorker rg_worker_; - - // publishers for route file, current route state and route event - ros::Publisher route_pub_; - ros::Publisher route_state_pub_; - ros::Publisher route_event_pub_; - ros::Publisher route_marker_pub_; - - // subscriber to current pose in the map - ros::Subscriber pose_sub_; - ros::Subscriber twist_sub_; - ros::Subscriber geo_sub_; - - // route service servers - ros::ServiceServer get_available_route_srv_; - ros::ServiceServer set_active_route_srv_; - ros::ServiceServer abort_active_route_srv_; - - // initialize this node before running - void initialize(); - - }; -} - - diff --git a/route/include/route/route_config.hpp b/route/include/route/route_config.hpp new file mode 100644 index 0000000000..19341ea951 --- /dev/null +++ b/route/include/route/route_config.hpp @@ -0,0 +1,50 @@ +#pragma once + +/* + * Copyright (C) 2022 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 + +namespace route +{ + + /** + * \brief Struct containing the algorithm configuration values for the route node + */ + struct Config + { + double max_crosstrack_error = 2.0; // (Meters) Max cross track tolerance error + double destination_downtrack_range = 10.0; // (Meters) Tracks how far a stop must be from the end to be accepted as a completed route + double route_spin_rate = 10.0; // (Hz) Spin rate of the Route node + int cte_max_count = 4; // Max number of consecutive timesteps outside of the route allowable before triggering a LEFT_ROUTE event + std::string route_file_path = "NULL"; // Path to the directory that contains the route file(s) for CARMA + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "route::Config { " << std::endl + << "max_crosstrack_error: " << c.max_crosstrack_error << std::endl + << "destination_downtrack_range: " << c.destination_downtrack_range << std::endl + << "route_spin_rate: " << c.route_spin_rate << std::endl + << "cte_max_count: " << c.cte_max_count << std::endl + << "route_file_path: " << c.route_file_path << std::endl + << "}" << std::endl; + return output; + } + }; + +} // route \ No newline at end of file diff --git a/route/include/route_generator_worker.h b/route/include/route/route_generator_worker.hpp old mode 100755 new mode 100644 similarity index 54% rename from route/include/route_generator_worker.h rename to route/include/route/route_generator_worker.hpp index dfd3a6ee11..52369f1ca8 --- a/route/include/route_generator_worker.h +++ b/route/include/route/route_generator_worker.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2020-2022 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 @@ -16,40 +16,41 @@ * 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 + +#include #include #include -#include -#include -#include +#include #include -#include -#include #include -#include #include #include #include - -#include "route_state_worker.h" +#include "route/route_state_worker.hpp" namespace route { @@ -57,19 +58,20 @@ namespace route { { public: + /** + * \brief Constructor for RouteGeneratorWorker class taking in dependencies via dependency injection + */ + RouteGeneratorWorker(tf2_ros::Buffer& tf2_buffer); + /** * \brief reroutingChecker function to set the rerouting flag locally */ std::function reroutingChecker; + /** * \brief setReroutingChecker function to set the rerouting flag */ - void setReroutingChecker(std::function inputFunction); - - /** - * \brief Constructor for RouteGeneratorWorker class taking in dependencies via dependency injection - */ - RouteGeneratorWorker() = default; + void setReroutingChecker(const std::function inputFunction); /** * \brief Dependency injection for world model pointer. @@ -77,6 +79,18 @@ namespace route { */ void setWorldModelPtr(carma_wm::WorldModelConstPtr wm); + /** + * \brief Dependency injection for logger interface. + * \param logger Logger interface that will be used by worker class + */ + void setLoggerInterface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); + + /** + * \brief Dependency injection for clock object. + * \param clock Clock object that will be used by worker class + */ + void setClock(rclcpp::Clock::SharedPtr clock); + /** * \brief Generate Lanelet2 route based on input destinations * \param start Lanelet 2D point in map frame indicates the starting point of selected route @@ -92,124 +106,132 @@ namespace route { const carma_wm::LaneletRoutingGraphConstPtr graph_pointer) const; /** - * \brief Get_available_route service callback. Call this service will response with a list of route names for user to select - * \param req A empty cav_srvs::GetAvailableRoutesRequest - * \param resp A cav_srvs::GetAvailableRoutesResponse msg contains a list of empty Route messages with only route name populated + * \brief Get_available_route service callback. Calls to this service will respond with a list of route names for user to select + * \param req An empty carma_planning_msgs::srv::GetAvailableRoutes::Request + * \param resp A carma_planning_msgs::srv::GetAvailableRoutes::Response msg contains a list of empty Route messages with only route name populated */ - bool get_available_route_cb(cav_srvs::GetAvailableRoutesRequest &req, cav_srvs::GetAvailableRoutesResponse &resp); - + bool getAvailableRouteCb(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr resp); + /** * \brief Set_active_route service callback. User can select a route to start following by calling this service - * \param req A cav_srvs::SetActiveRouteRequest msg which contains either a route name a user wants to select or - * an array of cav_msgs/Position3D destination points to generate a route from. - * \param resp A cav_srvs::SetActiveRouteResponse msg contains error status showing if the routing succeeded + * \param req A carma_planning_msgs::srv::SetActiveRoute::Request msg which contains either a route name a user wants to select or + * an array of carma_v2x_msgs::msg::Position3D destination points to generate a route from. + * \param resp A carma_planning_msgs::srv::SetActiveRoute::Response msg contains error status indicating whether the routing succeeded */ - bool set_active_route_cb(cav_srvs::SetActiveRouteRequest &req, cav_srvs::SetActiveRouteResponse &resp); + bool setActiveRouteCb(const std::shared_ptr, + const std::shared_ptr req, + std::shared_ptr resp); /** - * \brief Abort_active_route service callback. User can call this service to abort a current following route and back to route selection stage - * \param req A cav_srvs::AbortActiveRouteRequest msg which contains the route name user wants to stop following - * \param resp A cav_srvs::AbortActiveRouteResponse msg contains error status showing if there is an active route + * \brief Abort_active_route service callback. User can call this service to abort a current following route and return to route selection stage + * \param req A carma_planning_msgs::srv::AbortActiveRoute::Request msg which contains the route name user wants to stop following + * \param resp A carma_planning_msgs::srv::AbortActiveRoute::Response msg contains error status showing if there is an active route */ - bool abort_active_route_cb(cav_srvs::AbortActiveRouteRequest &req, cav_srvs::AbortActiveRouteResponse &resp); + bool abortActiveRouteCb(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr resp); /** * \brief Callback for the front bumper pose transform */ - void bumper_pose_cb(); + void bumperPoseCb(); /** * \brief Callback for the twist subscriber, which will store latest twist locally * \param msg Latest twist message */ - void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); + void twistCb(geometry_msgs::msg::TwistStamped::UniquePtr msg); /** * \brief Callback for the georeference subscriber used to set the map projection * \param msg The latest georeference */ - void georeference_cb(const std_msgs::StringConstPtr& msg); + void georeferenceCb(std_msgs::msg::String::UniquePtr msg); /** * \brief Set method for configurable parameter * \param path The location of route files */ - void set_route_file_path(const std::string& path); + void setRouteFilePath(const std::string& path); /** * \brief Set method for configurable parameter - * \param ct_max_error Maximum cross track error which can trigger left route event * \param dt_dest_range Minimum down track error which can trigger route complete event */ - void set_ctdt_param(double ct_max_error, double dt_dest_range); + void setDowntrackDestinationRange(double dt_dest_range); /** * \brief Method to pass publishers into worker class * \param route_event_pub Route event publisher * \param route_state_pub Route state publisher * \param route_pub Route publisher - * \param route_marker_pub publisher + * \param route_marker_pub Route marker publisher */ - void set_publishers(ros::Publisher route_event_pub, ros::Publisher route_state_pub, ros::Publisher route_pub,ros::Publisher route_marker_pub); - + void setPublishers(const carma_ros2_utils::PubPtr& route_event_pub, + const carma_ros2_utils::PubPtr& route_state_pub, + const carma_ros2_utils::PubPtr& route_pub, + const carma_ros2_utils::PubPtr& route_marker_pub); + /** * \brief Helper function to check whether a route's shortest path contains any duplicate Lanelet IDs. * 'true' indicates that the route's shortest path contains duplicate Lanelet IDs. * \param route Route object from lanelet2 lib routing function */ - bool check_for_duplicate_lanelets_in_shortest_path(const lanelet::routing::Route& route) const; + bool checkForDuplicateLaneletsInShortestPath(const lanelet::routing::Route& route) const; /** * \brief Function to take route destination points from a vector of 3D points and convert them from lat/lon values to to coordinates in map frame based on the projection string - * \param destinations A vector of cav_msgs::Position3D points containing destination points provided as lat/long values + * \param destinations A vector of carma_v2x_msgs::msg::Position3D points containing destination points provided as lat/long values */ - std::vector load_route_destinations_in_map_frame(const std::vector& destinations) const; + std::vector loadRouteDestinationsInMapFrame(const std::vector& destinations) const; /** * \brief Function to load route destination points from a route file and store them in a vector of 3D points * \param route_id This function will read the route file with provided route_id */ - std::vector load_route_destination_gps_points_from_route_id(const std::string& route_id) const; + std::vector loadRouteDestinationGpsPointsFromRouteId(const std::string& route_id) const; /** * \brief Helper function to generate a CARMA route message based on planned lanelet route * \param route Route object from lanelet2 lib routing function */ - cav_msgs::Route compose_route_msg(const lanelet::Optional& route); + carma_planning_msgs::msg::Route composeRouteMsg(const lanelet::Optional& route) const; /** * \brief Spin callback which will be called frequently based on spin rate */ - bool spin_callback(); + bool spinCallback(); + /** - * \brief compose_route_marker_msg is a function to generate route rviz markers + * \brief composeRouteMarkerMsg is a function to generate route rviz markers * \param route Route object from lanelet2 lib routing function */ - visualization_msgs::Marker compose_route_marker_msg(const lanelet::Optional& route); + visualization_msgs::msg::Marker composeRouteMarkerMsg(const lanelet::Optional& route); /** - * \brief crosstrack_error_check is a function that determines when the vehicle has left the route and reports when a crosstrack error has + * \brief crosstrackErrorCheck is a function that determines when the vehicle has left the route and reports when a crosstrack error has * taken place * * \param msg Msg that contains the vehicle's current position * \param current_llt The lanelet that the vehicle is currently in - * \param llt_track The crosstrack and downtrack distance of the current lanelet * */ - bool crosstrack_error_check(const geometry_msgs::PoseStampedConstPtr& msg, lanelet::ConstLanelet current_llt); + bool crosstrackErrorCheck(const std::shared_ptr& msg, lanelet::ConstLanelet current_llt); /** * \brief set the crosstrack error counter maximum limit * * \param cte_max the maximum amount of acceptable crosstrack error instances */ - void set_CTE_count_max(int cte_max); + void setCrosstrackErrorCountMax(int cte_max); /** * \brief set the maximum crosstrack error distance * - * \param cte_dist maximum distance value (specified in the route_config_params.yaml file) + * \param cte_dist maximum distance value (specified in the this package's parameters.yaml configuration file) */ - void set_CTE_dist(double cte_dist); + void setCrosstrackErrorDistance(double cte_dist); /** * \brief "Get the closest lanelet on the route relative to the vehicle's current position. @@ -217,17 +239,17 @@ namespace route { * * \param position the current position of the vehicle */ - lanelet::ConstLanelet get_closest_lanelet_from_route_llts(lanelet::BasicPoint2d position); + lanelet::ConstLanelet getClosestLaneletFromRouteLanelets(lanelet::BasicPoint2d position) const; - //Added for Unit Testing - void addllt(lanelet::ConstLanelet llt); + // Added for Unit Testing + void addLanelet(lanelet::ConstLanelet llt); /** * \brief After route is invalidated, this function returns a new route based on the destinations points. * \param destination_points_in_map vector of destination points * \note Destination points will be removed if the current pose is past those points. */ - lanelet::Optional reroute_after_route_invalidation(std::vector& destination_points_in_map); + lanelet::Optional rerouteAfterRouteInvalidation(const std::vector& destination_points_in_map); /** * \brief Initialize transform lookup from front bumper to map @@ -235,7 +257,7 @@ namespace route { void initializeBumperTransformLookup(); // Current vehicle pose if it has been recieved - boost::optional vehicle_pose_; + boost::optional vehicle_pose_; private: @@ -251,29 +273,34 @@ namespace route { carma_wm::WorldModelConstPtr world_model_; // route messages waiting to be updated and published - cav_msgs::Route route_msg_; - cav_msgs::RouteEvent route_event_msg_; - cav_msgs::RouteState route_state_msg_; - visualization_msgs::Marker route_marker_msg_; + carma_planning_msgs::msg::Route route_msg_; + carma_planning_msgs::msg::RouteEvent route_event_msg_; + carma_planning_msgs::msg::RouteState route_state_msg_; + visualization_msgs::msg::Marker route_marker_msg_; + std::vector points_; //List of lanelets in the route lanelet::ConstLanelets route_llts; - - // maximum cross track error which can trigger left route event - double cross_track_max_; // minimum down track error which can trigger route complete event double down_track_target_range_; - // current cross track and down track distance relative to the route - double current_crosstrack_distance_, current_downtrack_distance_; + // current cross track distance + double current_crosstrack_distance_; + + // current down track distance relevant to the route + double current_downtrack_distance_; // current pose lanelet::BasicPoint2d current_loc_; - // current lanelet down track and cross track distance - double ll_crosstrack_distance_, ll_downtrack_distance_; + // current lanelet cross track distance + double ll_crosstrack_distance_; + + // current lanelet down track + double ll_downtrack_distance_; + lanelet::Id ll_id_; // current speed limit on current lanelet @@ -282,8 +309,12 @@ namespace route { double current_speed_ = 0; //A small static value for comparing doubles static constexpr double epsilon_ = 0.001; + // local copy of Route publihsers - ros::Publisher route_event_pub_, route_state_pub_, route_pub_,route_marker_pub_; + carma_ros2_utils::PubPtr route_event_pub_; + carma_ros2_utils::PubPtr route_state_pub_; + carma_ros2_utils::PubPtr route_pub_; + carma_ros2_utils::PubPtr route_marker_pub_; // a bool flag indicates a new route has been generated such that a local copy of Route message should be published again bool new_route_msg_generated_ = false; @@ -293,9 +324,10 @@ namespace route { std::queue route_event_queue; // private helper function to add a new route event into event queue - void publish_route_event(uint8_t event_type); + void publishRouteEvent(uint8_t event_type); - double cross_track_dist; + // maximum cross track error which can trigger left route event + double cross_track_dist_; // counter to record how many times vehicle's position exceeds crosstrack distance int cte_count_ = 0; @@ -305,20 +337,20 @@ namespace route { // destination points in map std::vector destination_points_in_map_; - - // Vehicle front bumper pose - geometry_msgs::PoseStampedPtr bumper_pose_; - // The current map projection for lat/lon to map frame conversion boost::optional map_proj_; - geometry_msgs::TransformStamped tf_; + geometry_msgs::msg::TransformStamped tf_; tf2::Stamped frontbumper_transform_; + // TF listenser - tf2_ros::Buffer tf2_buffer_; + tf2_ros::Buffer& tf2_buffer_; std::unique_ptr tf2_listener_; - }; + // Logger interface + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; -} + rclcpp::Clock::SharedPtr clock_; + }; +} // namespace route \ No newline at end of file diff --git a/route/include/route/route_node.hpp b/route/include/route/route_node.hpp new file mode 100644 index 0000000000..394a10fbd6 --- /dev/null +++ b/route/include/route/route_node.hpp @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2022 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 +#include +#include + +#include +#include "route/route_config.hpp" +#include "route/route_generator_worker.hpp" + +namespace route +{ + + /** + * \class Route + * \brief The route package provides the following functionality: + * - Route generation which provides the list of available routes and provides vehicle travel route + description and management. + * - Route state management which provides the current state of the route following, + * including tracking vehicle cross track and down track distances along the active route + */ + class Route : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr twist_sub_; + carma_ros2_utils::SubPtr geo_sub_; + + // Publishers + carma_ros2_utils::PubPtr route_pub_; + carma_ros2_utils::PubPtr route_state_pub_; + carma_ros2_utils::PubPtr route_event_pub_; + carma_ros2_utils::PubPtr route_marker_pub_; + + // Service Servers + carma_ros2_utils::ServicePtr get_available_route_srv_; + carma_ros2_utils::ServicePtr set_active_route_srv_; + carma_ros2_utils::ServicePtr abort_active_route_srv_; + + // Timers + rclcpp::TimerBase::SharedPtr spin_timer_; + + // Node configuration + Config config_; + + // tf2 buffer holds the tree of transforms + tf2_ros::Buffer tf2_buffer_; + + // wm listener and pointer to the actual wm object + carma_wm::WMListener wml_; + carma_wm::WorldModelConstPtr wm_; + + // route generator worker + RouteGeneratorWorker rg_worker_; + + public: + /** + * \brief Route constructor + */ + explicit Route(const rclcpp::NodeOptions &); + + /** + * \brief Function callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + }; + +} // route diff --git a/route/include/route_state_worker.h b/route/include/route/route_state_worker.hpp old mode 100755 new mode 100644 similarity index 88% rename from route/include/route_state_worker.h rename to route/include/route/route_state_worker.hpp index c3a675c097..724ff34ab6 --- a/route/include/route_state_worker.h +++ b/route/include/route/route_state_worker.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2020-2021 LEIDOS. + * Copyright (C) 2020-2022 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 @@ -16,6 +16,7 @@ * the License. */ +#include #include namespace route { @@ -66,18 +67,22 @@ namespace route { * \brief Process route event based on designed state machine diagram * \param event Incoming route event */ - void on_route_event(RouteEvent event); + void onRouteEvent(RouteEvent event); /** * \brief Get current route state machine state */ - RouteState get_route_state() const; + RouteState getRouteState() const; + + void setLoggerInterface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); private: // private local variable tracks the current route satte RouteState state_ = RouteState::LOADING; + // Logger interface + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; }; } \ No newline at end of file diff --git a/route/launch/route_launch.py b/route/launch/route_launch.py new file mode 100644 index 0000000000..ad4d214797 --- /dev/null +++ b/route/launch/route_launch.py @@ -0,0 +1,68 @@ +# Copyright (C) 2022 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA route_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('route'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='route_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='route', + plugin='route::Route', + name='route_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/route/package.xml b/route/package.xml old mode 100755 new mode 100644 index 471af6bf23..16741e707e --- a/route/package.xml +++ b/route/package.xml @@ -1,14 +1,11 @@ - - - - - - diff --git a/traffic_incident_parser/launch/traffic_incident_parser_launch.py b/traffic_incident_parser/launch/traffic_incident_parser_launch.py new file mode 100644 index 0000000000..5732ec4eda --- /dev/null +++ b/traffic_incident_parser/launch/traffic_incident_parser_launch.py @@ -0,0 +1,62 @@ +# Copyright (C) 2022 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA traffic_incident_parser_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='traffic_incident_parser_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='traffic_incident_parser', + plugin='traffic_incident_parser::TrafficIncidentParserNode', + name='traffic_incident_parser_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/traffic_incident_parser/package.xml b/traffic_incident_parser/package.xml index 966aaa6450..52be9b5af2 100644 --- a/traffic_incident_parser/package.xml +++ b/traffic_incident_parser/package.xml @@ -1,7 +1,7 @@ - - - - - - - - \ No newline at end of file diff --git a/trajectory_executor/launch/trajectory_executor_launch.py b/trajectory_executor/launch/trajectory_executor_launch.py new file mode 100644 index 0000000000..6cf3d32c95 --- /dev/null +++ b/trajectory_executor/launch/trajectory_executor_launch.py @@ -0,0 +1,68 @@ +# Copyright (C) 2022 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA trajectory_executor_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='DEBUG') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('trajectory_executor'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='trajectory_executor_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='trajectory_executor', + plugin='trajectory_executor::TrajectoryExecutor', + name='trajectory_executor_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/trajectory_executor/package.xml b/trajectory_executor/package.xml index 9f153bcaa9..d63d92ad0a 100644 --- a/trajectory_executor/package.xml +++ b/trajectory_executor/package.xml @@ -1,59 +1,43 @@ - + trajectory_executor - 3.3.0 - CARMA Trajectory Executor pacakge - - - - - rushk - + 4.0.0 + The trajectory_executor package - - - - Apache 2.0 License + carma + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake - - - - - + rclcpp + carma_ros2_utils + rclcpp_components + carma_planning_msgs - - - - + ament_lint_auto + ament_cmake_gtest - catkin - cav_msgs - cav_srvs - roscpp - std_msgs - carma_utils - carma_cmake_common + launch + launch_ros - - - - + ament_cmake diff --git a/traffic_incident_parser/test/main.cpp b/trajectory_executor/src/main.cpp similarity index 55% rename from traffic_incident_parser/test/main.cpp rename to trajectory_executor/src/main.cpp index d07eafdc26..c346bb6b00 100644 --- a/traffic_incident_parser/test/main.cpp +++ b/trajectory_executor/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 LEIDOS. + * Copyright (C) 2022 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 @@ -14,10 +14,20 @@ * the License. */ -#include +#include +#include "trajectory_executor/trajectory_executor_node.hpp" -// Run all the tests -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + + rclcpp::shutdown(); + + return 0; } diff --git a/trajectory_executor/src/test/test_utils.h b/trajectory_executor/src/test/test_utils.h deleted file mode 100644 index bf75cafb45..0000000000 --- a/trajectory_executor/src/test/test_utils.h +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Copyright (C) 2018-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 - - -/** - * Test fixture for TrajectoryExecutor testing - * Maintains publishers, subscribers, and message tracking for all tests. - * State is reset between tests to ensure clean results. - */ -class TrajectoryExecutorTestSuite : public ::testing::Test -{ - public: - TrajectoryExecutorTestSuite() { - _nh = ros::NodeHandle(); - traj_pub = _nh.advertise("trajectory", 5); - traj_sub = _nh.subscribe("/guidance/pure_pursuit/plan_jerk_trajectory", 100, - &TrajectoryExecutorTestSuite::trajEmitCallback, this); - traj_sub2 = _nh.subscribe("/guidance/PlatooningControlPlugin/plan_trajectory", 100, - &TrajectoryExecutorTestSuite::trajEmitCallback, this); - sys_alert_sub = _nh.subscribe("system_alert", 100, - &TrajectoryExecutorTestSuite::sysAlertCallback, this); - } - ros::NodeHandle _nh; - ros::Publisher traj_pub; - ros::Subscriber traj_sub; - ros::Subscriber traj_sub2; - ros::Subscriber sys_alert_sub; - int msg_count = 0; - int last_points = 9999; - bool shrinking = true; - bool recv_sys_alert = false; - - void trajEmitCallback(cav_msgs::TrajectoryPlan msg) { - msg_count++; - if (msg.trajectory_points.size() <= last_points && shrinking) { - last_points = msg.trajectory_points.size(); - } else { - shrinking = false; - } - } - - void sysAlertCallback(cav_msgs::SystemAlert msg) { - recv_sys_alert = true; - } -}; - -/*! - * \brief Waits for the specified number of subscribers to exist on the topic - * before continuing. Will wait until num_subscribers exist or until timeout_millis - * has elapsed. - * - * \param pub The publisher of the topic you are waiting for - * \param num_subscribers The target number of subscribers to acheive - * \param timeout_millis The maximum number of milliseconds to wait - */ -void waitForSubscribers(ros::Publisher pub, int num_subscribers, int timeout_millis) { - int elapsed_millis = 0; - while (pub.getNumSubscribers() < num_subscribers && elapsed_millis < timeout_millis) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - elapsed_millis += 100; - } - - if (elapsed_millis > timeout_millis && pub.getNumSubscribers() < num_subscribers) { - ROS_WARN_STREAM("Target number of subscribers for " << pub.getTopic() - << " not reached due to timeout! (" << pub.getNumSubscribers() - << "/" << num_subscribers << " subscribers)"); - } -} - -/*! - * \brief Build a small sample TrajectoryPlan message - * - * \return A 10-point cav_msgs::TrajectoryPlan message containing sample data - */ -cav_msgs::TrajectoryPlan buildSampleTraj() { - cav_msgs::TrajectoryPlan plan; - - plan.header.stamp = ros::Time::now(); - plan.trajectory_id = "TEST TRAJECTORY 1"; - - ros::Time cur_time = ros::Time::now(); - for (int i = 0; i < 10; i++) { - cav_msgs::TrajectoryPlanPoint p; - p.controller_plugin_name = "Pure Pursuit Jerk"; - p.lane_id = "0"; - p.planner_plugin_name = "cruising"; - ros::Duration dur(i * 0.13); - p.target_time = cur_time + dur; - p.x = 10 * i; - p.y = 10 * i; - plan.trajectory_points.push_back(p); - } - - return plan; -} - -cav_msgs::TrajectoryPlan buildSampleTraj2() { - cav_msgs::TrajectoryPlan plan; - - plan.header.stamp = ros::Time::now(); - plan.trajectory_id = "TEST TRAJECTORY 2"; - - ros::Time cur_time = ros::Time::now(); - for (int i = 0; i < 10; i++) { - cav_msgs::TrajectoryPlanPoint p; - p.controller_plugin_name = "PlatooningControlPlugin"; - p.lane_id = "0"; - p.planner_plugin_name = "cruising"; - ros::Duration dur(i * 0.13); - p.target_time = cur_time + dur; - p.x = 10 * i; - p.y = 10 * i; - plan.trajectory_points.push_back(p); - } - - return plan; -} diff --git a/trajectory_executor/src/test/trajectory_executor_test_1.cpp b/trajectory_executor/src/test/trajectory_executor_test_1.cpp deleted file mode 100644 index 91ac69467e..0000000000 --- a/trajectory_executor/src/test/trajectory_executor_test_1.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright (C) 2018-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 "test_utils.h" - -/*! - * \brief Test that the trajectory executor can receive an input plan and not immediately crash - */ -TEST_F(TrajectoryExecutorTestSuite, test_message_no_crash) { - waitForSubscribers(traj_pub, 1, 500); - cav_msgs::TrajectoryPlan plan = buildSampleTraj(); - - traj_pub.publish(plan); - - ASSERT_FALSE(recv_sys_alert) << "Received system shutdown alert message from TrajectoryExecutor node."; -} - -/*! - * \brief Test that the trajectory executor will output the trajectory on - * the desired topic after receiving it on the input - */ -TEST_F(TrajectoryExecutorTestSuite, test_emit_traj) { - waitForSubscribers(traj_pub, 1, 500); - cav_msgs::TrajectoryPlan plan = buildSampleTraj2(); - - traj_pub.publish(plan); - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - ASSERT_GT(msg_count, 0) << "Failed to receive trajectory execution message from TrajectoryExecutor node."; -} - -/*! - * \brief Test that the trajectory executor will shutdown properly after running - * over the end of it's current trajectory - */ -TEST_F(TrajectoryExecutorTestSuite, test_runover_shutdown) { - waitForSubscribers(traj_pub, 1, 500); - cav_msgs::TrajectoryPlan plan = buildSampleTraj(); - - traj_pub.publish(plan); - - std::this_thread::sleep_for(std::chrono::milliseconds(1500)); - ASSERT_TRUE(recv_sys_alert) << "Failed to receive system shutdown alert message from TrajectoryExecutor node."; -} - -/*! - * \brief Main entrypoint for unit tests - */ -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "trajectory_executor_test_1"); - - std::thread spinner([] {while (ros::ok()) ros::spin();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} diff --git a/trajectory_executor/src/test/trajectory_executor_test_2.cpp b/trajectory_executor/src/test/trajectory_executor_test_2.cpp deleted file mode 100644 index a2cad2c428..0000000000 --- a/trajectory_executor/src/test/trajectory_executor_test_2.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2018-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 "test_utils.h" - -/*! - * \brief Test that the Trajectory executor properly emits all sub-trajectories as it - * iterates through a single trajectory - */ -TEST_F(TrajectoryExecutorTestSuite, test_emit_multiple) { - waitForSubscribers(traj_pub, 1, 500); - cav_msgs::TrajectoryPlan plan = buildSampleTraj(); - - traj_pub.publish(plan); - - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - - ASSERT_LE(10, msg_count) << "Failed to receive whole trajectory from TrajectoryExecutor node."; - ASSERT_TRUE(shrinking) << "Output trajectory plans were not shrunk each time step as expected."; -} - -/*! - * \brief Main entrypoint for unit tests - */ -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "trajectory_executor_test_2"); - - std::thread spinner([] {while (ros::ok()) ros::spinOnce();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} diff --git a/trajectory_executor/src/test/trajectory_executor_test_3.cpp b/trajectory_executor/src/test/trajectory_executor_test_3.cpp deleted file mode 100644 index b41192d530..0000000000 --- a/trajectory_executor/src/test/trajectory_executor_test_3.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2018-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 "test_utils.h" - -/*! - * \brief Test that the Trajectory Executor properly shuts down if it encounters a trajectory - * containing a reference to a control plugin which was not discovered. - */ -TEST_F(TrajectoryExecutorTestSuite, test_control_plugin_not_found) { - waitForSubscribers(traj_pub, 1, 500); - cav_msgs::TrajectoryPlan plan = buildSampleTraj(); - - plan.trajectory_points[3].controller_plugin_name = "NULL"; - traj_pub.publish(plan); - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - ASSERT_TRUE(recv_sys_alert) << "Failed to receive system shutdown alert message from TrajectoryExecutor node."; -} - -/*! - * \brief Main entrypoint for unit tests - */ -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "trajectory_executor_test_3"); - - std::thread spinner([] {while (ros::ok()) ros::spin();}); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - return res; -} diff --git a/trajectory_executor/src/trajectory_executor/trajectory_executor.cpp b/trajectory_executor/src/trajectory_executor/trajectory_executor.cpp deleted file mode 100644 index 126af84a25..0000000000 --- a/trajectory_executor/src/trajectory_executor/trajectory_executor.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright (C) 2018-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 "trajectory_executor/trajectory_executor.hpp" -#include -#include -#include -#include - -namespace trajectory_executor -{ - - TrajectoryExecutor::TrajectoryExecutor(int traj_frequency) : - _timesteps_since_last_traj(0), - _min_traj_publish_tickrate_hz(traj_frequency) { } - - TrajectoryExecutor::TrajectoryExecutor() : - _timesteps_since_last_traj(0), - _min_traj_publish_tickrate_hz(10) { } - - std::map TrajectoryExecutor::queryControlPlugins() - { - // Hard coded stub for MVP since plugin manager won't be developed yet - // TODO: Query plugin manager to receive actual list of plugins and their corresponding topics - ROS_DEBUG("Executing stub behavior for plugin discovery MVP..."); - std::map out; - - _private_nh->param("default_control_plugin", default_control_plugin_, "NULL"); - _private_nh->param("default_control_plugin_topic", default_control_plugin_topic_, "NULL"); - - out[default_control_plugin_] = default_control_plugin_topic_; - - //Hardcoding platoon control plugins - - std::string control_plugin2 = "PlatooningControlPlugin"; - std::string control_plugin_topic2 = "/guidance/PlatooningControlPlugin/plan_trajectory"; - out[control_plugin2]=control_plugin_topic2; - - return out; - } - - void TrajectoryExecutor::onNewTrajectoryPlan(const cav_msgs::TrajectoryPlan& msg) - { - std::unique_lock lock(_cur_traj_mutex); // Acquire lock until end of this function scope - ROS_DEBUG("Received new trajectory plan!"); - ROS_DEBUG_STREAM("New Trajectory plan ID: " << msg.trajectory_id); - ROS_DEBUG_STREAM("New plan contains " << msg.trajectory_points.size() << " points"); - - _cur_traj = std::unique_ptr(new cav_msgs::TrajectoryPlan(msg)); - _timesteps_since_last_traj = 0; - ROS_DEBUG_STREAM("Successfully swapped trajectories!"); - } - - void TrajectoryExecutor::guidanceStateMonitor(const cav_msgs::GuidanceStateConstPtr& msg) - { - std::unique_lock lock(_cur_traj_mutex); // Acquire lock until end of this function scope - // TODO need to handle control handover once alernative planner system is finished - if(msg->state != cav_msgs::GuidanceState::ENGAGED) - { - _cur_traj= nullptr; - } - } - - void TrajectoryExecutor::onTrajEmitTick(const ros::TimerEvent& te) - { - std::unique_lock lock(_cur_traj_mutex); - ROS_DEBUG("TrajectoryExecutor tick start!"); - - if (_cur_traj != nullptr) { - if (!_cur_traj->trajectory_points.empty()) { - // Determine the relevant control plugin for the current timestep - std::string control_plugin = _cur_traj->trajectory_points[0].controller_plugin_name; - // if it instructed to use default control_plugin - if (control_plugin == "default" || control_plugin =="") - control_plugin = default_control_plugin_; - - std::map::iterator it = _traj_publisher_map.find(control_plugin); - if (it != _traj_publisher_map.end()) { - ROS_DEBUG("Found match for control plugin %s at point %d in current trajectory!", - control_plugin.c_str(), - _timesteps_since_last_traj); - it->second.publish(*_cur_traj); - } else { - std::ostringstream description_builder; - description_builder << "No match found for control plugin " - << control_plugin << " at point " - << _timesteps_since_last_traj << " in current trajectory!"; - - throw std::invalid_argument(description_builder.str()); - } - _timesteps_since_last_traj++; - } else { - throw std::out_of_range("Ran out of trajectory data to consume!"); - } - } else { - ROS_DEBUG("Awaiting initial trajectory publication..."); - } - ROS_DEBUG("TrajectoryExecutor tick completed succesfully!"); - } - - void TrajectoryExecutor::run() - { - ROS_DEBUG("Starting operations for TrajectoryExecutor component..."); - _timer = _private_nh->createTimer( - ros::Duration(ros::Rate(this->_min_traj_publish_tickrate_hz)), - &TrajectoryExecutor::onTrajEmitTick, - this); - - ROS_DEBUG("TrajectoryExecutor component started succesfully! Starting to spin."); - - ros::CARMANodeHandle::spin(); - } - - bool TrajectoryExecutor::init() - { - ROS_DEBUG("Initializing TrajectoryExecutor node..."); - - _public_nh = std::unique_ptr(new ros::CARMANodeHandle()); - _private_nh = std::unique_ptr(new ros::CARMANodeHandle("~")); - ROS_DEBUG("Initialized all node handles"); - - _private_nh->param("trajectory_publish_rate", _min_traj_publish_tickrate_hz, 10); - - ROS_DEBUG_STREAM("Initalized params with trajectory_publish_rate " << _min_traj_publish_tickrate_hz); - - this->_plan_sub = this->_public_nh->subscribe("trajectory", 5, &TrajectoryExecutor::onNewTrajectoryPlan, this); - this->_state_sub = this->_public_nh->subscribe("state", 5, &TrajectoryExecutor::guidanceStateMonitor, this); - - this->_cur_traj = std::unique_ptr(); - ROS_DEBUG("Subscribed to inbound trajectory plans."); - - ROS_DEBUG("Setting up publishers for control plugin topics..."); - - std::map control_plugin_topics; - auto discovered_control_plugins = queryControlPlugins(); - - for (auto it = discovered_control_plugins.begin(); it != discovered_control_plugins.end(); it++) - { - ROS_DEBUG("Trajectory executor discovered control plugin %s listening on topic %s.", it->first.c_str(), it->second.c_str()); - ros::Publisher control_plugin_pub = _public_nh->advertise(it->second, 1000); - control_plugin_topics.insert(std::make_pair(it->first, control_plugin_pub)); - } - - this->_traj_publisher_map = control_plugin_topics; - ROS_DEBUG("TrajectoryExecutor component initialized succesfully!"); - - return true; - } -} diff --git a/trajectory_executor/src/trajectory_executor_node.cpp b/trajectory_executor/src/trajectory_executor_node.cpp new file mode 100644 index 0000000000..efe142a051 --- /dev/null +++ b/trajectory_executor/src/trajectory_executor_node.cpp @@ -0,0 +1,179 @@ +/* + * Copyright (C) 2022 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 "trajectory_executor/trajectory_executor_node.hpp" + +namespace trajectory_executor +{ + namespace std_ph = std::placeholders; + + TrajectoryExecutor::TrajectoryExecutor(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.trajectory_publish_rate = declare_parameter("trajectory_publish_rate", config_.trajectory_publish_rate); + config_.default_control_plugin = declare_parameter("default_control_plugin", config_.default_control_plugin); + config_.default_control_plugin_topic = declare_parameter("default_control_plugin_topic", config_.default_control_plugin_topic); + } + + rcl_interfaces::msg::SetParametersResult TrajectoryExecutor::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params({{"trajectory_publish_rate", config_.trajectory_publish_rate}}, parameters); + auto error_2 = update_params({ + {"default_control_plugin", config_.default_control_plugin}, + {"default_control_plugin_topic", config_.default_control_plugin_topic}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error_2; + + return result; + } + + std::map TrajectoryExecutor::queryControlPlugins() + { + // Hard coded stub for MVP since plugin manager won't be developed yet + // TODO: Query plugin manager to receive actual list of plugins and their corresponding topics + RCLCPP_DEBUG_STREAM(get_logger(), "Executing stub behavior for plugin discovery MVP..."); + std::map out; + + out[config_.default_control_plugin] = config_.default_control_plugin_topic; + + //Hardcoding platoon control plugins + std::string control_plugin2 = "PlatooningControlPlugin"; + std::string control_plugin_topic2 = "/guidance/PlatooningControlPlugin/plan_trajectory"; + out[control_plugin2] = control_plugin_topic2; + + return out; + } + + carma_ros2_utils::CallbackReturn TrajectoryExecutor::handle_on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO_STREAM(get_logger(), "TrajectoryExecutor trying to configure"); + + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("trajectory_publish_rate", config_.trajectory_publish_rate); + get_parameter("default_control_plugin", config_.default_control_plugin); + get_parameter("default_control_plugin_topic", config_.default_control_plugin_topic); + + RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&TrajectoryExecutor::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + plan_sub_ = create_subscription("trajectory", 5, + std::bind(&TrajectoryExecutor::onNewTrajectoryPlan, this, std_ph::_1)); + state_sub_ = create_subscription("state", 5, + std::bind(&TrajectoryExecutor::guidanceStateMonitor, this, std_ph::_1)); + + cur_traj_ = std::unique_ptr(); + + RCLCPP_DEBUG_STREAM(get_logger(), "Setting up publishers for control plugin topics..."); + + std::map> control_plugin_topics; + auto discovered_control_plugins = queryControlPlugins(); + + for (auto it = discovered_control_plugins.begin(); it != discovered_control_plugins.end(); it++) + { + RCLCPP_DEBUG_STREAM(get_logger(), "Trajectory executor discovered control plugin " << it->first.c_str() << " listening on topic " << it->second.c_str()); + carma_ros2_utils::PubPtr control_plugin_pub = create_publisher(it->second, 1000); + control_plugin_topics.insert(std::make_pair(it->first, control_plugin_pub)); + } + + traj_publisher_map_ = control_plugin_topics; + RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor component initialized succesfully!"); + + // Return success if everthing initialized successfully + return CallbackReturn::SUCCESS; + } + carma_ros2_utils::CallbackReturn TrajectoryExecutor::handle_on_activate(const rclcpp_lifecycle::State &) + { + // Create timer for publishing outbound trajectories to the control plugins + int timer_period_ms = (1 / config_.trajectory_publish_rate) * 1000; // Conversion from frequency (Hz) to milliseconds time period + + timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(timer_period_ms), + std::bind(&TrajectoryExecutor::onTrajEmitTick, this)); + + return CallbackReturn::SUCCESS; + } + + void TrajectoryExecutor::onTrajEmitTick() + { + RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor tick start!"); + + if (cur_traj_ != nullptr) { + // Determine the relevant control plugin for the current timestep + std::string control_plugin = cur_traj_->trajectory_points[0].controller_plugin_name; + // if it instructed to use default control_plugin + if (control_plugin == "default" || control_plugin =="") { + control_plugin = config_.default_control_plugin; + } + + std::map>::iterator it = traj_publisher_map_.find(control_plugin); + if (it != traj_publisher_map_.end()) { + RCLCPP_DEBUG_STREAM(get_logger(), "Found match for control plugin " << control_plugin.c_str() << " at point " << timesteps_since_last_traj_ << " in current trajectory!"); + it->second->publish(*cur_traj_); + } else { + std::ostringstream description_builder; + description_builder << "No match found for control plugin " + << control_plugin << " at point " + << timesteps_since_last_traj_ << " in current trajectory!"; + + throw std::invalid_argument(description_builder.str()); + } + timesteps_since_last_traj_++; + } else { + RCLCPP_DEBUG_STREAM(get_logger(), "Awaiting initial trajectory publication..."); + } + + RCLCPP_DEBUG_STREAM(get_logger(), "TrajectoryExecutor tick completed succesfully!"); + + } + + void TrajectoryExecutor::onNewTrajectoryPlan(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) + { + RCLCPP_DEBUG_STREAM(get_logger(), "Received new trajectory plan!"); + RCLCPP_DEBUG_STREAM(get_logger(), "New Trajectory plan ID: " << msg->trajectory_id); + RCLCPP_DEBUG_STREAM(get_logger(), "New plan contains " << msg->trajectory_points.size() << " points"); + + cur_traj_ = std::unique_ptr(move(msg)); + timesteps_since_last_traj_ = 0; + RCLCPP_INFO_STREAM(get_logger(), "Successfully swapped trajectories!"); + } + + void TrajectoryExecutor::guidanceStateMonitor(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) + { + // TODO need to handle control handover once alernative planner system is finished + if(msg->state != carma_planning_msgs::msg::GuidanceState::ENGAGED) + { + cur_traj_= nullptr; + } + } + +} // trajectory_executor + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(trajectory_executor::TrajectoryExecutor) diff --git a/trajectory_executor/test/test_trajectory_executor.cpp b/trajectory_executor/test/test_trajectory_executor.cpp new file mode 100644 index 0000000000..c30c65acb0 --- /dev/null +++ b/trajectory_executor/test/test_trajectory_executor.cpp @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2022 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 "trajectory_executor/trajectory_executor_node.hpp" +#include "trajectory_executor_test_suite.cpp" + +namespace trajectory_executor +{ + /*! + * \brief Test that the trajectory executor will output the trajectory on + * the desired topic after receiving a trajectory plan for the Pure Pursuit controller plugin + */ + TEST(TrajectoryExecutorTest, test_emit_multiple) + { + // Create and activate TrajectoryExecutor node + rclcpp::NodeOptions options; + auto traj_executor_node = std::make_shared(options); + traj_executor_node->configure(); //Call configure state transition + traj_executor_node->activate(); //Call activate state transition to get not read for runtime + + // Create and activate TrajectoryExecutorTestSuite node + rclcpp::NodeOptions options2; + auto test_suite_node = std::make_shared(options2); + test_suite_node->configure(); + test_suite_node->activate(); + + // Add these nodes to an executor to spin them and trigger callbacks + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(traj_executor_node->get_node_base_interface()); + executor.add_node(test_suite_node->get_node_base_interface()); + + // Generate a 'Pure Pursuit' trajectory plan and publish it to the /trajectory topic + carma_planning_msgs::msg::TrajectoryPlan plan = trajectory_executor_test_suite::buildSampleTraj(); + test_suite_node->traj_pub_->publish(plan); + + // Spin executor for 2 seconds + auto end_time = std::chrono::system_clock::now() + std::chrono::seconds(2); + while(std::chrono::system_clock::now() < end_time){ + executor.spin_once(); + } + + ASSERT_LE(10, test_suite_node->msg_count) << "Failed to receive whole trajectory from TrajectoryExecutor node."; + } + + /*! + * \brief Test that the trajectory executor will output the trajectory on + * the desired topic after receiving a trajectory plan for the PlatooningControlPlugin controller plugin + */ + TEST(TrajectoryExecutorTestSuite, test_emit_traj) { + // Create and activate TrajectoryExecutor node + rclcpp::NodeOptions options; + auto traj_executor_node = std::make_shared(options); + traj_executor_node->configure(); //Call configure state transition + traj_executor_node->activate(); //Call activate state transition to get not read for runtime + + // Create and activate TrajectoryExecutorTestSuite node + rclcpp::NodeOptions options2; + auto test_suite_node = std::make_shared(options2); + test_suite_node->configure(); + test_suite_node->activate(); + + // Add these nodes to an executor to spin them and trigger callbacks + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(traj_executor_node->get_node_base_interface()); + executor.add_node(test_suite_node->get_node_base_interface()); + + // Generate a 'PlatooningControlPlugin' trajectory plan and publish it to the /trajectory topic + carma_planning_msgs::msg::TrajectoryPlan plan = trajectory_executor_test_suite::buildSampleTraj2(); + test_suite_node->traj_pub_->publish(plan); + + // Spin executor for 2 seconds + auto end_time = std::chrono::system_clock::now() + std::chrono::seconds(2); + while(std::chrono::system_clock::now() < end_time){ + executor.spin_once(); + } + + ASSERT_GT(test_suite_node->msg_count, 0) << "Failed to receive whole trajectory from TrajectoryExecutor node."; + } + + /*! + * \brief Test that the Trajectory Executor properly shuts down if it encounters a trajectory + * containing a reference to a control plugin which was not discovered. + */ + TEST(TrajectoryExecutorTestSuite, test_control_plugin_not_found) { + // Create and activate TrajectoryExecutor node + rclcpp::NodeOptions options; + auto traj_executor_node = std::make_shared(options); + traj_executor_node->configure(); //Call configure state transition + traj_executor_node->activate(); //Call activate state transition to get not read for runtime + + // Create and activate TrajectoryExecutorTestSuite node + rclcpp::NodeOptions options2; + auto test_suite_node = std::make_shared(options2); + test_suite_node->configure(); + test_suite_node->activate(); + + // Add these nodes to an executor to spin them and trigger callbacks + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(traj_executor_node->get_node_base_interface()); + executor.add_node(test_suite_node->get_node_base_interface()); + + // Generate a 'Pure Pursuit' trajectory plan + carma_planning_msgs::msg::TrajectoryPlan plan = trajectory_executor_test_suite::buildSampleTraj(); + + // Assign controller plugin name NULL to first trajectory plan point; this is a plugin that trajectory_executor will not recognize + plan.trajectory_points[0].controller_plugin_name = "NULL"; + + // Publish trajectory plan from the test suite node + test_suite_node->traj_pub_->publish(plan); + + try { + // Spin executor for 2 seconds + auto end_time = std::chrono::system_clock::now() + std::chrono::seconds(2); + while(std::chrono::system_clock::now() < end_time){ + executor.spin_some(); + } + } + catch (const std::exception& e) + { + ASSERT_EQ(test_suite_node->msg_count, 0) << "TrajectoryExecutor published plans that began with an unknown control plugin"; + } + } + +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/trajectory_executor/test/trajectory_executor_3.test b/trajectory_executor/test/trajectory_executor_3.test deleted file mode 100644 index ef5098c5e6..0000000000 --- a/trajectory_executor/test/trajectory_executor_3.test +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - diff --git a/trajectory_executor/test/trajectory_executor_test_suite.cpp b/trajectory_executor/test/trajectory_executor_test_suite.cpp new file mode 100644 index 0000000000..53a48371aa --- /dev/null +++ b/trajectory_executor/test/trajectory_executor_test_suite.cpp @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2022 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 + +namespace trajectory_executor_test_suite +{ + + namespace std_ph = std::placeholders; + + /** + * TrajectoryExecutorTestSuite: Test fixture for TrajectoryExecutor testing + * Maintains publishers, subscribers, and message tracking for all tests. + * State is reset between tests to ensure clean results. + */ + class TrajectoryExecutorTestSuite : public carma_ros2_utils::CarmaLifecycleNode + { + + public: + /*! + * \brief Constructor for TrajectoryExecutorTestSuite + */ + explicit TrajectoryExecutorTestSuite(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) {} + + // Publisher + carma_ros2_utils::PubPtr traj_pub_; + + // Subscribers + carma_ros2_utils::SubPtr traj_sub_; + carma_ros2_utils::SubPtr traj_sub2_; + + int msg_count = 0; + + void trajEmitCallback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) { + msg_count++; + } + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) { + // Setup Publisher + traj_pub_ = create_publisher("trajectory", 5); + + // Setup Subscribers + traj_sub_ = create_subscription("/guidance/pure_pursuit/plan_trajectory", 100, + std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1)); + traj_sub2_ = create_subscription("/guidance/PlatooningControlPlugin/plan_trajectory", 100, + std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1)); + + return CallbackReturn::SUCCESS; + } + + }; + + /*! + * \brief Helper Function: Builds a small sample TrajectoryPlan message for the Pure Pursuit + * controller plugin + * + * \return A 10-point TrajectoryPlan message containing sample data + */ + carma_planning_msgs::msg::TrajectoryPlan buildSampleTraj() { + carma_planning_msgs::msg::TrajectoryPlan plan; + + plan.header.stamp = rclcpp::Time(0,0); + plan.trajectory_id = "TEST TRAJECTORY 1"; + + rclcpp::Time cur_time = rclcpp::Time(0,0); + for (int i = 0; i < 10; i++) { + carma_planning_msgs::msg::TrajectoryPlanPoint p; + p.controller_plugin_name = "Pure Pursuit"; + p.lane_id = "0"; + p.planner_plugin_name = "cruising"; + rclcpp::Duration dur((i * 0.13)*1e9); // Convert seconds to nanoseconds + p.target_time = cur_time + dur; + p.x = 10 * i; + p.y = 10 * i; + plan.trajectory_points.push_back(p); + } + + return plan; + } + + /*! + * \brief Helper Function: Builds a small sample TrajectoryPlan message for the PlatooningControlPlugin + * controller plugin + * + * \return A 10-point TrajectoryPlan message containing sample data + */ + carma_planning_msgs::msg::TrajectoryPlan buildSampleTraj2() { + carma_planning_msgs::msg::TrajectoryPlan plan; + + plan.header.stamp = rclcpp::Time(0,0); + plan.trajectory_id = "TEST TRAJECTORY 2"; + + rclcpp::Time cur_time = rclcpp::Time(0,0); + for (int i = 0; i < 10; i++) { + carma_planning_msgs::msg::TrajectoryPlanPoint p; + p.controller_plugin_name = "PlatooningControlPlugin"; + p.lane_id = "0"; + p.planner_plugin_name = "cruising"; + rclcpp::Duration dur((i * 0.13)*1e9); // Convert seconds to nanoseconds + p.target_time = cur_time + dur; + p.x = 10 * i; + p.y = 10 * i; + plan.trajectory_points.push_back(p); + } + + return plan; + } + +} // trajectory_executor_test_suite \ No newline at end of file