diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 32a2b7e4fb..4f9596d1c9 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -52,7 +52,7 @@ sonar.modules= bsm_generator, \ platooning_tactical_plugin, \ port_drayage_plugin, \ mobilitypath_publisher, \ - platoon_control_plugin, \ + platooning_control_plugin, \ rosbag_mock_drivers, \ lightbar_manager, \ inlanecruising_plugin, \ @@ -73,7 +73,6 @@ sonar.modules= bsm_generator, \ object_visualizer, \ points_map_filter, \ light_controlled_intersection_tactical_plugin, \ - platoon_control_ihp, \ carma_guidance_plugins, \ carma_cloud_client, \ approaching_emergency_vehicle_plugin, \ @@ -97,7 +96,7 @@ truck_inspection_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/t roadway_objects.sonar.projectBaseDir = /opt/carma/src/carma-platform/roadway_objects platooning_strategic_IHP.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_strategic_IHP platooning_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_tactical_plugin -platoon_control_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control +platooning_control_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control mobilitypath_publisher.sonar.projectBaseDir = /opt/carma/src/carma-platform/mobilitypath_publisher mock_lightbar_driver.sonar.projectBaseDir = /opt/carma/src/carma-platform/mock_drivers/mock_lightbar_driver port_drayage_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/port_drayage_plugin @@ -120,7 +119,6 @@ frame_transformer.sonar.projectBaseDir = /opt/carma/src/carma-platform/frame_tra object_visualizer.sonar.projectBaseDir = /opt/carma/src/carma-platform/object_visualizer points_map_filter.sonar.projectBaseDir = /opt/carma/src/carma-platform/points_map_filter light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/light_controlled_intersection_tactical_plugin -platoon_control_ihp.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control_ihp lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/lci_strategic_plugin carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_guidance_plugins carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cloud_client @@ -162,8 +160,8 @@ platooning_strategic_IHP.sonar.sources = src platooning_strategic_IHP.sonar.exclusions =test/** platooning_tactical_plugin.sonar.sources = src platooning_tactical_plugin.sonar.exclusions =test/** -platoon_control_plugin.sonar.sources = src -platoon_control_plugin.sonar.exclusions =test/** +platooning_control_plugin.sonar.sources = src +platooning_control_plugin.sonar.exclusions =test/** mobilitypath_publisher.sonar.sources = src mobilitypath_publisher.sonar.exclusions =test/** mock_lightbar_driver.sonar.sources = src @@ -208,8 +206,6 @@ points_map_filter.sonar.sources = src points_map_filter.sonar.exclusions =test/** light_controlled_intersection_tactical_plugin.sonar.sources = src light_controlled_intersection_tactical_plugin.sonar.exclusions =test/** -platoon_control_ihp.sonar.sources = src -platoon_control_ihp.sonar.exclusions =test/** lci_strategic_plugin.sonar.sources = src lci_strategic_plugin.sonar.exclusions =test/** carma_guidance_plugins.sonar.sources = src @@ -239,7 +235,7 @@ truck_inspection_client.sonar.tests = test roadway_objects.sonar.tests = test platooning_strategic_IHP.sonar.tests = test platooning_tactical_plugin.sonar.tests = test -platoon_control_plugin.sonar.tests = test +platooning_control_plugin.sonar.tests = test mobilitypath_publisher.sonar.tests = test mock_lightbar_driver.sonar.tests = test port_drayage_plugin.sonar.tests = test @@ -254,7 +250,6 @@ yield_plugin.sonar.tests = test basic_autonomy.sonar.tests = test mobilitypath_visualizer.sonar.tests = test sci_strategic_plugin.sonar.tests = test -platoon_control_ihp.sonar.tests = test stop_controlled_intersection_tactical_plugin.sonar.tests = test @@ -264,7 +259,6 @@ 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/carma/launch/guidance.launch b/carma/launch/guidance.launch index 9ef1788a45..16bbb146d0 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -41,7 +41,7 @@ - + @@ -58,15 +58,15 @@ - - - - + + + + - - + + @@ -87,25 +87,9 @@ - - - - + - - - - - - - - - - - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 13bb60cfe0..516a5e16fb 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -91,7 +91,10 @@ def generate_launch_description(): pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"] - lci_plugin_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"] + unique_vehicle_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"] + + platooning_control_param_file = os.path.join( + get_package_share_directory('platooning_control'), 'config/parameters.yaml') carma_inlanecruising_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', @@ -292,7 +295,7 @@ def generate_launch_description(): parameters=[ lci_strategic_plugin_file_path, vehicle_config_param_file, - lci_plugin_calibration_params + unique_vehicle_calibration_params ] ), ] @@ -559,6 +562,33 @@ def generate_launch_description(): ] ) + platooning_control_plugin_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platooning_control_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='platooning_control', + plugin='platooning_control::PlatooningControlPlugin', + name='platooning_control', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platooning_control_plugin', env_log_levels) } + ], + remappings = [ + ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ), + ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ), + ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ), + ("platooning_control/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/platooning_control/plan_trajectory" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ], + parameters=[ platooning_control_param_file, vehicle_config_param_file, unique_vehicle_calibration_params ] + ) + ] + ) + carma_stop_and_dwell_strategic_plugin_container = ComposableNodeContainer( package='carma_ros2_utils', name='carma_stop_and_dwell_strategic_plugin_container', @@ -630,6 +660,7 @@ def generate_launch_description(): carma_trajectory_follower_wrapper_container, #platooning_strategic_plugin_container, platooning_tactical_plugin_container, + platooning_control_plugin_container, intersection_transit_maneuvering_container ]) diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp index 5e0d582d93..87dd129742 100644 --- a/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp @@ -66,8 +66,11 @@ namespace carma_guidance_plugins // 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); + /** + * \brief Extending class provided method which can optionally handle trajectory plan callbacks. + */ + virtual void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); public: diff --git a/platooning_control/CMakeLists.txt b/platooning_control/CMakeLists.txt index 16943fcbc5..aaed1a8fa6 100644 --- a/platooning_control/CMakeLists.txt +++ b/platooning_control/CMakeLists.txt @@ -1,4 +1,5 @@ -# Copyright (C) 2018-2021 LEIDOS. + +# Copyright (C) 2024 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,112 +13,66 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) -project(platoon_control) +cmake_minimum_required(VERSION 3.5) +project(platooning_control) +# 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 - 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 -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec platooning_control_node_exec) +set(node_lib platooning_control_node) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} ) -file(GLOB_RECURSE headers */*.hpp */*.h) - -add_executable( ${PROJECT_NAME} - ${headers} - src/platoon_control.cpp - src/main.cpp) - - -add_library(platoon_control_plugin_lib - src/platoon_control.cpp - src/platoon_control_worker.cpp - src/pid_controller.cpp - src/pure_pursuit.cpp +# Build +ament_auto_add_library(${node_lib} SHARED + src/platooning_control.cpp + src/platooning_control_worker.cpp + src/pid_controller.cpp ) -add_dependencies(platoon_control_plugin_lib ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(platoon_control_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} platoon_control_plugin_lib) - - -############# -## Install ## -############# - - -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +ament_auto_add_executable(${node_exec} + src/main.cpp ) -## Install C++ -install(TARGETS ${PROJECT_NAME} platoon_control_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# Register component +rclcpp_components_register_nodes(${node_lib} "platooning_control::PlatooningControlPlugin") -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) +# Testing +if(BUILD_TESTING) -############# -## Testing ## -############# + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -catkin_add_gmock(${PROJECT_NAME}-test + ament_add_gtest(test_platooning_control + test/test_main.cpp + test/test_control.cpp test/test_pid.cpp - test/test_pure.cpp test/test_worker.cpp - test/test_control.cpp - test/test_main.cpp) + test/test_pure.cpp + ) -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test platoon_control_plugin_lib ${catkin_LIBRARIES}) -endif() + target_link_libraries(test_platooning_control ${node_lib}) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_platoon_control test/mynode.test test/test_mynode.cpp) - target_link_libraries(test_platoon_control ${catkin_LIBRARIES}) endif() - +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/platooning_control/README.md b/platooning_control/README.md index 02b19ac49c..ab94f8e67c 100644 --- a/platooning_control/README.md +++ b/platooning_control/README.md @@ -1,7 +1,6 @@ -# platoon_control +# platooning_control Platooning Control plugin which allows platooning to maintain the gap; moreover, generates longitudinal and lateral control commands to follow the trajectory. The structure of this plugin is very similar to the control plugin for IHP2, so the same design document is included here. -The difference between platoon_control, and platoon_control_ihp is that the IHP plugin includes logic to open or close the gap between platoon members, to allow for a new member to join or an existing memeber to exit the platoon. Overview https://usdot-carma.atlassian.net/wiki/spaces/CUCS/pages/2392981532/Basic+Travel+and+IHP diff --git a/platooning_control/config/parameters.yaml b/platooning_control/config/parameters.yaml index 796653bd42..066643621a 100644 --- a/platooning_control/config/parameters.yaml +++ b/platooning_control/config/parameters.yaml @@ -1,19 +1,14 @@ -standStillHeadway: 12.0 # Standstill gap between vehicles (m) -maxAccel: 1.5 # Maximum acceleration absolute value used in controller filters (m/s^2) -Kp: 0.4 # Proportional weight for PID controller -Kd: 0.0 # Derivative Weight for PID controller -Ki: 0.03 # Integral weight for PID controller -maxValue: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) -minValue: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) -cmdTmestamp: 100 # Timestep to calculate ctrl commands (ms) -adjustmentCap: 15.0 # Adjustment cap for speed command (m/s) -dt: 0.1 # Timestep to calculate ctrl commands (s) -integratorMax: 30 # Max limit for integrator term -integratorMin: -30 # Min limit for integrator term -lowpassGain: 0.5 ##Lowpass filter gain -lookaheadRatio: 2.0 # Ratio to calculate lookahead distance -minLookaheadDist: 15.0 # Min lookahead distance (m) -correctionAngle: 0.0 # Correction angle to improve steering accuracy -integratorMax_pp: 0.1 # Max integrator val for pure pursuit integral controller -integratorMin_pp: -0.1 # Min integrator val for pure pursuit integral controller -Ki_pp: 0.012 # Integral weight for pure pursuit integral controller \ No newline at end of file +stand_still_headway_m: 12.0 # Standstill gap between vehicles (m) +max_accel_ms2: 1.5 # Maximum acceleration absolute value used in controller filters (m/s^2) +kp: 0.4 # Proportional weight for PID controller +kd: 0.0 # Derivative Weight for PID controller +ki: 0.03 # Integral weight for PID controller +max_delta_speed_per_timestep: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) +min_delta_speed_per_timestep: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) +cmd_timestamp_ms: 100 # Timestep to calculate ctrl commands (ms) +adjustment_cap_mps: 15.0 # Adjustment cap for speed command (m/s) +integrator_max: 30.0 # Max limit for integrator term +integrator_min: -30.0 # Min limit for integrator term +vehicle_response_lag: 0.2 +enable_max_adjustment_filter: true +enable_max_accel_filter: true \ No newline at end of file diff --git a/platooning_control/include/platoon_control.h b/platooning_control/include/platoon_control.h deleted file mode 100644 index 1e25135541..0000000000 --- a/platooning_control/include/platoon_control.h +++ /dev/null @@ -1,170 +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 -#include -#include -#include -#include -#include -#include -#include -#include "platoon_control_worker.h" - - - - -namespace platoon_control -{ - /** - * \brief This class includes logic for Platoon control. It includes publishers and subscribers and their callback functions - */ - class PlatoonControlPlugin - { - public: - - /** - * \brief Default constructor for PlatoonControlPlugin class - */ - PlatoonControlPlugin(); - - /** - * \brief Initialize plugin variables and publishers/subscribers - */ - void initialize(); - - /** - * \brief general starting point of this node - */ - void run(); - - /** - * \brief generate control signal by calculating speed and steering commands. - * \param point0 start point of control window - * \param point_end end point of control wondow - */ - void generateControlSignals(const cav_msgs::TrajectoryPlanPoint& point0, const cav_msgs::TrajectoryPlanPoint& point_end); - - /** - * \brief Compose twist message from linear and angular velocity commands. - * \param linear_vel linear velocity in m/s - * \param angular_vel angular velocity in rad/s - * \return twist message - */ - geometry_msgs::TwistStamped composeTwistCmd(double linear_vel, double angular_vel); - - /** - * \brief Compose control message from speed and steering commands. - * \param linear_vel linear velocity in m/s - * \param steering_angle steering angle in rad - * \return control command - */ - autoware_msgs::ControlCommandStamped composeCtrlCmd(double linear_vel, double steering_angle); - - /** - * \brief find the point correspoding to the lookahead distance - * \param trajectory_plan trajectory plan - * \return trajectory point - */ - cav_msgs::TrajectoryPlanPoint getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan); - - /** - * \brief 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 - PlatoonControlWorker pcw_; - - // platooning config object - PlatooningControlPluginConfig 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 - - /** - * \brief callback function for pose - * \param msg pose stamped msg - */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief callback function for platoon info - * \param msg platoon info msg - */ - void platoonInfo_cb(const cav_msgs::PlatooningInfoConstPtr& msg); - - /** - * \brief callback function for trajectory plan - * \param msg trajectory plan msg - */ - void trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp); - - /** - * \brief callback function for current twist - * \param msg twist stamped msg - */ - void currentTwist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist); - - /** - * \brief calculate average speed of a set of trajectory points - * \param trajectory_points set of trajectory points - * \return trajectory speed - */ - 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/include/platoon_control_config.h b/platooning_control/include/platoon_control_config.h deleted file mode 100644 index 3f40f4305d..0000000000 --- a/platooning_control/include/platoon_control_config.h +++ /dev/null @@ -1,82 +0,0 @@ -#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 PlatooningControlPluginConfig -{ - double standStillHeadway = 12.0; // Standstill gap between vehicles (m) - double maxAccel = 2.5; // Maximum acceleration absolute value used in controller filters (m/s^2) - double Kp = 0.5; // Proportional weight for PID controller - double Kd = -0.5; // Derivative Weight for PID controller - double Ki = 0.0; // Integral weight for PID controller - double maxValue = 2; // Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) - double minValue = -10; // Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) - double dt = 0.1; // Timestep to calculate ctrl commands (s) - double adjustmentCap = 10; // Adjustment cap for speed command (m/s) - int cmdTmestamp = 100; // Timestamp to calculate ctrl commands (ms) - double integratorMax = 100; // Max limit for integrator term - double integratorMin = -100; // Max limit for integrator term - double Kdd = 4.5; //coefficient for smooth steering - double wheelBase = 3.09; //Wheelbase of the vehicle (m) - double lowpassGain = 0.5; // Lowpass filter gain - double lookaheadRatio = 2.0; // Ratio to calculate lookahead distance - double minLookaheadDist = 6.0; // Min lookahead distance (m) - std::string vehicleID = "DEFAULT_VEHICLE_ID"; // Vehicle id is the license plate of the vehicle - int shutdownTimeout = 200; // Timeout to stop generating ctrl signals after stopped receiving trajectory (ms) - int ignoreInitialInputs = 0; // num inputs to throw away after startup - double correctionAngle = 0.0; //Correction angle to improve steering accuracy - double integratorMax_pp = 0.0; //Max integrator val for pure pursuit integral controller - double integratorMin_pp = 0.0; //Min integrator val for pure pursuit integral controller - double Ki_pp = 0.0; // Integral weight for pure pursuit integral controller - - - friend std::ostream& operator<<(std::ostream& output, const PlatooningControlPluginConfig& c) - { - output << "PlatooningControlPluginConfig { " << 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 - << "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; - } -}; \ No newline at end of file diff --git a/platooning_control/include/platoon_control_worker.h b/platooning_control/include/platoon_control_worker.h deleted file mode 100644 index 25c641a402..0000000000 --- a/platooning_control/include/platoon_control_worker.h +++ /dev/null @@ -1,158 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#include -#include -#include -#include -#include -#include -#include "pid_controller.h" -#include "pure_pursuit.h" -#include "platoon_control_config.h" -#include - - - - - -namespace platoon_control -{ - /** - * \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 - - /** - * \brief This is the worker class for platoon controller. It is responsible for generating and smoothing the speed and steering control commands from trajectory points. - */ - class PlatoonControlWorker - { - public: - - /** - * \brief Default constructor for platooning control worker - */ - PlatoonControlWorker(); - - /** - * \brief Update configurations - * \param new_config new configuration - */ - void updateConfigParams(PlatooningControlPluginConfig new_config); - - /** - * \brief Returns latest speed command - * \return lastest speed command in m/s - */ - double getLastSpeedCommand() const; - - /** - * \brief Generates speed commands (in m/s) based on the trajectory point - * \param point trajectory point - */ - void generateSpeed(const cav_msgs::TrajectoryPlanPoint& point); - - /** - * \brief Generates steering commands (in rad) based on lookahead trajectory point - * \param point trajectory point - */ - void generateSteer(const cav_msgs::TrajectoryPlanPoint& point); - - /** - * \brief Sets the platoon leader object using info from msg - * \param leader leader information msg received from strategic plugin - */ - void setLeader(const PlatoonLeaderInfo& leader); - - /** - * \brief set current pose - * \param msg pose value - */ - void setCurrentPose(const geometry_msgs::PoseStamped msg); - - - /** - * \brief set current speed - * \param speed speed value - */ - void setCurrentSpeed(double speed); - - // Member Variables - double speedCmd = 0; - double currentSpeed = 0; - double lastCmdSpeed = 0.0; - double speedCmd_ = 0; - double steerCmd_ = 0; - double angVelCmd_ = 0; - double desired_gap_ = ctrl_config_.standStillHeadway; - double actual_gap_ = 0.0; - bool last_cmd_set_ = false; - - // Platoon Leader - PlatoonLeaderInfo platoon_leader; - - // geometry pose - geometry_msgs::Pose current_pose_; - - - private: - // config parameters - PlatooningControlPluginConfig 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/include/pid_controller.h b/platooning_control/include/platooning_control/pid_controller.hpp similarity index 84% rename from platooning_control/include/pid_controller.h rename to platooning_control/include/platooning_control/pid_controller.hpp index 459c276353..ff433af4fa 100644 --- a/platooning_control/include/pid_controller.h +++ b/platooning_control/include/platooning_control/pid_controller.hpp @@ -1,5 +1,5 @@ /*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. +* Copyright (C) 2024 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,22 +15,20 @@ ------------------------------------------------------------------------------*/ -#include -#include "platoon_control_config.h" +#include +#include "platooning_control/platooning_control_config.hpp" - -namespace platoon_control +namespace platooning_control { - /** * \brief This class includes logic for PID controller. PID controller is used in this plugin to maintain the inter-vehicle gap by adjusting the speed. */ - class PIDController - { + class PIDController + { public: - /** + /** * \brief Constructor for the PID controller class */ PIDController(); @@ -38,9 +36,10 @@ namespace platoon_control /** * \brief plugin config object */ - PlatooningControlPluginConfig config_; - - // ~PIDController(); + std::shared_ptr config_ = std::make_shared(); + + + // ~PIDController(); // Kp - proportional gain // Ki - Integral gain @@ -61,15 +60,9 @@ namespace platoon_control void reset(); - - - - - private: double _pre_error = 0.0; double _integral = 0.0; - - }; -} + }; +} \ No newline at end of file diff --git a/platooning_control/include/platooning_control/platooning_control.hpp b/platooning_control/include/platooning_control/platooning_control.hpp new file mode 100644 index 0000000000..def1a6e7e3 --- /dev/null +++ b/platooning_control/include/platooning_control/platooning_control.hpp @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2024 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 "platooning_control/platooning_control_config.hpp" +#include "platooning_control/platooning_control_worker.hpp" +#include +#include +#include + +namespace pure_pursuit = autoware::motion::control::pure_pursuit; +namespace platooning_control +{ + + /** + * \brief This class includes node-level logic for Platooning Control such as its publishers, subscribers, and their callback functions. + * Platooning Control is used for generating control commands to maintain the gap in platoon as well as generating longitudinal and lateral control commands to follow the trajectory. + */ + class PlatooningControlPlugin : public carma_guidance_plugins::ControlPlugin + { + + public: + /** + * \brief PlatooningControlPlugin constructor + */ + explicit PlatooningControlPlugin(const rclcpp::NodeOptions& options); + + /** + * \brief generate control signal by calculating speed and steering commands. + * \param point0 start point of control window + * \param point_end end point of control wondow + */ + autoware_msgs::msg::ControlCommandStamped generate_control_signals(const carma_planning_msgs::msg::TrajectoryPlanPoint& first_trajectory_point, const geometry_msgs::msg::PoseStamped& current_pose, const geometry_msgs::msg::TwistStamped& current_twist); + + /** + * \brief Compose twist message from linear and angular velocity commands. + * \param linear_vel linear velocity in m/s + * \param angular_vel angular velocity in rad/s + * \return twist message + */ + geometry_msgs::msg::TwistStamped compose_twist_cmd(double linear_vel, double angular_vel); + + motion::motion_common::State convert_state(const geometry_msgs::msg::PoseStamped& pose, const geometry_msgs::msg::TwistStamped& twist) const; + + double trajectory_speed_ = 0.0; + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); + + /** + * \brief callback function for trajectory plan + * \param msg trajectory plan msg + */ + void current_trajectory_callback(const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr tp); + + /** + * \brief Compose control message from speed and steering commands. + * \param linear_vel linear velocity in m/s + * \param steering_angle steering angle in rad + * \return control command + */ + autoware_msgs::msg::ControlCommandStamped compose_ctrl_cmd(double linear_vel, double steering_angle); + + /** + * \brief Returns availability of plugin. Always true + */ + bool get_availability() override; + + /** + * \brief Returns version id of plugn. + */ + std::string get_version_id() override; + + //// + // Overrides + //// + + autoware_msgs::msg::ControlCommandStamped generate_command() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin() override; + + std::shared_ptr pp_; + + + private: + + // Node configuration + PlatooningControlPluginConfig config_; + + // platoon control worker object + PlatooningControlWorker pcw_; + + // Variables + PlatoonLeaderInfo platoon_leader_; + double prev_input_time_ms_ = 0; //timestamp of the previous trajectory plan input received + long consecutive_input_counter_ = 0; //num inputs seen without a timeout + + /** + * \brief callback function for platoon info + * \param msg platoon info msg + */ + void platoon_info_cb(const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg); + + /** + * \brief calculate average speed of a set of trajectory points + * \param trajectory_points set of trajectory points + * \return trajectory speed + */ + double get_trajectory_speed(const std::vector& trajectory_points); + + + // Subscribers + carma_ros2_utils::SubPtr trajectory_plan_sub_; + carma_ros2_utils::SubPtr platoon_info_sub_; + + // Publishers + carma_ros2_utils::PubPtr platoon_info_pub_; + + // Unit Test Accessors + FRIEND_TEST(PlatooningControlPluginTest, test_platoon_info_cb); + FRIEND_TEST(PlatooningControlPluginTest, test_get_trajectory_speed); + FRIEND_TEST(PlatooningControlPluginTest, test_generate_controls); + FRIEND_TEST(PlatooningControlPluginTest, test_current_trajectory_callback); + + }; + +} // platooning_control diff --git a/platooning_control/include/platooning_control/platooning_control_config.hpp b/platooning_control/include/platooning_control/platooning_control_config.hpp new file mode 100644 index 0000000000..d4b5ee0a63 --- /dev/null +++ b/platooning_control/include/platooning_control/platooning_control_config.hpp @@ -0,0 +1,107 @@ +#pragma once + +/* + * Copyright (C) 2024 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_control +{ + +/** + * \brief Stuct containing the algorithm configuration values for the PlatooningControlPlugin + */ + struct PlatooningControlPluginConfig + { + double stand_still_headway_m = 12.0; // Standstill gap between vehicles (m) + double max_accel_mps2 = 2.5; // Maximum acceleration absolute value used in controller filters (m/s^2) + double kp = 0.5; // Proportional weight for PID controller + double kd = -0.5; // Derivative Weight for PID controller + double ki = 0.0; // Integral weight for PID controller + double max_delta_speed_per_timestep = 2; // Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) + double min_delta_speed_per_timestep = -10; // Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) + + double adjustment_cap_mps = 10; // Adjustment cap for speed command (m/s) + int cmd_timestamp_ms = 100; // Timestamp to calculate ctrl commands (ms) + double integrator_max = 100; // Max limit for integrator term + double integrator_min = -100; // Max limit for integrator term + std::string vehicle_id = "DEFAULT_VEHICLE_ID"; // Vehicle id is the license plate of the vehicle + int shutdown_timeout = 200; // Timeout to stop generating ctrl signals after stopped receiving trajectory (ms) + int ignore_initial_inputs = 0; // num inputs to throw away after startup + + //Pure Pursuit configs + double vehicle_response_lag = 0.2; // An approximation of the delay (sec) between sent vehicle commands and the vehicle begining a meaningful acceleration to that command + double max_lookahead_dist = 100.0; + double min_lookahead_dist = 6.0; // Min lookahead distance (m) + double speed_to_lookahead_ratio = 2.0; // Ratio to calculate lookahead distance + bool is_interpolate_lookahead_point = true; + bool is_delay_compensation = true; // not to be confused with vehicle_response_lag, which is applied nonetheless + double emergency_stop_distance = 0.1; + double speed_thres_traveling_direction = 0.3; + double dist_front_rear_wheels = 3.5; + + double dt = 0.1; // Timestep to calculate ctrl commands (s) + double integrator_max_pp = 0.0; //Max integrator val for pure pursuit integral controller + double integrator_min_pp = 0.0; //Min integrator val for pure pursuit integral controller + double ki_pp = 0.0; // Integral weight for pure pursuit integral controller"; + bool is_integrator_enabled = false; + bool enable_max_adjustment_filter = true; + bool enable_max_accel_filter = true; + + + + // Stream operator for this config + friend std::ostream& operator<<(std::ostream& output, const PlatooningControlPluginConfig& c) + { + output << "PlatooningControlPluginConfig { " << std::endl + << "stand_still_headway_m: " << c.stand_still_headway_m << std::endl + << "max_accel_mps2: " << c.max_accel_mps2 << std::endl + << "kp: " << c.kp << std::endl + << "kd: " << c.kd << std::endl + << "ki: " << c.ki << std::endl + << "max_delta_speed_per_timestep: " << c.max_delta_speed_per_timestep << std::endl + << "min_delta_speed_per_timestep_: " << c.min_delta_speed_per_timestep << std::endl + << "adjustment_cap_mps: " << c.adjustment_cap_mps << std::endl + << "cmd_timestamp_ms: " << c.cmd_timestamp_ms << std::endl + << "integrator_max: " << c.integrator_max << std::endl + << "integrator_min: " << c.integrator_min << std::endl + << "vehicle_id: " << c.vehicle_id << std::endl + << "shutdown_timeout: " << c.shutdown_timeout << std::endl + << "ignore_initial_inputs: " << c.ignore_initial_inputs << std::endl + //Pure Pursuit configs + << "vehicle_response_lag" << c.vehicle_response_lag << std::endl + << "max_lookahead_dist: " << c.max_lookahead_dist << std::endl + << "min_lookahead_dist: " << c.min_lookahead_dist << std::endl + << "speed_to_lookahead_ratio: " << c.speed_to_lookahead_ratio << std::endl + << "is_interpolate_lookahead_point: " << c.is_interpolate_lookahead_point << std::endl + << "is_delay_compensation: " << c.is_delay_compensation << std::endl + << "emergency_stop_distance: " << c.emergency_stop_distance << std::endl + << "speed_thres_traveling_direction: "<< c.speed_thres_traveling_direction << std::endl + << "dist_front_rear_wheels: " << c.dist_front_rear_wheels << std::endl + << "dt: " << c.dt << std::endl + << "integrator_max_pp: " << c.integrator_max_pp << std::endl + << "integrator_min_pp: " << c.integrator_min_pp << std::endl + << "ki_pp_: " << c.ki_pp << std::endl + << "is_integrator_enabled" << c.is_integrator_enabled < +#include +#include +#include +#include +#include +#include +#include "platooning_control/pid_controller.hpp" +#include "platooning_control/platooning_control_config.hpp" +#include + +namespace platooning_control +{ + /** + * \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 + /** + * \brief This is the worker class for platoon controller. It is responsible for generating and smoothing the speed and steering control commands from trajectory points. + */ + class PlatooningControlWorker + { + public: + + /** + * \brief Default constructor for platooning control worker + */ + PlatooningControlWorker(); + + /** + * \brief Returns latest speed command + * \return lastest speed command in m/s + */ + double get_last_speed_command() const; + + /** + * \brief Generates speed commands (in m/s) based on the trajectory point + * \param point trajectory point + */ + void generate_speed(const carma_planning_msgs::msg::TrajectoryPlanPoint& point); + + /** + * \brief Sets the platoon leader object using info from msg + * \param leader leader information msg received from strategic plugin + */ + void set_leader(const PlatoonLeaderInfo& leader); + + + /** + * \brief set current speed + * \param speed speed value + */ + void set_current_speed(double speed); + + // Member Variables + + // Platoon Leader + PlatoonLeaderInfo platoon_leader; + + // config parameters + std::shared_ptr ctrl_config_ = std::make_shared(); + + double speedCmd = 0; + double currentSpeed = 0; + double lastCmdSpeed = 0.0; + double speedCmd_ = 0; + double steerCmd_ = 0; + double angVelCmd_ = 0; + double desired_gap_ = ctrl_config_->stand_still_headway_m; + double actual_gap_ = 0.0; + bool last_cmd_set_ = false; + + + private: + + // pid controller object + PIDController pid_ctrl_; + + double dist_to_front_vehicle; + + + }; +} \ No newline at end of file diff --git a/platooning_control/include/pure_pursuit.h b/platooning_control/include/pure_pursuit.h deleted file mode 100644 index a7270622f0..0000000000 --- a/platooning_control/include/pure_pursuit.h +++ /dev/null @@ -1,147 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - - -#include -#include -#include -#include -#include -#include -#include -#include "platoon_control_config.h" - - - - - - - -namespace platoon_control -{ - - /** - * \brief This class includes logic for Pure Pursuit controller. This controller is used to calculate a steering angle using the current pose of the vehcile and a lookahead point. - */ - - class PurePursuit - { - - - public: - - /** - * \brief Default constructor for pure pursuit - */ - PurePursuit(); - - /** - * \brief calculates steering angle based on lookahead trajectory point - * \param tp lookahead trajectory point - */ - void calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp); - - /** - * \brief calculates curvature to the lookahead trajectory point - * \param tp lookahead trajectory point - * \return curvature to the lookahead point - */ - double calculateKappa(const cav_msgs::TrajectoryPlanPoint& tp); - - /** - * \brief calculates sin of the heading angle to the target point - * \param tp lookahead trajectory point - * \param current_pose current pose of the vehicle - * \return sin of the heading angle - */ - double getAlphaSin(cav_msgs::TrajectoryPlanPoint tp, geometry_msgs::Pose current_pose); - - /** - * \brief Lowpass filter to smoothen control signal - * \param gain filter gain - * \param prev_value previous value - * \param value current value - * \return smoothened control signal - */ - double lowPassfilter(double gain, double prev_value, double value); - - /** - * \brief returns steering angle - * \return steering angle in rad - */ - double getSteeringAngle(); - - /** - * \brief returns angular velocity - * \return angular velocity in rad/s - */ - double getAngularVelocity(); - - // geometry pose - geometry_msgs::Pose current_pose_; - double velocity_ = 0.0; - double angular_velocity_ = 0; - double steering_angle_ = 0; - - PlatooningControlPluginConfig config_; - - private: - - /** - * \brief calculate lookahead distance - * \param tp trajectory point - * \return lookahead distance from next trajectory point - */ - double getLookaheadDist(const cav_msgs::TrajectoryPlanPoint& tp) const; - - /** - * \brief calculate yaw angle of the vehicle - * \param tp trajectory point - * \return yaw angle of the vehicle in rad - */ - double getYaw(const cav_msgs::TrajectoryPlanPoint& tp) const; - - /** - * \brief calculate steering direction - * \param tp trajectory point - * \return steering direction (+1 is left and -1 is right) - */ - 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; - - double _integral = 0.0; - - // helper function (if needed) - // inline double deg2rad(double deg) const - // { - // return deg * M_PI / 180; - // } // convert degree to radian - - - }; -} diff --git a/platooning_control/launch/platoon_control.launch b/platooning_control/launch/platoon_control.launch deleted file mode 100644 index eeccbfcd27..0000000000 --- a/platooning_control/launch/platoon_control.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - diff --git a/platooning_control/launch/platoon_control.launch.py b/platooning_control/launch/platoon_control.launch.py new file mode 100644 index 0000000000..f65cc62529 --- /dev/null +++ b/platooning_control/launch/platoon_control.launch.py @@ -0,0 +1,68 @@ +# Copyright (C) 2024 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 platooning_control_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('platooning_control'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platooning_control_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='platooning_control', + plugin='platooning_control::PlatooningControlPlugin', + name='platooning_control', + 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/platooning_control/package.xml b/platooning_control/package.xml index c44645798f..71e3a7ba2e 100644 --- a/platooning_control/package.xml +++ b/platooning_control/package.xml @@ -1,16 +1,46 @@ + + + - platoon_control - 3.3.0 - The node is to control a platooning maneuver - carma - Apache 2.0 License - catkin - carma_utils - cav_msgs - roscpp - std_msgs + platooning_control + 1.0.0 + The platooning_control package + + carma + + Apache 2.0 + + ament_cmake carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_guidance_plugins + carma_planning_msgs autoware_msgs - + pure_pursuit + basic_autonomy + + ament_lint_auto + ament_cmake_gtest + + launch_ros + + + ament_cmake + diff --git a/platooning_control/src/main.cpp b/platooning_control/src/main.cpp index e5f9271998..42594d401f 100644 --- a/platooning_control/src/main.cpp +++ b/platooning_control/src/main.cpp @@ -1,32 +1,34 @@ +/* + * Copyright (C) 2024 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. + */ -/*------------------------------------------------------------------------------ -* 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 "platooning_control/platooning_control.hpp" -------------------------------------------------------------------------------*/ +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); -#include -#include "platoon_control.h" + auto node = std::make_shared(rclcpp::NodeOptions()); -int main(int argc, char** argv) -{ - - ros::init(argc, argv, "platoon_control"); - platoon_control::PlatoonControlPlugin node; - node.run(); - return 0; -}; + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/platooning_control/src/pid_controller.cpp b/platooning_control/src/pid_controller.cpp index 70da0c914e..52da6729b7 100644 --- a/platooning_control/src/pid_controller.cpp +++ b/platooning_control/src/pid_controller.cpp @@ -1,5 +1,5 @@ /*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. +* Copyright (C) 2024 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,63 +15,60 @@ ------------------------------------------------------------------------------*/ -#include "pid_controller.h" +#include "platooning_control/pid_controller.hpp" - -namespace platoon_control +namespace platooning_control { - PIDController::PIDController(){} - - double PIDController::calculate( double setpoint, double pv ){ + PIDController::PIDController(){} - // Calculate error + double PIDController::calculate( double setpoint, double pv ){ + // Calculate error double error = setpoint - pv; - ROS_DEBUG_STREAM("PID error: " << error); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"),"PID error: " << error); // Proportional term - double Pout = config_.Kp * error; - ROS_DEBUG_STREAM("Proportional term: " << Pout); + double Pout = config_->kp * error; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Proportional term: " << Pout); // Integral term - _integral += error * config_.dt; - ROS_DEBUG_STREAM("Integral term: " << _integral); + _integral += error * config_->dt; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"),"Integral term: " << _integral); - if (_integral > config_.integratorMax){ - _integral = config_.integratorMax; + if (_integral > config_->integrator_max){ + _integral = config_->integrator_max; } - else if (_integral < config_.integratorMin){ - _integral = config_.integratorMin; + else if (_integral < config_->integrator_min){ + _integral = config_->integrator_min; } - double Iout = config_.Ki * _integral; - ROS_DEBUG_STREAM("Iout: " << Iout); + double Iout = config_->ki * _integral; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "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); + double derivative = (error - _pre_error) / config_->dt; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "derivative term: " << derivative); + double Dout = config_->kd * derivative; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Dout: " << Dout); // Calculate total output double output = Pout + Iout + Dout; - ROS_DEBUG_STREAM("total controller output: " << output); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "total controller output: " << output); // Restrict to max/min - if( output > config_.maxValue ) - output = config_.maxValue; - else if( output < config_.minValue ) - output = config_.minValue; + if( output > config_->max_delta_speed_per_timestep ) + output = config_->max_delta_speed_per_timestep; + else if( output < config_->min_delta_speed_per_timestep ) + output = config_->min_delta_speed_per_timestep; // Save error to previous error _pre_error = error; return output; - } - - + } - void PIDController::reset() { + void PIDController::reset() { _integral = 0.0; _pre_error = 0.0; } + } diff --git a/platooning_control/src/platoon_control.cpp b/platooning_control/src/platoon_control.cpp deleted file mode 100644 index 04b3f16ad0..0000000000 --- a/platooning_control/src/platoon_control.cpp +++ /dev/null @@ -1,323 +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 "platoon_control.h" - -namespace platoon_control -{ -// @SONAR_STOP@ - PlatoonControlPlugin::PlatoonControlPlugin() - { - pcw_ = PlatoonControlWorker(); - } - - - void PlatoonControlPlugin::initialize(){ - - nh_.reset(new ros::CARMANodeHandle()); - pnh_.reset(new ros::CARMANodeHandle("~")); - - PlatooningControlPluginConfig config; - - 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); - 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("/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("platoon_control/plan_trajectory", 1, &PlatoonControlPlugin::trajectoryPlan_cb, this); - - // Current Twist Subscriber - current_twist_sub_ = nh_->subscribe("current_velocity", 1, &PlatoonControlPlugin::currentTwist_cb, this); - - // Platoon Info Subscriber - platoon_info_sub_ = nh_->subscribe("platoon_info", 1, &PlatoonControlPlugin::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, &PlatoonControlPlugin::pose_cb, this); - - plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "platoon_control"; - 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 PlatoonControlPlugin::run(){ - initialize(); - 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; - } - - // 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 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) - { - 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; - - 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; - pcw_.desired_gap_ = platooing_info_msg.desired_gap; - - platooing_info_msg.host_cmd_speed = pcw_.speedCmd_; - platoon_info_pub_.publish(platooing_info_msg); - } - - - void PlatoonControlPlugin::currentTwist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist){ - current_speed_ = twist->twist.linear.x; - } - - -// @SONAR_START@ - - geometry_msgs::TwistStamped PlatoonControlPlugin::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 PlatoonControlPlugin::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 PlatoonControlPlugin::generateControlSignals(const cav_msgs::TrajectoryPlanPoint& first_trajectory_point, const cav_msgs::TrajectoryPlanPoint& lookahead_point){ - - 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 PlatoonControlPlugin::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/src/platooning_control.cpp b/platooning_control/src/platooning_control.cpp new file mode 100644 index 0000000000..6dd2106aa7 --- /dev/null +++ b/platooning_control/src/platooning_control.cpp @@ -0,0 +1,393 @@ +/* + * Copyright (C) 2024 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 "platooning_control/platooning_control.hpp" + +namespace platooning_control +{ + namespace std_ph = std::placeholders; + + PlatooningControlPlugin::PlatooningControlPlugin(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::ControlPlugin(options), config_(PlatooningControlPluginConfig()), pcw_(PlatooningControlWorker()) + { + + // Declare parameters + config_.stand_still_headway_m = declare_parameter("stand_still_headway_m", config_.stand_still_headway_m); + config_.max_accel_mps2 = declare_parameter("max_accel_mps2", config_.max_accel_mps2); + config_.kp = declare_parameter("kp", config_.kp); + config_.kd = declare_parameter("kd", config_.kd); + config_.ki = declare_parameter("ki", config_.ki); + config_.max_delta_speed_per_timestep = declare_parameter("max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep); + config_.min_delta_speed_per_timestep = declare_parameter("min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep); + config_.adjustment_cap_mps = declare_parameter("adjustment_cap_mps", config_.adjustment_cap_mps); + config_.cmd_timestamp_ms = declare_parameter("cmd_timestamp_ms", config_.cmd_timestamp_ms); + config_.integrator_max = declare_parameter("integrator_max", config_.integrator_max); + config_.integrator_min = declare_parameter("integrator_min", config_.integrator_min); + + config_.vehicle_response_lag = declare_parameter("vehicle_response_lag", config_.vehicle_response_lag); + config_.max_lookahead_dist = declare_parameter("maximum_lookahead_distance", config_.max_lookahead_dist); + config_.min_lookahead_dist = declare_parameter("minimum_lookahead_distance", config_.min_lookahead_dist); + config_.speed_to_lookahead_ratio = declare_parameter("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio); + config_.is_interpolate_lookahead_point = declare_parameter("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point); + config_.is_delay_compensation = declare_parameter("is_delay_compensation", config_.is_delay_compensation); + config_.emergency_stop_distance = declare_parameter("emergency_stop_distance", config_.emergency_stop_distance); + config_.speed_thres_traveling_direction = declare_parameter("speed_thres_traveling_direction", config_.speed_thres_traveling_direction); + config_.dist_front_rear_wheels = declare_parameter("dist_front_rear_wheels", config_.dist_front_rear_wheels); + + config_.dt = declare_parameter("dt", config_.dt); + config_.integrator_max_pp = declare_parameter("integrator_max_pp", config_.integrator_max_pp); + config_.integrator_min_pp = declare_parameter("integrator_min_pp", config_.integrator_min_pp); + config_.ki_pp = declare_parameter("Ki_pp", config_.ki_pp); + config_.is_integrator_enabled = declare_parameter("is_integrator_enabled", config_.is_integrator_enabled); + config_.enable_max_adjustment_filter = declare_parameter("enable_max_adjustment_filter", config_.enable_max_adjustment_filter); + config_.enable_max_accel_filter = declare_parameter("enable_max_accel_filter", config_.enable_max_accel_filter); + + //Global params (from vehicle config) + config_.vehicle_id = declare_parameter("vehicle_id", config_.vehicle_id); + config_.shutdown_timeout = declare_parameter("control_plugin_shutdown_timeout", config_.shutdown_timeout); + config_.ignore_initial_inputs = declare_parameter("control_plugin_ignore_initial_inputs", config_.ignore_initial_inputs); + + pcw_.ctrl_config_ = std::make_shared(config_); + + } + + rcl_interfaces::msg::SetParametersResult PlatooningControlPlugin::parameter_update_callback(const std::vector ¶meters) + { + auto error_double = update_params({ + {"stand_still_headway_m", config_.stand_still_headway_m}, + {"max_accel_mps2", config_.max_accel_mps2}, + {"kp", config_.kp}, + {"kd", config_.kd}, + {"ki", config_.ki}, + {"max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep}, + {"min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep}, + {"adjustment_cap_mps", config_.adjustment_cap_mps}, + {"integrator_max", config_.integrator_max}, + {"integrator_min", config_.integrator_min}, + + {"vehicle_response_lag", config_.vehicle_response_lag}, + {"max_lookahead_dist", config_.max_lookahead_dist}, + {"min_lookahead_dist", config_.min_lookahead_dist}, + {"speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio}, + {"emergency_stop_distance",config_.emergency_stop_distance}, + {"speed_thres_traveling_direction", config_.speed_thres_traveling_direction}, + {"dist_front_rear_wheels", config_.dist_front_rear_wheels}, + {"dt", config_.dt}, + {"integrator_max_pp", config_.integrator_max_pp}, + {"integrator_min_pp", config_.integrator_min_pp}, + {"Ki_pp", config_.ki_pp}, + }, parameters); + + auto error_int = update_params({ + {"cmd_timestamp_ms", config_.cmd_timestamp_ms}, + }, parameters); + + auto error_bool = update_params({ + {"is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point}, + {"is_delay_compensation",config_.is_delay_compensation}, + {"is_integrator_enabled", config_.is_integrator_enabled}, + {"enable_max_adjustment_filter", config_.enable_max_adjustment_filter}, + {"enable_max_accel_filter", config_.enable_max_accel_filter}, + }, parameters); + + // vehicle_id, control_plugin_shutdown_timeout and control_plugin_ignore_initial_inputs are not updated as they are global params + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error_double && !error_int && !error_bool; + + return result; + + } + + carma_ros2_utils::CallbackReturn PlatooningControlPlugin::on_configure_plugin() + { + // Reset config + config_ = PlatooningControlPluginConfig(); + + // Load parameters + get_parameter("stand_still_headway_m", config_.stand_still_headway_m); + get_parameter("max_accel_mps2", config_.max_accel_mps2); + get_parameter("kp", config_.kp); + get_parameter("kd", config_.kd); + get_parameter("ki", config_.ki); + get_parameter("max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep); + get_parameter("min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep); + get_parameter("adjustment_cap_mps", config_.adjustment_cap_mps); + get_parameter("cmd_timestamp_ms", config_.cmd_timestamp_ms); + get_parameter("integrator_max", config_.integrator_max); + get_parameter("integrator_min", config_.integrator_min); + + get_parameter("vehicle_id", config_.vehicle_id); + get_parameter("control_plugin_shutdown_timeout", config_.shutdown_timeout); + get_parameter("control_plugin_ignore_initial_inputs", config_.ignore_initial_inputs); + get_parameter("enable_max_adjustment_filter", config_.enable_max_adjustment_filter); + get_parameter("enable_max_accel_filter", config_.enable_max_accel_filter); + + //Pure Pursuit params + get_parameter("vehicle_response_lag", config_.vehicle_response_lag); + get_parameter("maximum_lookahead_distance", config_.max_lookahead_dist); + get_parameter("minimum_lookahead_distance", config_.min_lookahead_dist); + get_parameter("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio); + get_parameter("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point); + get_parameter("is_delay_compensation", config_.is_delay_compensation); + get_parameter("emergency_stop_distance", config_.emergency_stop_distance); + get_parameter("speed_thres_traveling_direction", config_.speed_thres_traveling_direction); + get_parameter("dist_front_rear_wheels", config_.dist_front_rear_wheels); + + get_parameter("dt", config_.dt); + get_parameter("integrator_max_pp", config_.integrator_max_pp); + get_parameter("integrator_min_pp", config_.integrator_min_pp); + get_parameter("Ki_pp", config_.ki_pp); + get_parameter("is_integrator_enabled", config_.is_integrator_enabled); + + + RCLCPP_INFO_STREAM(rclcpp::get_logger("platooning_control"), "Loaded Params: " << config_); + + // create config for pure_pursuit worker + pure_pursuit::Config cfg{ + config_.min_lookahead_dist, + config_.max_lookahead_dist, + config_.speed_to_lookahead_ratio, + config_.is_interpolate_lookahead_point, + config_.is_delay_compensation, + config_.emergency_stop_distance, + config_.speed_thres_traveling_direction, + config_.dist_front_rear_wheels, + }; + + pure_pursuit::IntegratorConfig i_cfg; + i_cfg.dt = config_.dt; + i_cfg.integrator_max_pp = config_.integrator_max_pp; + i_cfg.integrator_min_pp = config_.integrator_min_pp; + i_cfg.Ki_pp = config_.ki_pp; + i_cfg.integral = 0.0; // accumulator of integral starts from 0 + i_cfg.is_integrator_enabled = config_.is_integrator_enabled; + + pp_ = std::make_shared(cfg, i_cfg); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&PlatooningControlPlugin::parameter_update_callback, this, std_ph::_1)); + + + // Trajectory Plan Subscriber + trajectory_plan_sub_ = create_subscription("platooning_control/plan_trajectory", 1, + std::bind(&PlatooningControlPlugin::current_trajectory_callback, this, std_ph::_1)); + + // Platoon Info Subscriber + platoon_info_sub_ = create_subscription("platoon_info", 1, std::bind(&PlatooningControlPlugin::platoon_info_cb, this, std_ph::_1)); + + + //Control Publishers + platoon_info_pub_ = create_publisher("platooning_info", 1); + + + // Return success if everthing initialized successfully + return CallbackReturn::SUCCESS; + } + + + autoware_msgs::msg::ControlCommandStamped PlatooningControlPlugin::generate_command() + { + + autoware_msgs::msg::ControlCommandStamped ctrl_msg; + if (!current_trajectory_ || !current_pose_ || !current_twist_) + return ctrl_msg; + + // 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. + double current_time_ms = this->now().nanoseconds() / 1e6; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "current_time_ms = " << current_time_ms << ", prev_input_time_ms_ = " << prev_input_time_ms_ << ", input counter = " << consecutive_input_counter_); + + if(current_time_ms - prev_input_time_ms_ > config_.shutdown_timeout) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "returning due to timeout."); + consecutive_input_counter_ = 0; + return ctrl_msg; + } + + // 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_.ignore_initial_inputs) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "returning due to first data input"); + return ctrl_msg; + } + + carma_planning_msgs::msg::TrajectoryPlanPoint second_trajectory_point = current_trajectory_.get().trajectory_points[1]; + + trajectory_speed_ = get_trajectory_speed(current_trajectory_.get().trajectory_points); + + ctrl_msg = generate_control_signals(second_trajectory_point, current_pose_.get(), current_twist_.get()); + + return ctrl_msg; + + } + + void PlatooningControlPlugin::platoon_info_cb(const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg) + { + + platoon_leader_.staticId = msg->leader_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; + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader id: " << platoon_leader_.staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader pose: " << platoon_leader_.vehiclePosition); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader cmd speed: " << platoon_leader_.commandSpeed); + + carma_planning_msgs::msg::PlatooningInfo platooning_info_msg = *msg; + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "platooning_info_msg.actual_gap: " << platooning_info_msg.actual_gap); + + if (platooning_info_msg.actual_gap > 5.0) + { + platooning_info_msg.actual_gap -= 5.0; // TODO: temporary: should be vehicle length + } + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "platooning_info_msg.actual_gap: " << platooning_info_msg.actual_gap); + // platooing_info_msg.desired_gap = pcw_.desired_gap_; + // platooing_info_msg.actual_gap = pcw_.actual_gap_; + pcw_.actual_gap_ = platooning_info_msg.actual_gap; + pcw_.desired_gap_ = platooning_info_msg.desired_gap; + + platooning_info_msg.host_cmd_speed = pcw_.speedCmd_; + platoon_info_pub_->publish(platooning_info_msg); + } + + autoware_msgs::msg::ControlCommandStamped PlatooningControlPlugin::generate_control_signals(const carma_planning_msgs::msg::TrajectoryPlanPoint& first_trajectory_point, const geometry_msgs::msg::PoseStamped& current_pose, const geometry_msgs::msg::TwistStamped& current_twist) + { + pcw_.set_current_speed(trajectory_speed_); //TODO why this and not the actual vehicle speed? Method name suggests different use than this. + // pcw_.set_current_speed(current_twist_.get()); + pcw_.set_leader(platoon_leader_); + pcw_.generate_speed(first_trajectory_point); + + motion::control::controller_common::State state_tf = convert_state(current_pose, current_twist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Forced from frame_id: " << state_tf.header.frame_id << ", into: " << current_trajectory_.get().header.frame_id); + + current_trajectory_.get().header.frame_id = state_tf.header.frame_id; + + auto autoware_traj_plan = basic_autonomy::waypoint_generation::process_trajectory_plan(current_trajectory_.get(), config_.vehicle_response_lag); + + pp_->set_trajectory(autoware_traj_plan); + const auto cmd{pp_->compute_command(state_tf)}; + + auto steer_cmd = cmd.front_wheel_angle_rad; //autoware sets the front wheel angle as the calculated steer. https://github.com/usdot-fhwa-stol/autoware.auto/blob/3450f94fa694f51b00de272d412722d65a2c2d3e/AutowareAuto/src/control/pure_pursuit/src/pure_pursuit.cpp#L88 + + autoware_msgs::msg::ControlCommandStamped ctrl_msg = compose_ctrl_cmd(pcw_.speedCmd_, steer_cmd); + + return ctrl_msg; + } + + motion::motion_common::State PlatooningControlPlugin::convert_state(const geometry_msgs::msg::PoseStamped& pose, const geometry_msgs::msg::TwistStamped& twist) const + { + motion::motion_common::State state; + state.header = pose.header; + state.state.x = pose.pose.position.x; + state.state.y = pose.pose.position.y; + state.state.z = pose.pose.position.z; + state.state.heading.real = pose.pose.orientation.w; + state.state.heading.imag = pose.pose.orientation.z; + + state.state.longitudinal_velocity_mps = twist.twist.linear.x; + return state; + } + + void PlatooningControlPlugin::current_trajectory_callback(const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr tp) + { + if (tp->trajectory_points.size() < 2) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("platooning_control"), "PlatooningControlPlugin cannot execute trajectory as only 1 point was provided"); + return; + } + + current_trajectory_ = *tp; + prev_input_time_ms_ = this->now().nanoseconds() / 1000000; + ++consecutive_input_counter_; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "New trajectory plan #" << consecutive_input_counter_ << " at time " << prev_input_time_ms_); + rclcpp::Time tp_time(tp->header.stamp); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "tp header time = " << tp_time.nanoseconds() / 1000000); + } + + geometry_msgs::msg::TwistStamped PlatooningControlPlugin::compose_twist_cmd(double linear_vel, double angular_vel) + { + geometry_msgs::msg::TwistStamped cmd_twist; + cmd_twist.twist.linear.x = linear_vel; + cmd_twist.twist.angular.z = angular_vel; + cmd_twist.header.stamp = this->now(); + return cmd_twist; + } + + autoware_msgs::msg::ControlCommandStamped PlatooningControlPlugin::compose_ctrl_cmd(double linear_vel, double steering_angle) + { + autoware_msgs::msg::ControlCommandStamped cmd_ctrl; + cmd_ctrl.header.stamp = this->now(); + cmd_ctrl.cmd.linear_velocity = linear_vel; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "ctrl command speed " << cmd_ctrl.cmd.linear_velocity); + cmd_ctrl.cmd.steering_angle = steering_angle; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "ctrl command steering " << cmd_ctrl.cmd.steering_angle); + + return cmd_ctrl; + } + + bool PlatooningControlPlugin::get_availability() { + return true; // TODO for user implement actual check on availability if applicable to plugin + } + + std::string PlatooningControlPlugin::get_version_id() { + return "v1.0"; + } + + // extract maximum speed of trajectory + double PlatooningControlPlugin::get_trajectory_speed(const 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 = rclcpp::Time((trajectory_points[trajectory_points.size()-1].target_time)).seconds() - rclcpp::Time(trajectory_points[0].target_time).nanoseconds(); + + double avg_speed = d1/t1; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "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 = rclcpp::Time((trajectory_points[i + 1].target_time)).seconds() - rclcpp::Time(trajectory_points[i].target_time).seconds(); + double v = d/t; + if(v > trajectory_speed) + { + trajectory_speed = v; + } + } + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "trajectory speed: " << trajectory_speed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "avg trajectory speed: " << avg_speed); + + return avg_speed; //TODO: why are 2 speeds being calculated? Which should be returned? + + } + + +} // platooning_control + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(platooning_control::PlatooningControlPlugin) diff --git a/platooning_control/src/platoon_control_worker.cpp b/platooning_control/src/platooning_control_worker.cpp similarity index 51% rename from platooning_control/src/platoon_control_worker.cpp rename to platooning_control/src/platooning_control_worker.cpp index b1138c6ee6..603cba9cd6 100644 --- a/platooning_control/src/platoon_control_worker.cpp +++ b/platooning_control/src/platooning_control_worker.cpp @@ -1,6 +1,6 @@ /*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. +* Copyright (C) 2024 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,36 +16,23 @@ ------------------------------------------------------------------------------*/ -#include "platoon_control_worker.h" +#include "platooning_control/platooning_control_worker.hpp" -namespace platoon_control +namespace platooning_control { - - PlatoonControlWorker::PlatoonControlWorker() + + PlatooningControlWorker::PlatooningControlWorker() { pid_ctrl_ = PIDController(); - pp_ = PurePursuit(); - - } - void PlatoonControlWorker::updateConfigParams(PlatooningControlPluginConfig new_config) - { - ctrl_config_ = new_config; - pid_ctrl_.config_ = new_config; - pp_.config_ = new_config; } - double PlatoonControlWorker::getLastSpeedCommand() const { + double PlatooningControlWorker::get_last_speed_command() const { return speedCmd_; } - void PlatoonControlWorker::setCurrentPose(const geometry_msgs::PoseStamped msg) - { - current_pose_ = msg.pose; - } - - void PlatoonControlWorker::generateSpeed(const cav_msgs::TrajectoryPlanPoint& point) + void PlatooningControlWorker::generate_speed(const carma_planning_msgs::msg::TrajectoryPlanPoint& point) { double speed_cmd = 0; @@ -57,83 +44,83 @@ namespace platoon_control } PlatoonLeaderInfo leader = platoon_leader; - if(leader.staticId != "" && leader.staticId != ctrl_config_.vehicleID) + if(!leader.staticId.empty() && leader.staticId != ctrl_config_->vehicle_id) { double controllerOutput = 0.0; double leaderCurrentPosition = leader.vehiclePosition; - ROS_DEBUG_STREAM("The current leader position is " << leaderCurrentPosition); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The current leader position is " << leaderCurrentPosition); double desiredHostPosition = leaderCurrentPosition - desired_gap_; - ROS_DEBUG_STREAM("desiredHostPosition = " << desiredHostPosition); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "desiredHostPosition = " << desiredHostPosition); double hostVehiclePosition = leaderCurrentPosition - actual_gap_; - ROS_DEBUG_STREAM("hostVehiclePosition = " << hostVehiclePosition); - + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "hostVehiclePosition = " << hostVehiclePosition); + controllerOutput = pid_ctrl_.calculate(desiredHostPosition, 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); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Adjusted Speed Cmd = " << adjSpeedCmd << "; Controller Output = " << controllerOutput + << "; Leader CmdSpeed= " << leader.commandSpeed << "; Adjustment Cap " << ctrl_config_->adjustment_cap_mps); // 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"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "A speed command is generated from command generator: " << speed_cmd << " m/s"); - if(enableMaxAdjustmentFilter) + if(ctrl_config_->enable_max_adjustment_filter) { - 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; + if(speed_cmd > leader.commandSpeed + ctrl_config_->adjustment_cap_mps) { + speed_cmd = leader.commandSpeed + ctrl_config_->adjustment_cap_mps; + } else if(speed_cmd < leader.commandSpeed - ctrl_config_->adjustment_cap_mps) { + speed_cmd = leader.commandSpeed - ctrl_config_->adjustment_cap_mps; } - ROS_DEBUG_STREAM("The adjusted cmd speed after max adjustment cap is " << speed_cmd << " m/s"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The adjusted cmd speed after max adjustment cap is " << speed_cmd << " m/s"); } } - else if (leader.staticId == ctrl_config_.vehicleID) + else if (leader.staticId == ctrl_config_->vehicle_id) { - ROS_DEBUG_STREAM("Host vehicle is the leader"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Host vehicle is the leader"); speed_cmd = currentSpeed; - if(enableMaxAdjustmentFilter) + if(ctrl_config_->enable_max_adjustment_filter) { - if(speed_cmd > ctrl_config_.adjustmentCap) + if(speed_cmd > ctrl_config_->adjustment_cap_mps) { - speed_cmd = ctrl_config_.adjustmentCap; - } + speed_cmd = ctrl_config_->adjustment_cap_mps; + } - ROS_DEBUG_STREAM("The adjusted leader cmd speed after max adjustment cap is " << speed_cmd << " m/s"); - } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The adjusted leader cmd speed after max adjustment cap is " << speed_cmd << " m/s"); + } pid_ctrl_.reset(); } - else + 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"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "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(ctrl_config_->enable_max_accel_filter) { + + double max = lastCmdSpeed + (ctrl_config_->max_accel_mps2 * (ctrl_config_->cmd_timestamp_ms / 1000.0)); + double min = lastCmdSpeed - (ctrl_config_->max_accel_mps2 * (ctrl_config_->cmd_timestamp_ms / 1000.0)); if(speed_cmd > max) { - 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"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The speed command after max accel cap is: " << speed_cmd << " m/s"); } speedCmd_ = speed_cmd; @@ -142,27 +129,13 @@ namespace platoon_control } - void PlatoonControlWorker::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 PlatoonControlWorker::setLeader(const PlatoonLeaderInfo& leader){ + void PlatooningControlWorker::set_leader(const PlatoonLeaderInfo& leader){ platoon_leader = leader; } - void PlatoonControlWorker::setCurrentSpeed(double speed){ + void PlatooningControlWorker::set_current_speed(double speed){ currentSpeed = speed; - - } - - - -} + } +} \ No newline at end of file diff --git a/platooning_control/src/pure_pursuit.cpp b/platooning_control/src/pure_pursuit.cpp deleted file mode 100644 index c08462fcdd..0000000000 --- a/platooning_control/src/pure_pursuit.cpp +++ /dev/null @@ -1,160 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#include "pure_pursuit.h" -#include -#include - - - -namespace platoon_control -{ - 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); - 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); - 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/platooning_control/test/mynode.test b/platooning_control/test/mynode.test deleted file mode 100644 index d53b9613aa..0000000000 --- a/platooning_control/test/mynode.test +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - diff --git a/platooning_control/test/test_control.cpp b/platooning_control/test/test_control.cpp index d1b73e6938..d74613e1fd 100644 --- a/platooning_control/test/test_control.cpp +++ b/platooning_control/test/test_control.cpp @@ -1,52 +1,195 @@ +/* + * Copyright (C) 2024 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. + */ -/*------------------------------------------------------------------------------ -* 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.h" #include -#include +#include +#include +#include +#include +#include "platooning_control/platooning_control.hpp" -TEST(PlatoonControlPluginTest, test2) +TEST(PlatooningControlPluginTest, test_2) { - cav_msgs::TrajectoryPlan tp; - cav_msgs::TrajectoryPlanPoint point1; - point1.x = 1.0; - point1.y = 1.0; - + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); - cav_msgs::TrajectoryPlanPoint point2; - point2.x = 10.0; - point2.y = 10.0; + EXPECT_TRUE(worker_node->get_availability()); + EXPECT_EQ(worker_node->get_version_id(), "v1.0"); - cav_msgs::TrajectoryPlanPoint point3; - point3.x = 20.0; - point3.y = 20.0; + EXPECT_EQ(worker_node->generate_command().cmd.linear_velocity, 0.0); - tp.trajectory_points = {point1, point2, point3}; +} + +TEST(PlatooningControlPluginTest, test_convert_state) +{ + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.pose.position.x = 0.0; + pose_msg.pose.position.y = 0.0; + pose_msg.pose.position.z = 0.0; + pose_msg.pose.orientation.w = 1.0; + pose_msg.pose.orientation.z = 0.0; + geometry_msgs::msg::TwistStamped twist_msg; + twist_msg.twist.linear.x = 0.0; + + auto converted_state = worker_node->convert_state(pose_msg, twist_msg); + EXPECT_EQ(converted_state.state.x, pose_msg.pose.position.x); + EXPECT_EQ(converted_state.state.heading.imag, pose_msg.pose.orientation.z); - platoon_control::PlatoonControlPlugin pc; - pc.current_speed_ = 5; - cav_msgs::TrajectoryPlanPoint out = pc.getLookaheadTrajectoryPoint(tp); - EXPECT_EQ(out.x, 10.0); } +TEST(PlatooningControlPluginTest, test_compose_twist_cmd) +{ + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + auto linear_vel = 1.0; + auto angular_vel = 2.0; + + auto twist_cmd = worker_node->compose_twist_cmd(linear_vel,angular_vel); + + EXPECT_EQ(twist_cmd.twist.linear.x, linear_vel); + EXPECT_EQ(twist_cmd.twist.angular.z, angular_vel); +} + +namespace platooning_control +{ + TEST(PlatooningControlPluginTest, test_current_trajectory_callback) + { + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + carma_planning_msgs::msg::TrajectoryPlan traj_plan; + + carma_planning_msgs::msg::TrajectoryPlanPoint point1; + point1.x = 30.0; + point1.y = 20.0; + + carma_planning_msgs::msg::TrajectoryPlanPoint point2; + point2.x = 50.0; + point2.y = 60.0; + + + traj_plan.trajectory_points = {point1, point2}; + + worker_node->current_trajectory_callback(std::make_unique(traj_plan)); + EXPECT_EQ(worker_node->current_trajectory_.get().trajectory_points.size(), 2); + + } + + TEST(PlatooningControlPluginTest, test_platoon_info_cb) + { + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + worker_node->configure(); + worker_node->activate(); + + carma_planning_msgs::msg::PlatooningInfo msg; + msg.leader_id = "id"; + msg.leader_downtrack_distance = 2.0; + msg.leader_cmd_speed = 1.0; + msg.host_platoon_position=1; + + worker_node->platoon_info_cb(std::make_shared(msg)); + + // Test parameter update + rclcpp::ParameterValue bool_val(false); + rclcpp::Parameter param("enable_max_accel_filter", bool_val); + std::vector param_vec = {param}; + + worker_node->parameter_update_callback(param_vec); + EXPECT_FALSE(worker_node->config_.enable_max_accel_filter); + } + + TEST(PlatooningControlPluginTest, test_get_trajectory_speed) + { + + carma_planning_msgs::msg::TrajectoryPlanPoint point; + point.x = 30.0; + point.y = 15.0; + point.target_time.sec = 0; + + carma_planning_msgs::msg::TrajectoryPlanPoint point2; + point2.x = 50.0; + point2.y = 60.0; + point2.target_time.sec = 1.0; + + carma_planning_msgs::msg::TrajectoryPlanPoint point3; + point3.x = 55.0; + point3.y = 60.0; + point3.target_time.sec = 10.0; + + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + + EXPECT_NEAR(worker_node->get_trajectory_speed({point,point2,point3}), 5.1, 0.1); + } + + TEST(PlatooningControlPluginTest, test_generate_controls) + { + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + worker_node->configure(); + worker_node->activate(); + + platooning_control::PlatoonLeaderInfo msg; + msg.staticId = "id"; + msg.commandSpeed = 2.0; + msg.vehicleSpeed = 2.0; + msg.vehiclePosition = 0.5; + msg.leaderIndex = 0; + msg.NumberOfVehicleInFront=0; + + worker_node->platoon_leader_ = msg; + + worker_node->trajectory_speed_ = 4.47; + + carma_planning_msgs::msg::TrajectoryPlanPoint trajectory_point; + trajectory_point.x = 50.0; + trajectory_point.y = 60.0; + + geometry_msgs::msg::PoseStamped current_pose; + current_pose.pose.position.x = 0.0; + current_pose.pose.position.y = 0.0; + + geometry_msgs::msg::TwistStamped current_twist; + current_twist.twist.linear.x = 0.0; + + carma_planning_msgs::msg::TrajectoryPlanPoint point1; + point1.x = 50.0; + point1.y = 60.0; + carma_planning_msgs::msg::TrajectoryPlanPoint point2; + point2.x = 52.0; + point2.y = 60.0; + + carma_planning_msgs::msg::TrajectoryPlan traj_plan; + traj_plan.trajectory_points = {point1, point2}; + + worker_node->current_trajectory_callback(std::make_unique(traj_plan)); + auto control_cmd = worker_node->generate_control_signals(trajectory_point, current_pose, current_twist); + EXPECT_NEAR(4.47, control_cmd.cmd.linear_velocity, 0.5); + } +} \ No newline at end of file diff --git a/platooning_control/test/test_main.cpp b/platooning_control/test/test_main.cpp index 3bbdd41035..4be96717d8 100644 --- a/platooning_control/test/test_main.cpp +++ b/platooning_control/test/test_main.cpp @@ -1,29 +1,33 @@ +/* + * Copyright (C) 2024 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 -/*------------------------------------------------------------------------------ -* 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. -------------------------------------------------------------------------------*/ +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); -#include "pid_controller.h" -#include -#include + //Initialize ROS + rclcpp::init(argc, argv); + bool success = RUN_ALL_TESTS(); -// Run all the tests -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/platooning_control/test/test_mynode.cpp b/platooning_control/test/test_mynode.cpp deleted file mode 100644 index ee1d555c62..0000000000 --- a/platooning_control/test/test_mynode.cpp +++ /dev/null @@ -1,72 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#include -#include -#include -#include -#include -#include -#include -#include "platoon_control.h" - - -// These tests has been temporarily disabled to support Continuous Improvement (CI) processes. -// Related GitHub Issue: - -// Declare a test -// TEST(TestSuite, testCase1) -// { -// ros::NodeHandle nh = ros::NodeHandle(); -// ros::Publisher traj_pub_ = nh.advertise("platoon_control/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; -} diff --git a/platooning_control/test/test_pid.cpp b/platooning_control/test/test_pid.cpp index 085674f977..27b38dd774 100644 --- a/platooning_control/test/test_pid.cpp +++ b/platooning_control/test/test_pid.cpp @@ -1,57 +1,57 @@ +/* + * Copyright (C) 2024 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. + */ -/*------------------------------------------------------------------------------ -* 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 +#include +#include +#include +#include + +#include "platooning_control/platooning_control.hpp" TEST(PIDControllerTest, test1) { - platoon_control::PIDController pid; + platooning_control::PIDController pid; double res = pid.calculate(40, 38); - EXPECT_EQ(-9, res); + EXPECT_EQ(-9, res);; } -// These tests has been temporarily disabled to support Continuous Improvement (CI) processes. -// Related GitHub Issue: - -// TEST(PIDControllerTest, test2) -// { -// platoon_control::PIDController pid; -// double res = pid.calculate(20, 300); -// EXPECT_EQ(100, res); -// } +TEST(PIDControllerTest, test2) +{ + platooning_control::PIDController pid; + double res = pid.calculate(20, 300); + EXPECT_EQ(2, res); +} -// TEST(PIDControllerTest, test3) -// { -// platoon_control::PIDController pid; -// double res = pid.calculate(300, 20); -// EXPECT_EQ(-100, res); -// } +TEST(PIDControllerTest, test3) +{ + platooning_control::PIDController pid; + double res = pid.calculate(300, 20); + EXPECT_EQ(-10, res); +} -// TEST(PIDControllerTest, test4) -// { -// platoon_control::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); -// } +TEST(PIDControllerTest, test4) +{ + platooning_control::PIDController pid; + pid.reset(); + double res = pid.calculate(200, 20); + EXPECT_EQ(-10, res); + double res2 = pid.calculate(500,25); + EXPECT_EQ(-10, res2); + double res3 = pid.calculate(25,500); + EXPECT_EQ(2, res3); +} \ No newline at end of file diff --git a/platooning_control/test/test_pure.cpp b/platooning_control/test/test_pure.cpp index 76eeb19c73..d2e03cd0c7 100644 --- a/platooning_control/test/test_pure.cpp +++ b/platooning_control/test/test_pure.cpp @@ -1,47 +1,82 @@ +/* + * Copyright (C) 2024 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. + */ -/*------------------------------------------------------------------------------ -* 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 +#include +#include +#include +#include -TEST(PurePursuitTest, test_filter) -{ - PlatooningControlPluginConfig config; - config.lowpassGain = 0.5; - - platoon_control::PurePursuit pp; - pp.config_ = config; - double new_angle = pp.lowPassfilter(3.0, 0, config.lowpassGain); - EXPECT_EQ(1.5, new_angle); -} - -TEST(PurePursuitTest, test1) +#include "platooning_control/platooning_control.hpp" + + +TEST(PurePursuitTest, sanity_check) { + rclcpp::NodeOptions options; + auto node = std::make_shared(options); + node->configure(); + node->activate(); + + carma_planning_msgs::msg::TrajectoryPlanPoint tpp, tpp2, tpp3; + tpp.x = 100; + tpp.y = 100; + tpp.target_time = rclcpp::Time(1.0*1e9); // 14.14 m/s + + tpp2.x = 110; + tpp2.y = 110; + tpp2.target_time = rclcpp::Time(2.0*1e9); // 14.14 m/s + + tpp3.x = 120; + tpp3.y = 120; + tpp3.target_time = rclcpp::Time(3.0*1e9); // 14.14 m/s + + carma_planning_msgs::msg::TrajectoryPlan plan; + plan.initial_longitudinal_velocity = 14.14; + + motion::control::controller_common::State state_tf; + auto converted_time_now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + + state_tf.header.stamp = rclcpp::Time(converted_time_now*1e9); + + state_tf.state.heading.real = 3.14 / 2; + state_tf.state.heading.imag = 1.0; + + state_tf.state.x = 0; + state_tf.state.y = 0; + state_tf.state.longitudinal_velocity_mps = 4.0; //arbitrary speed for first point + plan.header.frame_id = state_tf.header.frame_id; + plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration(1.0*1e9); + + plan.trajectory_points = { tpp, tpp2, tpp3 }; + + auto traj = basic_autonomy::waypoint_generation::process_trajectory_plan(plan, 0.0); + node->pp_->set_trajectory(traj); + + const auto cmd{node->pp_->compute_command(state_tf)}; - cav_msgs::TrajectoryPlanPoint point; - point.x = 1.0; - point.y = 1.0; - point.target_time = ros::Time(1.0); - platoon_control::PurePursuit pp; - pp.calculateSteer(point); - EXPECT_EQ(0, pp.steering_angle_); + ASSERT_NEAR(cmd.front_wheel_angle_rad, -0.294355, 0.005); + ASSERT_NEAR(cmd.long_accel_mps2, 0.311803, 0.005); + ASSERT_NEAR(cmd.rear_wheel_angle_rad, 0, 0.001); + ASSERT_NEAR(cmd.velocity_mps, 14.14, 0.01); + auto steer_cmd = cmd.front_wheel_angle_rad; -} + auto speed_cmd = 5.0; + auto converted_cmd = node->compose_ctrl_cmd(speed_cmd, steer_cmd); + ASSERT_NEAR(converted_cmd.cmd.linear_velocity, speed_cmd, 0.01); + ASSERT_NEAR(converted_cmd.cmd.steering_angle, cmd.front_wheel_angle_rad, 0.001); +} \ No newline at end of file diff --git a/platooning_control/test/test_worker.cpp b/platooning_control/test/test_worker.cpp index 4d8e8a2fa6..04a027e40b 100644 --- a/platooning_control/test/test_worker.cpp +++ b/platooning_control/test/test_worker.cpp @@ -1,132 +1,147 @@ +/* + * Copyright (C) 2024 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. + */ -/*------------------------------------------------------------------------------ -* 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_worker.h" #include -#include +#include +#include +#include +#include -TEST(PlatoonControlWorkerTest, test1) +#include "platooning_control/platooning_control_worker.hpp" + + +TEST(PlatooningControlWorkerTest, test1) { - platoon_control::PlatoonControlWorker pcw; - cav_msgs::TrajectoryPlanPoint point; + platooning_control::PlatooningControlWorker pcw; + pcw.ctrl_config_ = std::make_shared(); + carma_planning_msgs::msg::TrajectoryPlanPoint point; point.x = 1.0; point.y = 2.0; - pcw.generateSpeed(point); + pcw.generate_speed(point); EXPECT_NEAR(0, pcw.speedCmd_, 0.1); } -TEST(PlatoonControlWorkerTest, test11) +TEST(PlatooningControlWorkerTest, test11) { - platoon_control::PlatoonLeaderInfo leader; - platoon_control::PlatoonControlWorker pcw; + platooning_control::PlatoonLeaderInfo leader; + platooning_control::PlatooningControlWorker pcw; + pcw.ctrl_config_ = std::make_shared(); leader.staticId = ""; leader.leaderIndex = 0; leader.NumberOfVehicleInFront = 1; - pcw.setLeader(leader); - cav_msgs::TrajectoryPlanPoint point; + pcw.set_leader(leader); + carma_planning_msgs::msg::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); + pcw.generate_speed(point); + EXPECT_NEAR(10.0, pcw.get_last_speed_command(), 0.5); } -TEST(PlatoonControlWorkerTest, test2) +TEST(PlatooningControlWorkerTest, test2) { - platoon_control::PlatoonControlWorker pcw; - platoon_control::PlatoonLeaderInfo leader; + platooning_control::PlatooningControlWorker pcw; + platooning_control::PlatooningControlPluginConfig config; + pcw.ctrl_config_ = std::make_shared(config); + platooning_control::PlatoonLeaderInfo leader; leader.commandSpeed = 10; leader.vehicleSpeed = 10; leader.vehiclePosition = 50; leader.staticId = "id"; leader.leaderIndex = 0; leader.NumberOfVehicleInFront = 1; - pcw.setLeader(leader); + pcw.set_leader(leader); - cav_msgs::TrajectoryPlanPoint point; + carma_planning_msgs::msg::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); + pcw.generate_speed(point); + EXPECT_NEAR(9.75, pcw.get_last_speed_command(), 0.5); - cav_msgs::TrajectoryPlanPoint point2; + carma_planning_msgs::msg::TrajectoryPlanPoint point2; point2.x = 30.0; point2.y = 40.0; - pcw.generateSpeed(point2); - EXPECT_NEAR(10, pcw.getLastSpeedCommand(), 0.5); + pcw.generate_speed(point2); + EXPECT_NEAR(10, pcw.get_last_speed_command(), 0.5); - cav_msgs::TrajectoryPlanPoint point3; + carma_planning_msgs::msg::TrajectoryPlanPoint point3; point3.x = 50.0; point3.y = 60.0; - pcw.generateSpeed(point3); - EXPECT_NEAR(10.25, pcw.getLastSpeedCommand(), 0.5); + pcw.generate_speed(point3); + EXPECT_NEAR(10.25, pcw.get_last_speed_command(), 0.5); + + config.enable_max_adjustment_filter = false; + config.vehicle_id = "id"; + pcw.generate_speed(point3); + EXPECT_NEAR(9.5, pcw.get_last_speed_command(), 0.5); + + config.vehicle_id = leader.staticId; + config.adjustment_cap_mps = 5.0; + config.enable_max_adjustment_filter = true; + pcw.set_current_speed(4.47); + carma_planning_msgs::msg::TrajectoryPlanPoint point4; + point4.x = 55.0; + point4.y = 60.0; + pcw.generate_speed(point4); + EXPECT_NEAR(9.0, pcw.get_last_speed_command(), 0.5); + } -TEST(PlatoonControlWorkerTest, test3) +TEST(PlatooningControlWorkerTest, test3) { - platoon_control::PlatoonControlWorker pcw; - platoon_control::PlatoonLeaderInfo leader; + platooning_control::PlatooningControlWorker pcw; + platooning_control::PlatooningControlPluginConfig config; + pcw.ctrl_config_ = std::make_shared(config); + platooning_control::PlatoonLeaderInfo leader; leader.commandSpeed = 10; leader.vehicleSpeed = 10; leader.vehiclePosition = 50; leader.staticId = "id"; leader.leaderIndex = 0; leader.NumberOfVehicleInFront = 2; - pcw.setLeader(leader); + pcw.set_leader(leader); - cav_msgs::TrajectoryPlanPoint point; + carma_planning_msgs::msg::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); + pcw.generate_speed(point); + EXPECT_NEAR(10.25, pcw.get_last_speed_command(), 0.5); - cav_msgs::TrajectoryPlanPoint point2; + carma_planning_msgs::msg::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); + pcw.generate_speed(point2); + EXPECT_NEAR(10.5, pcw.get_last_speed_command(), 0.5); - cav_msgs::TrajectoryPlanPoint point3; + carma_planning_msgs::msg::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); + pcw.generate_speed(point3); + EXPECT_NEAR(10.25, pcw.get_last_speed_command(), 0.5); - } - -TEST(PlatoonControlWorkerTest, test_steer) -{ - platoon_control::PlatoonControlWorker 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_control_ihp/CMakeLists.txt b/platooning_control_ihp/CMakeLists.txt deleted file mode 100644 index 342b4ebf15..0000000000 --- a/platooning_control_ihp/CMakeLists.txt +++ /dev/null @@ -1,128 +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. - -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() - -# This test has been temporarily disabled to support Continuous Improvement (CI) processes. -# Related GitHub Issue: - -#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/README.md b/platooning_control_ihp/README.md deleted file mode 100644 index f792e512f5..0000000000 --- a/platooning_control_ihp/README.md +++ /dev/null @@ -1,10 +0,0 @@ -# platoon_control_ihp - -Control plugin for the Integrated Highway Prototype 2 (IHP2) implementation which allows platooning to maintain the gap; moreover, generates longitudinal and lateral control commands to follow the trajectory. The difference between this plugin, and platoon_control is that this plugin includes logic to open or close the gap between platoon members, to allow for a new member to join or an existing memeber to exit the platoon. -Note despite the name, as of Aug 2022 testing for this IHP2 implementation was only up to 35mph on a closed test track per limitations of the CARMA Platform system. - -Overview -https://usdot-carma.atlassian.net/wiki/spaces/CUCS/pages/2392981532/Basic+Travel+and+IHP - -Design -https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2138767361/IHP2+Platooning+Plugin diff --git a/platooning_control_ihp/config/parameters.yaml b/platooning_control_ihp/config/parameters.yaml deleted file mode 100644 index f788279d39..0000000000 --- a/platooning_control_ihp/config/parameters.yaml +++ /dev/null @@ -1,23 +0,0 @@ -standStillHeadway: 12.0 # Standstill gap between vehicles (m) -maxAccel: 1.5 # Maximum acceleration absolute value used in controller filters (m/s^2) -Kp: 0.4 # Proportional weight for PID controller -Kd: 0.05 # Derivative Weight for PID controller -Ki: 0.0 # Integral weight for PID controller -maxValue: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) -minValue: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) -cmdTmestamp: 100 # Timestep to calculate ctrl commands (ms) -adjustmentCap: 11.0 # Adjustment cap for speed command (m/s) -dt: 0.1 # Timestep to calculate ctrl commands (s) -integratorMax: 100 # Max limit for integrator term -integratorMin: -100 # Min limit for integrator term -lowpassGain: 1.0 #Lowpass filter gain -lookaheadRatio: 1.6 # Ratio to calculate lookahead distance -minLookaheadDist: 15.0 # Min lookahead distance (m) -vehicleLength: 5.0 # Average length of a vehicle (m) -ss_theta: 1.5 # Minimum speed to be considered as moving (m/s) -standstill: 2.0 # Extra time needed to reacte to traffic sceanrios when vehicle is standstill (not moving) (s). -inter_tau: 1.5 # Inter-platoon time gap, refer to bumper to bumper gap time (s) -intra_tau: 0.6 #Intra-platoon time gao, refer to bumper to bumper gap time (s) -gap_weight: 0.9 # Weighted ratio for time-gap based calculation, -time_step: 5 # time step ga regulation algorithm uses to calculate desired position (s). -test_front_join: false #Flag to enable/disable front join functionality with two vehicles. diff --git a/platooning_control_ihp/include/pid_controller.h b/platooning_control_ihp/include/pid_controller.h deleted file mode 100644 index bafd8af36b..0000000000 --- a/platooning_control_ihp/include/pid_controller.h +++ /dev/null @@ -1,77 +0,0 @@ -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#include -#include "platoon_control_ihp_config.h" - - -namespace platoon_control_ihp -{ - /** - * \brief This class includes logic for PID controller. PID controller is used in this plugin to maintain the inter-vehicle gap by adjusting the speed. - */ - class PIDController - { - public: - - /** - * \brief Constructor for the PID controller class - */ - PIDController(); - - /** - * \brief plugin config object - */ - 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 ); - - - /** - * \brief function to calculate control command based on setpoint and process vale - * \param setpoint desired value - * \param pv current value - * \return the manipulated variable given a setpoint and current process value - */ - double calculate( double setpoint, double pv ); - // ~PID(); - - /** - * \brief reset the PID controller variables. - */ - 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 deleted file mode 100644 index 72691cdcd3..0000000000 --- a/platooning_control_ihp/include/platoon_control_ihp.h +++ /dev/null @@ -1,171 +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 -#include -#include -#include -#include -#include -#include -#include -#include "platoon_control_ihp_worker.h" - - - - -namespace platoon_control_ihp -{ - /** - * \brief This class includes logic for Platoon control IHP. It includes publishers and subscribers and their callback functions - */ - class PlatoonControlIHPPlugin - { - public: - - /** - * \brief Default constructor for PlatoonControlPluginIHP class - */ - PlatoonControlIHPPlugin(); - - /** - * \brief Initialize plugin variables and publishers/subscribers - */ - void initialize(); - - /** - * \brief general starting point of this node - */ - void run(); - - - /** - * \brief generate control signal by calculating speed and steering commands. - * \param point0 start point of control window - * \param point_end end point of control wondow - */ - void generateControlSignals(const cav_msgs::TrajectoryPlanPoint& point0, const cav_msgs::TrajectoryPlanPoint& point_end); - - /** - * \brief Compose twist message from linear and angular velocity commands. - * \param linear_vel linear velocity in m/s - * \param angular_vel angular velocity in rad/s - * \return twist message - */ - geometry_msgs::TwistStamped composeTwistCmd(double linear_vel, double angular_vel); - - /** - * \brief Compose control message from speed and steering commands. - * \param linear_vel linear velocity in m/s - * \param steering_angle steering angle in rad - * \return control command - */ - autoware_msgs::ControlCommandStamped composeCtrlCmd(double linear_vel, double steering_angle); - - /** - * \brief find the point correspoding to the lookahead distance - * \param trajectory_plan trajectory plan - * \return trajectory point - */ - cav_msgs::TrajectoryPlanPoint getLookaheadTrajectoryPoint(cav_msgs::TrajectoryPlan trajectory_plan); - - /** - * \brief 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 - - /** - * \brief callback function for pose - * \param msg pose stamped msg - */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief callback function for platoon info - * \param msg platoon info msg - */ - void platoonInfo_cb(const cav_msgs::PlatooningInfoConstPtr& msg); - - /** - * \brief callback function for trajectory plan - * \param msg trajectory plan msg - */ - void trajectoryPlan_cb(const cav_msgs::TrajectoryPlan::ConstPtr& tp); - - /** - * \brief callback function for current twist - * \param msg twist stamped msg - */ - void currentTwist_cb(const geometry_msgs::TwistStamped::ConstPtr& twist); - - /** - * \brief calculate average speed of a set of trajectory points - * \param trajectory_points set of trajectory points - * \return trajectory speed in m/s - */ - 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 deleted file mode 100644 index 42bc7d322a..0000000000 --- a/platooning_control_ihp/include/platoon_control_ihp_config.h +++ /dev/null @@ -1,91 +0,0 @@ -#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 standStillHeadway = 12.0; // Standstill gap between vehicles (m) - double maxAccel = 2.5; // Maximum acceleration absolute value used in controller filters (m/s^2) - double Kp = 0.5; // Proportional weight for PID controller - double Kd = -0.5; // Derivative Weight for PID controller - double Ki = 0.0; // Integral weight for PID controller - double maxValue = 2; // Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s) - double minValue = -10; // Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s) - double dt = 0.1; // Timestep to calculate ctrl commands (s) - double adjustmentCap = 10; // Adjustment cap for speed command (m/s) - int cmdTmestamp = 100; // Timestamp to calculate ctrl commands (ms) - double integratorMax = 100; // Max limit for integrator term - double integratorMin = -100; // Max limit for integrator term - double Kdd = 4.5; //coefficient for smooth steering - double lowpassGain = 0.5; // Lowpass filter gain - double lookaheadRatio = 2.0; // Ratio to calculate lookahead distance - double minLookaheadDist = 6.0; // Min lookahead distance (m) - 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 wheelBase = 3.7; //Wheelbase of the vehicle - // 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 = 1.5; // Minimum speed to be considered as moving, in m/s. - 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 - << "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 deleted file mode 100644 index fd42fba727..0000000000 --- a/platooning_control_ihp/include/platoon_control_ihp_worker.h +++ /dev/null @@ -1,175 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 - - /** - * \brief This is the worker class for platoon controller. It is responsible for generating and smoothing the speed and steering control commands from trajectory points. - */ - - class PlatoonControlIHPWorker - { - public: - - /** - * \brief Default constructor for platooning control worker - */ - PlatoonControlIHPWorker(); - - /** - * \brief Update configurations - * \param new_config new configuration - */ - void updateConfigParams(PlatooningControlIHPPluginConfig new_config); - - /** - * \brief Returns latest speed command - * \return latest speed command in m/s - */ - double getLastSpeedCommand() const; - - /** - * \brief Generates speed commands in m/s based on the trajectory point - * \param point trajectory point - */ - void generateSpeed(const cav_msgs::TrajectoryPlanPoint& point); - - /** - * \brief Generates steering commands in rad based on lookahead trajectory point - * \param point trajectory point - */ - void generateSteer(const cav_msgs::TrajectoryPlanPoint& point); - - /** - * \brief set platoon leader - * \param leader platoon leader information - */ - void setLeader(const PlatoonLeaderInfo& leader); - - /** - * \brief set current speed - * \param speed speed value in m/s - */ - void setCurrentSpeed(double speed); - - /** - * \brief UCLA: IHP gap regulation that calculate the deisred position (Dtd, in m) for the platoon members. - * \param leaderCurrentPosition: The current position (dtd) of the leader, in m. - * \return gap value - */ - 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; - - /** - * \brief sets current pose value - * \param msg: The current position - */ - 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 deleted file mode 100644 index cd9056c148..0000000000 --- a/platooning_control_ihp/include/pure_pursuit.h +++ /dev/null @@ -1,145 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - - -#include -#include -#include -#include -#include -#include -#include -#include "platoon_control_ihp_config.h" - - - - - - - -namespace platoon_control_ihp -{ - - /** - * \brief This class includes logic for Pure Pursuit controller. This controller is used to calculate a steering angle using the current pose of the vehcile and a lookahead point. - */ - class PurePursuit - { - - - public: - - /** - * \brief Default constructor for pure pursuit - */ - PurePursuit(); - - /** - * \brief calculates steering angle in rad based on lookahead trajectory point - * \param tp lookahead trajectory point - */ - void calculateSteer(const cav_msgs::TrajectoryPlanPoint& tp); - - /** - * \brief calculates curvature to the lookahead trajectory point - * \param tp lookahead trajectory point - * \return curvature to the lookahead point - */ - double calculateKappa(const cav_msgs::TrajectoryPlanPoint& tp); - - /** - * \brief calculates sin of the heading angle to the target point - * \param tp lookahead trajectory point - * \param current_pose current pose of the vehicle - * \return sin of the heading angle - */ - double getAlphaSin(cav_msgs::TrajectoryPlanPoint tp, geometry_msgs::Pose current_pose); - - /** - * \brief Lowpass filter to smoothen control signal - * \param gain filter gain - * \param prev_value previous value - * \param value current value - * \return smoothened control signal - */ - double lowPassfilter(double gain, double prev_value, double value); - - /** - * \brief returns steering angle - * \return steering angle in rad - */ - double getSteeringAngle(); - - /** - * \brief returns angular velocity - * \return angular velocity in rad/s - */ - double getAngularVelocity(); - - // geometry pose - geometry_msgs::Pose current_pose_; - double velocity_ = 0.0; - double angular_velocity_ = 0; - double steering_angle_ = 0; - - PlatooningControlIHPPluginConfig config_; - - private: - - /** - * \brief calculate lookahead distance - * \param tp trajectory point - * \return lookahead distance from next trajectory point in m - */ - double getLookaheadDist(const cav_msgs::TrajectoryPlanPoint& tp) const; - - /** - * \brief calculate yaw angle of the vehicle - * \param tp trajectory point - * \return yaw angle of the vehicle in rad - */ - double getYaw(const cav_msgs::TrajectoryPlanPoint& tp) const; - - /** - * \brief calculate steering direction - * \param tp trajectory point - * \return steering direction (+1 is left and -1 is right) - */ - 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/platooning_control_ihp/launch/platoon_control_ihp.launch b/platooning_control_ihp/launch/platoon_control_ihp.launch deleted file mode 100644 index 53fa23e3ab..0000000000 --- a/platooning_control_ihp/launch/platoon_control_ihp.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - diff --git a/platooning_control_ihp/package.xml b/platooning_control_ihp/package.xml deleted file mode 100644 index a2efd03893..0000000000 --- a/platooning_control_ihp/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - 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 deleted file mode 100644 index 3731b4bf49..0000000000 --- a/platooning_control_ihp/src/main.cpp +++ /dev/null @@ -1,32 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - - -#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 deleted file mode 100644 index 32dbc85cb9..0000000000 --- a/platooning_control_ihp/src/pid_controller.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 deleted file mode 100644 index afc663e564..0000000000 --- a/platooning_control_ihp/src/platoon_control_ihp.cpp +++ /dev/null @@ -1,321 +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 "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("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("platoon_control_ihp/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 = "platoon_control_ihp"; - 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 deleted file mode 100644 index 594503d07f..0000000000 --- a/platooning_control_ihp/src/platoon_control_ihp_worker.cpp +++ /dev/null @@ -1,239 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 deleted file mode 100644 index 423f6c022d..0000000000 --- a/platooning_control_ihp/src/pure_pursuit.cpp +++ /dev/null @@ -1,140 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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/platooning_control_ihp/test/mynode.test b/platooning_control_ihp/test/mynode.test deleted file mode 100644 index fe0d1979e1..0000000000 --- a/platooning_control_ihp/test/mynode.test +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - diff --git a/platooning_control_ihp/test/test_control.cpp b/platooning_control_ihp/test/test_control.cpp deleted file mode 100644 index ba95cda4ee..0000000000 --- a/platooning_control_ihp/test/test_control.cpp +++ /dev/null @@ -1,52 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 deleted file mode 100644 index 3bbdd41035..0000000000 --- a/platooning_control_ihp/test/test_main.cpp +++ /dev/null @@ -1,29 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 deleted file mode 100644 index e842ffe39e..0000000000 --- a/platooning_control_ihp/test/test_mynode.cpp +++ /dev/null @@ -1,71 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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("platoon_control/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; -} diff --git a/platooning_control_ihp/test/test_pid.cpp b/platooning_control_ihp/test/test_pid.cpp deleted file mode 100644 index 6b18fb2b6a..0000000000 --- a/platooning_control_ihp/test/test_pid.cpp +++ /dev/null @@ -1,57 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#include "pid_controller.h" -#include -#include - - -TEST(PIDControllerTest, test1) -{ - platoon_control_ihp::PIDController pid; - double res = pid.calculate(40, 38); - EXPECT_EQ(-9, res); -} - -// These tests has been temporarily disabled to support Continuous Improvement (CI) processes. -// Related GitHub Issue: - -// 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 deleted file mode 100644 index 6e38e1623e..0000000000 --- a/platooning_control_ihp/test/test_pure.cpp +++ /dev/null @@ -1,47 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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 deleted file mode 100644 index 15f173e5af..0000000000 --- a/platooning_control_ihp/test/test_worker.cpp +++ /dev/null @@ -1,132 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. -* -* Licensed under the Apache License, Version 2.0 (the "License"); you may not -* use this file except in compliance with the License. You may obtain a copy of -* the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -* License for the specific language governing permissions and limitations under -* the License. - -------------------------------------------------------------------------------*/ - -#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/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index b132b7b407..e7cfff96b7 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -23,15 +23,16 @@ - /guidance/plugins/inlanecruising_plugin - /guidance/pure_pursuit_wrapper - /guidance/yield_plugin + - /guidance/plugins/platoon_control - # List of nodes which will not be directly managed by this subsystem controller + # List of nodes which will not be directly managed by this subsystem controller # but which are required to be operational for the subsystem to function unmanaged_required_nodes: [''] # TODO add the controller driver once it is integrated with ROS2 # Boolean: If this flag is true then all nodes under subsystem_namespace are treated as required in addition to any nodes in required_subsystem_nodes full_subsystem_required: false - # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. # Required plugins will be automatically activated at startup # Required plugins cannot be deactivated by the user required_plugins: @@ -50,11 +51,10 @@ - /guidance/plugins/stop_and_wait_plugin - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/stop_controlled_intersection_tactical_plugin - - /guidance/plugins/platoon_control_ihp - /guidance/plugins/platoon_strategic_ihp - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin - + # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin @@ -65,4 +65,5 @@ - /guidance/plugins/cooperative_lanechange - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin - - /guidance/plugins/pure_pursuit_wrapper \ No newline at end of file + - /guidance/plugins/pure_pursuit_wrapper + - /guidance/plugins/platoon_control \ No newline at end of file