├── CMakeLists.txt ├── README.md ├── cfg ├── dyn_collision_tuner.cfg └── dyn_sqp_tuner.cfg ├── misc └── intro_fig.png ├── msg ├── OTSection.msg └── OTSections.msg ├── package.xml └── src ├── collision_prediction.py ├── dynamic_collision_server.py ├── dynamic_sqp_server.py ├── gaussian_process_opp_traj.py ├── opponent_trajectory.py ├── predictor_opponent_trajectory.py ├── sqp_avoidance_node.py └── update_waypoints.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(predictive_spliner) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | dynamic_reconfigure 15 | message_generation 16 | ) 17 | 18 | generate_dynamic_reconfigure_options( 19 | cfg/dyn_collision_tuner.cfg 20 | cfg/dyn_sqp_tuner.cfg 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | add_message_files( 58 | FILES 59 | OTSection.msg 60 | OTSections.msg 61 | ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | # INCLUDE_DIRS include 114 | # LIBRARIES predictive_spliner 115 | # CATKIN_DEPENDS rospy std_msgs 116 | # DEPENDS system_lib 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | # include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/predictive_spliner.cpp 133 | # ) 134 | 135 | ## Add cmake target dependencies of the library 136 | ## as an example, code may need to be generated before libraries 137 | ## either from message generation or dynamic reconfigure 138 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 139 | 140 | ## Declare a C++ executable 141 | ## With catkin_make all packages are built within a single CMake context 142 | ## The recommended prefix ensures that target names across packages don't collide 143 | # add_executable(${PROJECT_NAME}_node src/predictive_spliner_node.cpp) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # catkin_install_python(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 176 | # install(TARGETS ${PROJECT_NAME}_node 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark libraries for installation 181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 182 | # install(TARGETS ${PROJECT_NAME} 183 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 186 | # ) 187 | 188 | ## Mark cpp header files for installation 189 | # install(DIRECTORY include/${PROJECT_NAME}/ 190 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 191 | # FILES_MATCHING PATTERN "*.h" 192 | # PATTERN ".svn" EXCLUDE 193 | # ) 194 | 195 | ## Mark other files for installation (e.g. launch and bag files, etc.) 196 | # install(FILES 197 | # # myfile1 198 | # # myfile2 199 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 200 | # ) 201 | 202 | ############# 203 | ## Testing ## 204 | ############# 205 | 206 | ## Add gtest based cpp test target and link libraries 207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_predictive_spliner.cpp) 208 | # if(TARGET ${PROJECT_NAME}-test) 209 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 210 | # endif() 211 | 212 | ## Add folders to be run by python nosetests 213 | # catkin_add_nosetests(test) 214 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Predictive Spliner: Data-Driven Overtaking in Autonomous Racing 2 | 3 | arXiv e-print Badge 4 | 5 | 6 | Predictive Spliner is a data-driven overtaking planner for autonomous racing, leveraging opponent trajectory prediction through Gaussian Process (GP) regression. The algorithm computes effective overtaking maneuvers while considering both spatial and temporal information, enhancing performance and safety during head-to-head races. Check out our preprint [Predictive Spliner: Data-Driven Overtaking in Autonomous Racing](http://arxiv.org/abs/2410.04868) on ArXiv for more information. Or check out our explanatory [Youtube video](https://www.youtube.com/watch?v=Zqd-OcUf77k). 7 | 8 | GA 9 | 10 | ## Key Features 11 | - **Data-Driven Approach:** Learns opponent behavior and predicts future trajectories for safe overtaking. 12 | - **High Performance:** Achieves up to 83.1% of its own speed during overtaking with an average success rate of 84.5%. 13 | - **Experimental Validation:** Tested on a 1:10 scale autonomous racing platform with a robust comparison to state-of-the-art algorithms. 14 | 15 | ## Installation 16 | The predictive spliner planner is part of the [ForzaETH Race Stack](https://github.com/ForzaETH/race_stack). Please refer to the [installation guide](https://github.com/ForzaETH/race_stack/blob/main/INSTALLATION.md) for detailed instructions and perform the quickstart guide below to run the planner. 17 | 18 | # Quickstart Guide 19 | This is how to run the predictive spliner planner with the headtohead node in the base simulator. The same steps can be followed analogously to run the planner on the physical robot. 20 | 21 | Start a roscore: 22 | ```bash 23 | roscore 24 | ``` 25 | Launch the base simulator with the desired map and sim true: 26 | ```bash 27 | roslaunch stack_master base_system.launch sim:=True racecar_version:=SIM map_name:=f 28 | ``` 29 | Launch headtohead with the predictive_spliner as planner. Note: that perception is set to `False` due to the simulated opponent not providing any perception data, hence set it to `True` when testing on the physical robot: 30 | ```bash 31 | roslaunch stack_master headtohead.launch perception:=False planner:=predictive_spliner 32 | ``` 33 | Launch a dummy obstacle publisher: 34 | ```bash 35 | roslaunch obstacle_publisher obstacle_publisher.launch speed_scaler:=0.3 36 | ``` 37 | Start `rqt` and enable the overtaking sectors: 38 | ```bash 39 | rqt 40 | ``` 41 | 42 | Refer to this video to see the quickstart in action: [Quickstart Video](https://drive.google.com/file/d/1LsW0w8d9_j87QMiO1_ByDNJKnnVSB6d2/view?usp=sharing) 43 | 44 | 45 | ## Citing Predictive Spliner 46 | 47 | If you found our stack helpful in your research, we would appreciate if you cite it as follows: 48 | ``` 49 | @misc{baumann2024predictivesplinerdatadrivenovertaking, 50 | title={Predictive Spliner: Data-Driven Overtaking in Autonomous Racing Using Opponent Trajectory Prediction}, 51 | author={Nicolas Baumann and Edoardo Ghignone and Cheng Hu and Benedict Hildisch and Tino Hämmerle and Alessandro Bettoni and Andrea Carron and Lei Xie and Michele Magno}, 52 | year={2024}, 53 | eprint={2410.04868}, 54 | archivePrefix={arXiv}, 55 | primaryClass={cs.RO}, 56 | url={https://arxiv.org/abs/2410.04868}, 57 | } 58 | ``` 59 | -------------------------------------------------------------------------------- /cfg/dyn_collision_tuner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | PACKAGE = "predictive_spliner" 3 | import rospkg 4 | import yaml, os 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | ros_path = rospkg.RosPack().get_path('predictive_spliner') 8 | 9 | 10 | gen = ParameterGenerator() 11 | gen.add("n_time_steps", int_t, 0, "Number of time steps for prediction", 400, 10, 500) 12 | gen.add("dt", double_t, 0, "Time step for prediction", 0.02, 0.01, 0.1) 13 | gen.add("save_distance_front", double_t, 0, "Length of car in the front plus margin for saftey", 0.6, 0.1, 1.0) 14 | gen.add("save_distance_back", double_t, 0, "Length of car in the back plus margin for saftey", 0.6, 0.1, 1.0) 15 | gen.add("max_v", double_t, 0, "Maximum velocity of the car", 10.0, 5.0, 15.0) 16 | gen.add("min_v", double_t, 0, "Minimum velocity of the car", 0.0, 0.0, 5.0) 17 | gen.add("max_a", double_t, 0, "Maximum acceleration of the car", 7.0, 4.0, 20.0) 18 | gen.add("min_a", double_t, 0, "Minimum acceleration of the car", 5.0, 1.0, 7.0) 19 | gen.add("max_expire_counter", int_t, 0, "Maximum n of iterations until collision info gets deleted", 10, 0, 20) 20 | 21 | gen.add("update_waypoints", bool_t, 0, "Update waypoints", True) 22 | gen.add("speed_offset", double_t, 0, "Add speed offset", 0.0, -1, 1) 23 | 24 | 25 | exit(gen.generate(PACKAGE, "dynamic_collision_tuner", "dyn_collision_tuner")) -------------------------------------------------------------------------------- /cfg/dyn_sqp_tuner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | PACKAGE = "predictive_spliner" 3 | import rospkg 4 | import yaml, os 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | ros_path = rospkg.RosPack().get_path('predictive_spliner') 8 | 9 | 10 | gen = ParameterGenerator() 11 | gen.add("evasion_dist", double_t, 0, "Orthogonal distance of the apex to the obstacle", 0.35, 0.0, 1.25) 12 | gen.add("obs_traj_tresh", double_t, 0, "Threshold of the obstacle towards raceline to be considered for evasion", 1.5, 0.1, 2) 13 | gen.add("spline_bound_mindist", double_t, 0, "Splines may never be closer to the track bounds than this param in meters", 0.20, 0.05, 1.0) 14 | gen.add("lookahead_dist", double_t, 0, "Lookahead distance in meters", 15.0, 1.0, 50.0) 15 | gen.add("avoidance_resolution", int_t, 0, "Number of points used to generate avoidance path", 30, 10, 100) 16 | gen.add("back_to_raceline_before", double_t, 0, "Distance in meters before obstacle to stay on the raceline", 6, 0.5, 10.0) 17 | gen.add("back_to_raceline_after", double_t, 0, "Distance in meters after obstacle to go back on the raceline", 8, 0.5, 10.0) 18 | 19 | gen.add("avoid_static_obs", bool_t, 0, "Avoid static obstacles", False) 20 | 21 | exit(gen.generate(PACKAGE, "sqp_dynamic_tuner", "dyn_sqp_tuner")) -------------------------------------------------------------------------------- /misc/intro_fig.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ForzaETH/predictive-spliner/ddbfbb2312eccda7bb5d84469ec0980ddb07a5e9/misc/intro_fig.png -------------------------------------------------------------------------------- /msg/OTSection.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | 3 | # track location 4 | float64 s_start 5 | float64 s_end 6 | 7 | # map locations 8 | float64 start_x 9 | float64 end_x 10 | float64 start_y 11 | float64 end_y 12 | 13 | # information 14 | string side 15 | bool possible_trailing 16 | float64 score 17 | int32 attempts 18 | 19 | 20 | -------------------------------------------------------------------------------- /msg/OTSections.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | OTSection[] ot_sections -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | predictive_spliner 4 | 0.0.0 5 | The predictve spliner planner package 6 | 7 | 8 | 9 | 10 | Benedict Hildisch 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | message_generation 40 | 41 | 42 | 43 | 44 | 45 | message_runtime 46 | 47 | 48 | 49 | 50 | catkin 51 | roscpp 52 | rospy 53 | std_msgs 54 | dynamic_reconfigure 55 | roscpp 56 | rospy 57 | std_msgs 58 | dynamic_reconfigure 59 | roscpp 60 | rospy 61 | std_msgs 62 | dynamic_reconfigure 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/collision_prediction.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import numpy as np 5 | import rospy 6 | from std_msgs.msg import Bool, Float32, String 7 | from nav_msgs.msg import Odometry 8 | from f110_msgs.msg import ObstacleArray, WpntArray, Wpnt, Obstacle, ObstacleArray, OpponentTrajectory 9 | from visualization_msgs.msg import Marker, MarkerArray 10 | from frenet_conversion.srv import Glob2FrenetArr, Frenet2GlobArr 11 | import time 12 | from dynamic_reconfigure.msg import Config 13 | import copy 14 | 15 | class CollisionPredictor: 16 | """ 17 | Predict the region of collision between the ego car and the opponent. 18 | """ 19 | 20 | def __init__(self): 21 | # Initialize the node 22 | rospy.init_node('collision_predictor', anonymous=True) 23 | 24 | # ROS Parameters 25 | self.opponent_traj_topic = '/opponent_trajectory' 26 | 27 | 28 | # Subscriber 29 | rospy.Subscriber("/perception/obstacles", ObstacleArray, self.opponent_state_cb) 30 | rospy.Subscriber("/car_state/odom_frenet", Odometry, self.odom_cb) 31 | rospy.Subscriber(self.opponent_traj_topic, OpponentTrajectory, self.opponent_trajectory_cb) 32 | rospy.Subscriber('/global_waypoints_updated', WpntArray, self.wpnts_updated_cb) 33 | rospy.Subscriber("/state_machine", String, self.state_cb) 34 | rospy.Subscriber("/dynamic_collision_tuner_node/parameter_updates", Config, self.dyn_param_cb) 35 | 36 | 37 | self.frenet2glob = rospy.ServiceProxy("convert_frenet2globarr_service", Frenet2GlobArr) 38 | 39 | self.loop_rate = 10 #Hz 40 | 41 | # Callback data 42 | self.opponent_pos = ObstacleArray() 43 | self.car_odom = Odometry() 44 | self.wpnts_opponent = list() 45 | self.wpnts_updated = list() 46 | self.state = String() 47 | 48 | self.speed_offset = 0 # m/s 49 | 50 | # Simulation parameters 51 | self.time_steps = 200 52 | self.dt = 0.02 # s 53 | self.save_distance_front = 0.6 # m 54 | self.save_distance_back = 0.4 # m 55 | self.max_v = 10 # m/s 56 | self.min_v = 0 # m/s 57 | self.max_a = 5.5 # m/s^2 58 | self.min_a = 5 # m/s^2 59 | self.max_expire_counter = 10 60 | 61 | # Number of time steps before prediction expires. Set when collision is published. 62 | self.expire_counter = 0 63 | 64 | # Visualization 65 | self.marker_beginn = self.marker_init(a = 0.5, r = 0.63, g = 0.13, b = 0.94, id = 0) 66 | self.marker_end = self.marker_init(a = 0.5, r = 0.63, g = 0.13, b = 0.94, id = 1) 67 | 68 | # Opponent 69 | self.opponent_lap_count = None 70 | 71 | # Publisher 72 | self.marker_pub_beginn = rospy.Publisher("/collision_predict/beginn", Marker, queue_size=10) 73 | self.marker_pub_end = rospy.Publisher("/collision_predict/end", Marker, queue_size=10) 74 | self.collision_obs_pub = rospy.Publisher("/collision_prediction/obstacles", ObstacleArray, queue_size=10) 75 | self.force_trailing_pub = rospy.Publisher("collision_prediction/force_trailing", Bool, queue_size=10) 76 | 77 | 78 | ### CALLBACKS ### 79 | # TODO: Only sees the first dynamic obstacle as opponent... 80 | def opponent_state_cb(self, data: ObstacleArray): 81 | self.opponent_pos.header = data.header 82 | is_dynamic = False 83 | if len(data.obstacles) > 0: 84 | for obs in data.obstacles: 85 | if obs.is_static == False: # and obs.is_opponent == True: # Changed by Tino and Nicolas for compatibility with new obstacle message 86 | self.opponent_pos.obstacles = [obs] 87 | is_dynamic = True 88 | break 89 | if is_dynamic == False: 90 | self.opponent_pos.obstacles = [] 91 | 92 | def odom_cb(self, data: Odometry): self.car_odom = data 93 | 94 | def opponent_trajectory_cb(self, data: OpponentTrajectory): 95 | self.wpnts_opponent = data.oppwpnts # exclude last point (because last point == first point) <- Hopefully this is still the case? 96 | self.max_s_opponent = self.wpnts_opponent[-1].s_m 97 | self.opponent_lap_count = data.lap_count 98 | 99 | def wpnts_updated_cb(self, data: WpntArray): 100 | self.wpnts_updated = data.wpnts[:-1] 101 | self.max_s_updated = self.wpnts_updated[-1].s_m # Should be the same as self.max_s but just in case. Only used for wrap around 102 | 103 | def state_cb(self, data: String): 104 | self.state = data.data 105 | 106 | # Callback triggered by dynamic spline reconf 107 | def dyn_param_cb(self, params: Config): 108 | """ 109 | Notices the change in the parameters and changes spline params 110 | """ 111 | self.time_steps = rospy.get_param("dynamic_collision_tuner_node/n_time_steps", 200) 112 | self.dt = rospy.get_param("dynamic_collision_tuner_node/dt", 0.02) 113 | self.save_distance_front = rospy.get_param("dynamic_collision_tuner_node/save_distance_front", 0.6) 114 | self.save_distance_back = rospy.get_param("dynamic_collision_tuner_node/save_distance_back", 0.4) 115 | self.max_v = rospy.get_param("dynamic_collision_tuner_node/max_v", 10) 116 | self.min_v = rospy.get_param("dynamic_collision_tuner_node/min_v", 0) 117 | self.max_a = rospy.get_param("dynamic_collision_tuner_node/max_a", 5.5) 118 | self.min_a = rospy.get_param("dynamic_collision_tuner_node/min_a", 5) 119 | self.max_expire_counter = rospy.get_param("dynamic_collision_tuner_node/max_expire_counter", 10) 120 | self.speed_offset = rospy.get_param("dynamic_collision_tuner_node/max_expire_counter", 0) 121 | 122 | print( 123 | f"[Coll. Pred.] Dynamic reconf triggered new params:\n" 124 | f" N time stepts: {self.time_steps}, \n" 125 | f" dt: {self.dt} [s], \n" 126 | f" save_distance_front: {self.save_distance_front} [m], \n" 127 | f" save_distance_back: {self.save_distance_back} [m], \n" 128 | f" max_v, min_v, max_a, min_a: {self.max_v, self.min_v, self.max_a, self.min_a}, \n" 129 | f" max_expire_counter: {self.max_expire_counter}" 130 | ) 131 | 132 | 133 | ### HELPER FUNCTIONS ### 134 | def marker_init(self, a = 1, r = 1, g = 0, b = 0, id = 0): 135 | marker = Marker(header=rospy.Header(stamp = rospy.Time.now(), frame_id = "map"),id = id, type = Marker.SPHERE) 136 | # Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 137 | marker.pose.orientation.w = 1.0 138 | # Set the marker's scale 139 | marker.scale.x = 0.4 140 | marker.scale.y = 0.4 141 | marker.scale.z = 0.4 142 | # Set the marker's color 143 | marker.color.a = a # Alpha (transparency) 144 | marker.color.r = r # Red 145 | marker.color.g = g # Green 146 | marker.color.b = b # Blue 147 | return marker 148 | 149 | def delete_all(self) -> None: 150 | empty_marker = Marker(header=rospy.Header(stamp = rospy.Time.now(), frame_id = "map"),id = 0) 151 | empty_marker.action = Marker.DELETE 152 | self.marker_pub_beginn.publish(empty_marker) 153 | empty_marker.id = 1 154 | self.marker_pub_end.publish(empty_marker) 155 | 156 | empty_obs_arr = ObstacleArray(header = rospy.Header(stamp = rospy.Time.now(), frame_id = "map")) 157 | self.collision_obs_pub.publish(empty_obs_arr) 158 | 159 | ### MAIN LOOP ### 160 | def loop(self): 161 | """ 162 | Compute the Region of collision. 163 | """ 164 | rate = rospy.Rate(self.loop_rate) 165 | rospy.loginfo("[Coll. Pred.] Collision Predictor wating...") 166 | rospy.wait_for_message("/global_waypoints", WpntArray) 167 | rospy.wait_for_message("/global_waypoints_scaled", WpntArray) 168 | rospy.wait_for_message("/global_waypoints_updated", WpntArray) 169 | rospy.loginfo("[Coll. Pred.] Updated waypoints recived!") 170 | rospy.wait_for_message(self.opponent_traj_topic, OpponentTrajectory) 171 | rospy.loginfo("[Coll. Pred.] Opponent waypoints recived!") 172 | rospy.wait_for_message("/perception/obstacles", ObstacleArray) 173 | rospy.loginfo("[Coll. Pred.] Obstacles reveived!") 174 | rospy.loginfo("[Coll. Pred.] Collision Predictor ready!") 175 | 176 | while not rospy.is_shutdown(): 177 | 178 | opponent_pos_copy = copy.deepcopy(self.opponent_pos) 179 | 180 | if len(opponent_pos_copy.obstacles) != 0: 181 | # Get the global s points 182 | s_points_global_array = np.array([wpnt.s_m for wpnt in self.wpnts_opponent]) # Opponent 183 | s_points_updated_array = np.array([wpnt.s_m for wpnt in self.wpnts_updated]) # Ego (updated) 184 | 185 | # Get current position of the car and opponent 186 | ego_position = self.car_odom.pose.pose.position.x 187 | opponent_position = opponent_pos_copy.obstacles[0].s_center 188 | # Handle wrap around 189 | if ego_position > self.max_s_updated * (2/3) and opponent_position < self.max_s_updated * (1/3): 190 | opponent_position += self.max_s_updated 191 | 192 | # Check if opponent is close to predicted opponent race line 193 | current_opponent_d = self.opponent_pos.obstacles[0].d_center 194 | opponent_approx_indx = np.abs(s_points_global_array - opponent_position).argmin() 195 | opponent_raceline_d = self.wpnts_opponent[opponent_approx_indx].d_m 196 | 197 | if abs(current_opponent_d - opponent_raceline_d) > 0.25 or self.opponent_lap_count < 1: 198 | self.force_trailing_pub.publish(True) 199 | else: 200 | self.force_trailing_pub.publish(False) 201 | 202 | # Get current speed of the ego car 203 | current_ego_speed = self.car_odom.twist.twist.linear.x 204 | 205 | # Temp params 206 | beginn = False 207 | end = False 208 | beginn_s = 0 209 | end_s = 0 210 | beginn_d = 0 211 | end_d = 0 212 | 213 | start = time.process_time() 214 | for i in range(self.time_steps): 215 | # Get the speed at position i + 1 216 | opponent_approx_indx = np.abs(s_points_global_array - opponent_position % self.max_s_opponent).argmin() # Opponent 217 | ego_approx_indx = np.abs(s_points_updated_array- ego_position % self.max_s_updated).argmin() # Ego (scaled) 218 | 219 | opponent_speed = self.wpnts_opponent[opponent_approx_indx].proj_vs_mps # Opponent 220 | ego_speed = self.wpnts_updated[ego_approx_indx].vx_mps # Ego (updated) 221 | 222 | # Interpolate the acceleration 223 | acceleration = self.min_a + (self.max_a - self.min_a) / max([self.max_v - self.min_v, 10e-6]) * (self.max_v - current_ego_speed) 224 | 225 | if current_ego_speed < ego_speed: 226 | ego_position = (ego_position + current_ego_speed * self.dt + 0.5 * acceleration * self.dt**2) 227 | opponent_position = (opponent_position + opponent_speed * self.dt) 228 | current_ego_speed += acceleration * self.dt 229 | else: 230 | ego_position = (ego_position + ego_speed * self.dt) 231 | opponent_position = (opponent_position + opponent_speed * self.dt) 232 | 233 | opponent_d = self.wpnts_opponent[opponent_approx_indx].d_m 234 | 235 | # Find begin of the collision 236 | if (beginn == False 237 | and ((opponent_position - ego_position)%self.max_s_updated < self.save_distance_front 238 | or abs(opponent_position - ego_position) < self.save_distance_front)): 239 | 240 | beginn_s = ego_position 241 | beginn_d = opponent_d 242 | beginn = True 243 | # Find the end of the collision 244 | elif (beginn == True and end == False and ((ego_position - opponent_position) > self.save_distance_back)): 245 | end_s = ego_position 246 | end_d = opponent_d 247 | end = True 248 | break 249 | 250 | # If no end has been found use latest predicted opponent position 251 | if beginn == True and end == False: 252 | end_s = opponent_position 253 | end_d = opponent_d 254 | 255 | # Check which d is upper and lower bound 256 | if beginn == True: 257 | if end == True: # or end == False: # Add or end == False for more agressive "oppoent loop" behavior 258 | if beginn_d < end_d: 259 | d_right = beginn_d - 0.25 260 | d_left = end_d + 0.25 261 | else: 262 | d_right = end_d - 0.25 263 | d_left = beginn_d + 0.25 264 | else: # Create a wall if there is no end of the collision in sight 265 | d_right = -10 266 | d_left = 10 267 | 268 | collision_obs = Obstacle(id = 0, d_left = d_left, d_right = d_right, d_center = (d_right + d_left)/2, vs = 0, vd = 0, is_actually_a_gap = False, is_static = False) 269 | collision_obs.s_start = beginn_s 270 | collision_obs.s_end = end_s 271 | collision_obs.s_center = (beginn_s + end_s)/2 272 | 273 | collision_obs_arr = ObstacleArray(header = rospy.Header(stamp = rospy.Time.now(), frame_id = "map"), obstacles = [collision_obs]) 274 | self.collision_obs_pub.publish(collision_obs_arr) 275 | self.expire_counter = 0 276 | 277 | # Visualize the collision (Watchout for wrap around) 278 | position_beginn = self.frenet2glob([beginn_s%self.max_s_updated], [beginn_d]) 279 | self.marker_beginn.pose.position.x = position_beginn.x[0] 280 | self.marker_beginn.pose.position.y = position_beginn.y[0] 281 | self.marker_pub_beginn.publish(self.marker_beginn) 282 | 283 | position_end = self.frenet2glob([end_s%self.max_s_updated], [end_d]) 284 | self.marker_end.pose.position.x = position_end.x[0] 285 | self.marker_end.pose.position.y = position_end.y[0] 286 | self.marker_pub_end.publish(self.marker_end) 287 | 288 | 289 | self.expire_counter += 1 290 | if self.expire_counter >= self.max_expire_counter: 291 | self.expire_counter = self.max_expire_counter 292 | self.delete_all() 293 | 294 | # print("Time: {}".format(time.process_time() - start)) 295 | 296 | rate.sleep() 297 | 298 | if __name__ == '__main__': 299 | """ 300 | 301 | """ 302 | node = CollisionPredictor() 303 | node.loop() 304 | -------------------------------------------------------------------------------- /src/dynamic_collision_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | from dynamic_reconfigure.server import Server 4 | from predictive_spliner.cfg import dyn_collision_tunerConfig 5 | 6 | def callback(config, level): 7 | config.n_time_steps = round(config.n_time_steps) 8 | config.dt= config.dt 9 | config.save_distance_front = round(config.save_distance_front, 2) 10 | config.save_distance_back = round(config.save_distance_back, 2) 11 | config.max_v = round(config.max_v, 2) 12 | config.min_v = round(config.min_v, 2) 13 | config.max_a = round(config.max_a, 2) 14 | config.min_a = round(config.min_a, 2) 15 | config.max_expire_counter = round(config.max_expire_counter) 16 | config.update_waypoints = config.update_waypoints 17 | config.speed_offset = round(config.speed_offset, 3) 18 | return config 19 | 20 | if __name__ == "__main__": 21 | rospy.init_node("dynamic_collision_tuner_node", anonymous=False) 22 | print('[Planner] Dynamic Collision Server Launched...') 23 | srv = Server(dyn_collision_tunerConfig, callback) 24 | rospy.spin() 25 | -------------------------------------------------------------------------------- /src/dynamic_sqp_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | from dynamic_reconfigure.server import Server 4 | from predictive_spliner.cfg import dyn_sqp_tunerConfig 5 | 6 | def callback(config, level): 7 | config.evasion_dist = round(config.evasion_dist, 2) 8 | config.obs_traj_tresh = round(config.obs_traj_tresh,2) 9 | config.spline_bound_mindist = round(config.spline_bound_mindist, 3) 10 | config.lookahead_dist = round(config.lookahead_dist, 2) 11 | config.avoidance_resolution = round(config.avoidance_resolution) 12 | config.back_to_raceline_before = round(config.back_to_raceline_before, 2) 13 | config.back_to_raceline_after = round(config.back_to_raceline_after, 2) 14 | 15 | config.avoid_static_obs = config.avoid_static_obs 16 | return config 17 | 18 | if __name__ == "__main__": 19 | rospy.init_node("dynamic_sqp_tuner_node", anonymous=False) 20 | print('[Planner] Dynamic SQP Server Launched...') 21 | srv = Server(dyn_sqp_tunerConfig, callback) 22 | rospy.spin() -------------------------------------------------------------------------------- /src/gaussian_process_opp_traj.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import rospy 5 | from f110_msgs.msg import ObstacleArray, OpponentTrajectory, OppWpnt, WpntArray, ProjOppTraj 6 | from visualization_msgs.msg import Marker, MarkerArray 7 | from sklearn.gaussian_process import GaussianProcessRegressor 8 | from sklearn.gaussian_process.kernels import RBF, Matern, WhiteKernel, ConstantKernel 9 | import time 10 | from scipy.optimize import fmin_l_bfgs_b 11 | from frenet_converter.frenet_converter import FrenetConverter 12 | 13 | from ccma import CCMA 14 | 15 | class GaussianProcessOppTraj(object): 16 | def __init__(self): 17 | #Node 18 | rospy.init_node('gaussian_process_opp_traj', anonymous=True) 19 | self.rate = rospy.Rate(10) 20 | self.opp_traj = OpponentTrajectory() 21 | self.opp_traj_gp = OpponentTrajectory() 22 | self.opp_positions_in_map_frenet = [] #testing with Rosbag 23 | 24 | #Subscribers 25 | rospy.Subscriber('/proj_opponent_trajectory', ProjOppTraj, self.proj_opp_traj_cb) 26 | rospy.Subscriber('/global_waypoints', WpntArray, self.glb_wpnts_cb) # global waypoints 27 | 28 | #Publishers 29 | self.opp_traj_gp_pub = rospy.Publisher('/opponent_trajectory', OpponentTrajectory, queue_size=10) 30 | self.opp_traj_marker_pub = rospy.Publisher('/opponent_traj_markerarray', MarkerArray, queue_size=10) 31 | 32 | #Frenet Converter 33 | rospy.wait_for_message("/global_waypoints", WpntArray) 34 | self.converter = self.initialize_converter() 35 | 36 | #Callback 37 | def proj_opp_traj_cb(self,data: ProjOppTraj): 38 | self.proj_opp_traj = data 39 | 40 | def glb_wpnts_cb(self, data): 41 | self.waypoints = np.array([[wpnt.x_m, wpnt.y_m] for wpnt in data.wpnts]) 42 | self.glb_wpnts = data 43 | self.track_length = data.wpnts[-1].s_m 44 | 45 | 46 | #Functions 47 | def initialize_converter(self) -> bool: 48 | 49 | """Initialize the FrenetConverter object""" 50 | 51 | rospy.wait_for_message("/global_waypoints", WpntArray) 52 | 53 | # Initialize the FrenetConverter object 54 | converter = FrenetConverter(self.waypoints[:, 0], self.waypoints[:, 1]) 55 | rospy.loginfo("[Tracking] initialized FrenetConverter object") 56 | 57 | return converter 58 | 59 | #Main Loop 60 | def get_gp_opp_traj(self): 61 | # Define the constant kernels with a lower bound for the constant_value parameter 62 | constant_kernel1_d = ConstantKernel(constant_value=0.5, constant_value_bounds=(1e-6, 1e3)) 63 | constant_kernel2_d = ConstantKernel(constant_value=0.2, constant_value_bounds=(1e-6, 1e3)) 64 | constant_kernel1_vs = ConstantKernel(constant_value=0.5, constant_value_bounds=(1e-6, 1e3)) 65 | constant_kernel2_vs = ConstantKernel(constant_value=0.2, constant_value_bounds=(1e-6, 1e3)) 66 | 67 | # Define the Gaussian Process kernel 68 | self.kernel_vs = constant_kernel1_vs * RBF(length_scale=1.0) + constant_kernel2_vs * WhiteKernel(noise_level=1) 69 | self.kernel_d = constant_kernel1_d * Matern(length_scale=1.0, nu=3/2) + constant_kernel2_d * WhiteKernel(noise_level=1) 70 | first_half_lap = True 71 | self.global_wpnts = rospy.wait_for_message("/global_waypoints", WpntArray) 72 | self.max_velocity = max([wnpt.vx_mps for wnpt in self.global_wpnts.wpnts]) 73 | ego_s_original = [wnpt.s_m for wnpt in self.global_wpnts.wpnts] 74 | #pop last point of original ego_s since it is double 75 | ego_s_original.pop() 76 | ego_s_doublelap = ego_s_original.copy() 77 | for i in range(len(ego_s_original)): 78 | ego_s_doublelap.append(ego_s_original[i]+self.track_length) 79 | 80 | #create a oppwpmt lap with velocity 100 81 | oppwpnts_list = self.make_initial_opponent_trajectory_msg(ego_s_original=ego_s_original) 82 | proj_opp_traj = ProjOppTraj() 83 | sorted_detection_list = [] 84 | while not rospy.is_shutdown(): 85 | proj_opp_traj = rospy.wait_for_message('/proj_opponent_trajectory', ProjOppTraj) 86 | self.lap_count = proj_opp_traj.lapcount 87 | opp_on_traj = proj_opp_traj.opp_is_on_trajectory 88 | nr_of_points = proj_opp_traj.nrofpoints 89 | if opp_on_traj == True and len(proj_opp_traj.detections) != 0: 90 | if self.lap_count <=1: 91 | sorted_detection_list_first_lap, around_origin = self.create_sorted_detection_list(proj_opponent_detections=proj_opp_traj.detections, sorted_detection_list=[], ego_s_original=ego_s_original) 92 | for i in range(len(sorted_detection_list_first_lap)): 93 | sorted_detection_list.append(sorted_detection_list_first_lap[i]) 94 | 95 | opponent_s_sorted, opponent_d_sorted,opponent_vs_sorted, opponent_vd_sorted = self.get_sorted_s_d_vs_vd_lists(sorted_detection_list=sorted_detection_list_first_lap) 96 | 97 | if first_half_lap: 98 | first_half_lap = False 99 | ego_s_sorted_halflap = [] 100 | for i in range(len(ego_s_doublelap)): 101 | if ego_s_doublelap[i] > opponent_s_sorted[0] and ego_s_doublelap[i] < opponent_s_sorted[0]+(self.track_length/2): 102 | ego_s_sorted_halflap.append(ego_s_doublelap[i]) 103 | last_ego_s = ego_s_sorted_halflap[-1] 104 | first_ego_s = ego_s_sorted_halflap[0] 105 | 106 | else: 107 | #make a new ego_s_sorted_halflap with the last_ego_s as a statring point and first_ego_s as an end point 108 | ego_s_sorted_halflap = [] 109 | last_ego_s = last_ego_s%self.track_length 110 | if last_ego_s > first_ego_s: 111 | first_ego_s = first_ego_s+self.track_length 112 | 113 | for i in range(len(ego_s_doublelap)): 114 | if ego_s_doublelap[i] >= last_ego_s and ego_s_doublelap[i] <= first_ego_s: 115 | ego_s_sorted_halflap.append(ego_s_doublelap[i]) 116 | last_ego_s = ego_s_sorted_halflap[-1] 117 | first_ego_s = ego_s_sorted_halflap[0] 118 | 119 | 120 | oppwpnts_list , opp_traj_marker_array = self.get_opponnent_wpnts(whole_lap=False, ego_s_sorted=ego_s_sorted_halflap, opponent_s_sorted=opponent_s_sorted, 121 | opponent_d_sorted=opponent_d_sorted, opponent_vs_sorted=opponent_vs_sorted, 122 | opponent_vd_sorted=opponent_vd_sorted, arond_origin=around_origin, oppwpnts_list=oppwpnts_list) 123 | 124 | opp_traj_gp_msg = self.make_opponent_trajectory_msg(oppwpnts_list=oppwpnts_list, lap_count=self.lap_count, raw_oppenent_traj_msg=proj_opp_traj) 125 | #Publish 126 | self.opp_traj_gp_pub.publish(opp_traj_gp_msg) 127 | self.opp_traj_marker_pub.publish(opp_traj_marker_array) 128 | 129 | if around_origin:#reset s value of sorted_detection_list and sort again 130 | for i in range(len(sorted_detection_list)): 131 | sorted_detection_list[i].s = sorted_detection_list[i].s%self.track_length 132 | sorted_detection_list.sort(key=lambda x: x.s) 133 | 134 | if self.lap_count == 1: 135 | self.lap_count = 1.1 136 | sorted_detection_list, around_origin = self.create_sorted_detection_list(proj_opponent_detections=proj_opp_traj.detections, sorted_detection_list=sorted_detection_list, ego_s_original=ego_s_original) 137 | opponent_s_sorted, opponent_d_sorted,opponent_vs_sorted, opponent_vd_sorted = self.get_sorted_s_d_vs_vd_lists(sorted_detection_list=sorted_detection_list) 138 | oppwpnts_list , opp_traj_marker_array = self.get_opponnent_wpnts(whole_lap=True, ego_s_sorted=ego_s_original, opponent_s_sorted=opponent_s_sorted, 139 | opponent_d_sorted=opponent_d_sorted, opponent_vs_sorted=opponent_vs_sorted, 140 | opponent_vd_sorted=opponent_vd_sorted, arond_origin=False, oppwpnts_list=oppwpnts_list) 141 | #Publish 142 | self.opp_traj_gp_pub.publish(opp_traj_gp_msg) 143 | self.opp_traj_marker_pub.publish(opp_traj_marker_array) 144 | 145 | 146 | 147 | else: #adding additional points to the trajectory 148 | sorted_detection_list, around_origin = self.create_sorted_detection_list(proj_opponent_detections=proj_opp_traj.detections, sorted_detection_list=sorted_detection_list, ego_s_original=ego_s_original) 149 | 150 | opponent_s_sorted, opponent_d_sorted,opponent_vs_sorted, opponent_vd_sorted = self.get_sorted_s_d_vs_vd_lists(sorted_detection_list=sorted_detection_list) 151 | 152 | oppwpnts_list , opp_traj_marker_array = self.get_opponnent_wpnts(whole_lap=True, ego_s_sorted=ego_s_original, opponent_s_sorted=opponent_s_sorted, 153 | opponent_d_sorted=opponent_d_sorted, opponent_vs_sorted=opponent_vs_sorted, 154 | opponent_vd_sorted=opponent_vd_sorted, arond_origin=False, oppwpnts_list=oppwpnts_list) 155 | 156 | opp_traj_gp_msg = self.make_opponent_trajectory_msg(oppwpnts_list=oppwpnts_list, lap_count=self.lap_count, raw_oppenent_traj_msg=proj_opp_traj) 157 | 158 | #Publish 159 | self.opp_traj_gp_pub.publish(opp_traj_gp_msg) 160 | self.opp_traj_marker_pub.publish(opp_traj_marker_array) 161 | 162 | 163 | #Helper Functions 164 | def create_sorted_detection_list(self,proj_opponent_detections:list, sorted_detection_list:list, ego_s_original:list): 165 | 166 | """Sort the opponent trajectory based on the s position and return the sorted lists""" 167 | around_origin = False 168 | 169 | if self.lap_count <=1: 170 | for i in range(len(proj_opponent_detections)-1): 171 | if proj_opponent_detections[i].s > proj_opponent_detections[i+1].s: 172 | around_origin = True 173 | if around_origin: 174 | proj_opponent_detections[i+1].s = proj_opponent_detections[i+1].s+self.track_length 175 | else: 176 | for i in range(len(proj_opponent_detections)-1): 177 | proj_opponent_detections[i+1].s = proj_opponent_detections[i+1].s%self.track_length 178 | 179 | for i in range(len(proj_opponent_detections)): 180 | sorted_detection_list.append(proj_opponent_detections[i]) 181 | sorted_detection_list.sort(key=lambda x: x.s) 182 | printed_s_list = [sorted_detection_list[i].s for i in range(len(sorted_detection_list))] 183 | 184 | delta_s = 2*(self.track_length/len(ego_s_original)) #2*distance between waypoints 185 | if (len(sorted_detection_list) > 200) and (self.lap_count >=1): 186 | 187 | #if detections are too close together, remove the older one 188 | sorted_detection_list_new = [] 189 | last_s = sorted_detection_list[-1].s 190 | i=0 191 | for x in range(int(self.track_length/delta_s)): 192 | if last_s > (x+1)*delta_s: 193 | helper_list = [] 194 | while(sorted_detection_list[i].s < x*delta_s): 195 | helper_list.append(sorted_detection_list[i]) 196 | i=i+1 197 | if(len(helper_list)>0): 198 | helper_list.sort(key=lambda x: x.time) 199 | sorted_detection_list_new.append(helper_list[-1]) 200 | 201 | sorted_detection_list = sorted_detection_list_new 202 | 203 | return sorted_detection_list, around_origin 204 | 205 | 206 | def get_sorted_s_d_vs_vd_lists(self, sorted_detection_list:list): 207 | 208 | """Sort the opponent trajectory based on the s position and return the sorted lists""" 209 | 210 | opponent_s_sorted = [detection.s for detection in sorted_detection_list] 211 | opponent_d_sorted = [detection.d for detection in sorted_detection_list] 212 | opponent_vs_sorted = [detection.vs for detection in sorted_detection_list] 213 | opponent_vd_sorted = [detection.vd for detection in sorted_detection_list] 214 | 215 | 216 | return opponent_s_sorted, opponent_d_sorted,opponent_vs_sorted, opponent_vd_sorted 217 | 218 | def get_opponnent_wpnts(self,whole_lap: bool, ego_s_sorted: list, opponent_s_sorted: list, opponent_d_sorted: list, opponent_vs_sorted: list, opponent_vd_sorted: list, arond_origin: bool, oppwpnts_list: list): 219 | 220 | """Resample the opponent trajectory based on the ego vehicle's s position and return the resampled opponent trajectory (aso return the resampled opponent trajectory as a marker array)""" 221 | ego_s_sorted_copy = ego_s_sorted.copy() 222 | 223 | #testing with Rosbag 224 | #split self.opp_positions_in_map_frenet in a s/s/vs/vd list 225 | # opp_s = [position[0] for position in self.opp_positions_in_map_frenet] 226 | # opp_d = [position[1] for position in self.opp_positions_in_map_frenet] 227 | # opp_vs = [position[2] for position in self.opp_positions_in_map_frenet] 228 | # opp_vd = [position[3] for position in self.opp_positions_in_map_frenet] 229 | if whole_lap: 230 | 231 | #Predict the d coordinate with CCMA in case of a whole lap 232 | #stretch the s and d list to ensure that the CCMA works smoothly around the origin 233 | opp_s_copy = opponent_s_sorted.copy() 234 | opp_d_copy = opponent_d_sorted.copy() 235 | opp_s_copy.insert(0,opp_s_copy[-1]) 236 | opp_d_copy.insert(0,opp_d_copy[-1]) 237 | opp_s_copy.append(opp_s_copy[1]) 238 | opp_d_copy.append(opp_d_copy[1]) 239 | 240 | #convert to cartesian 241 | noisy_xy_points=self.converter.get_cartesian(opp_s_copy, opp_d_copy) 242 | noisy_xy_points = noisy_xy_points.transpose() 243 | #smooth the trajectory with CCMA 244 | ccma = CCMA(w_ma=5, w_cc=3) 245 | smoothed_xy_points = ccma.filter(noisy_xy_points) 246 | 247 | #convert back to frenet 248 | smoothed_sd_points = self.converter.get_frenet(smoothed_xy_points[:, 0], smoothed_xy_points[:, 1]) 249 | #sort the points based on s 250 | smoothed_s_points = smoothed_sd_points[0] 251 | smoothed_d_points = smoothed_sd_points[1] 252 | 253 | smoothed_s_points, smoothed_d_points = zip(*sorted(zip(smoothed_s_points, smoothed_d_points))) 254 | 255 | #interpolate the smoothed trajectory on the same s points as the ego vehicles trajectory 256 | d_pred_CCMA = np.interp(ego_s_sorted, smoothed_s_points, smoothed_d_points) 257 | 258 | 259 | #Preparing the data for the Gaussian Process 260 | #prepend the last points of the opponent trajectory to the beginning of the list 261 | n=-1 262 | nr_of_points_pre = 0 263 | for i in range(len(opponent_s_sorted)): 264 | if abs(opponent_s_sorted[n]-self.track_length) < 3: #go 3 m in negative direction 265 | opponent_s_sorted.insert(0,opponent_s_sorted[n]-self.track_length) 266 | n=n-1 267 | nr_of_points_pre = nr_of_points_pre+1 268 | n=-1 269 | for i in range(nr_of_points_pre): 270 | opponent_d_sorted.insert(0,opponent_d_sorted[n]) 271 | n=n-1 272 | n=-1 273 | for i in range(nr_of_points_pre): 274 | opponent_vs_sorted.insert(0,opponent_vs_sorted[n]) 275 | n=n-1 276 | n=-1 277 | for i in range(nr_of_points_pre): 278 | opponent_vd_sorted.insert(0,opponent_vd_sorted[n]) 279 | n=n-1 280 | n=-1 281 | #prepend last points of the ego_s_sorted to the beginning of the list as a negative value 282 | nr_of_points_ego_s = 0 283 | for i in range(len(ego_s_sorted_copy)): 284 | if abs(ego_s_sorted_copy[n]-self.track_length) < 3: 285 | ego_s_sorted_copy.insert(0,ego_s_sorted_copy[n]-self.track_length) 286 | n=n-1 287 | nr_of_points_ego_s = nr_of_points_ego_s+1 288 | #append the first points of the opponent trajectory to the end of the list 289 | n=0 290 | nr_of_points_app = 0 291 | for i in range(len(opponent_s_sorted)): 292 | if abs(opponent_s_sorted[nr_of_points_pre+n]) < 3: 293 | opponent_s_sorted.append(opponent_s_sorted[nr_of_points_pre+n]+self.track_length) 294 | n=n+1 295 | nr_of_points_app = nr_of_points_app+1 296 | n=0 297 | for i in range(nr_of_points_app): 298 | opponent_d_sorted.append(opponent_d_sorted[nr_of_points_pre+n]) 299 | n=n+1 300 | n=0 301 | for i in range(nr_of_points_app): 302 | opponent_vs_sorted.append(opponent_vs_sorted[nr_of_points_pre+n]) 303 | n=n+1 304 | n=0 305 | for i in range(nr_of_points_app): 306 | opponent_vd_sorted.append(opponent_vd_sorted[nr_of_points_pre+n]) 307 | n=n+1 308 | n=0 309 | 310 | #append first 3m of the ego_s_sorted to the end of the list as a value bigger than the track_length 311 | for i in range(len(ego_s_sorted_copy)): 312 | if abs(ego_s_sorted_copy[nr_of_points_ego_s+n]) < 3: 313 | ego_s_sorted_copy.append(ego_s_sorted_copy[nr_of_points_ego_s+n]+self.track_length) 314 | n=n+1 315 | train_s = np.array([opponent_s_sorted[i] for i in range(len(opponent_s_sorted))]) 316 | train_d = np.array([opponent_d_sorted[i] for i in range(len(opponent_d_sorted))]) 317 | train_vs = np.array([opponent_vs_sorted[i] for i in range(len(opponent_vs_sorted))]) 318 | train_vd = np.array([opponent_vd_sorted[i] for i in range(len(opponent_vd_sorted))]) 319 | else: 320 | train_s = np.array([opponent_s_sorted[i] for i in range(len(opponent_s_sorted))]) 321 | train_d = np.array([opponent_d_sorted[i] for i in range(len(opponent_d_sorted))]) 322 | train_vs = np.array([opponent_vs_sorted[i] for i in range(len(opponent_vs_sorted))]) 323 | train_vd = np.array([opponent_vd_sorted[i] for i in range(len(opponent_vd_sorted))]) 324 | 325 | opponent_s_sorted_reshape = train_s.reshape(-1, 1) 326 | opponent_d_sorted_reshape = train_d.reshape(-1, 1) 327 | opponent_vs_sorted_reshape = train_vs.reshape(-1, 1) 328 | opponent_vd_sorted_reshape = train_vd.reshape(-1, 1) 329 | 330 | #Define a range of s values for prediction 331 | ego_s_sorted_nparray = np.array(ego_s_sorted_copy) 332 | s_pred = ego_s_sorted_nparray.reshape(-1, 1) 333 | 334 | #Fit the Gaussian Process Regressor to the data 335 | def optimizer_wrapper(obj_func, initial_theta, bounds): 336 | solution, function_value, _ = fmin_l_bfgs_b(obj_func, initial_theta, bounds=bounds) 337 | return solution, function_value 338 | 339 | #Fit Vs 340 | gpr_vs = GaussianProcessRegressor(kernel=self.kernel_vs, optimizer=optimizer_wrapper) 341 | gpr_vs.fit(opponent_s_sorted_reshape, opponent_vs_sorted_reshape) 342 | vs_pred, sigma_vs = gpr_vs.predict(s_pred, return_std=True) 343 | 344 | #if not already predicted with CCMA fit D with GP 345 | if not whole_lap: 346 | gpr_d = GaussianProcessRegressor(kernel=self.kernel_d, optimizer=optimizer_wrapper) 347 | gpr_d.fit(opponent_s_sorted_reshape, opponent_d_sorted_reshape) 348 | d_pred_GP, sigma_d = gpr_d.predict(s_pred, return_std=True) 349 | 350 | #shorten the copy lists (that was changed) to the length of the original ego_s 351 | if whole_lap: 352 | n=0 353 | for i in range(len(ego_s_sorted_copy)): 354 | if ego_s_sorted_copy[i-n] >= self.track_length or ego_s_sorted_copy[i-n] < 0: 355 | ego_s_sorted_copy.pop(i-n) 356 | if not whole_lap: #(CCMA) 357 | d_pred_GP = np.delete(d_pred_GP, i-n) 358 | vs_pred = np.delete(vs_pred, i-n) 359 | n+=1 360 | 361 | if whole_lap: #(CCMA) 362 | #if False: 363 | resampled_opponent_d = d_pred_CCMA 364 | else: 365 | resampled_opponent_d = d_pred_GP 366 | resampled_opponent_vs = vs_pred 367 | resampled_opponent_vd = np.interp(ego_s_sorted, opponent_s_sorted, opponent_vd_sorted) 368 | 369 | if arond_origin: 370 | ego_s=[ego_s_sorted[i]%self.track_length for i in range(len(ego_s_sorted_copy))] 371 | else: 372 | ego_s=ego_s_sorted_copy 373 | 374 | 375 | resampled_wpnts_xy = self.converter.get_cartesian(ego_s , resampled_opponent_d.tolist()) 376 | 377 | # replace all the entries where there is a corresponding ego_s with the interpolated values 378 | i=0 379 | 380 | for i in range(len(oppwpnts_list)): 381 | for j in range(len(ego_s)): 382 | if abs(ego_s[j]-oppwpnts_list[i].s_m) < 1e-8: 383 | oppwpnts_list[i].x_m = resampled_wpnts_xy[0][j] 384 | oppwpnts_list[i].y_m = resampled_wpnts_xy[1][j] 385 | oppwpnts_list[i].d_m = resampled_opponent_d[j] 386 | oppwpnts_list[i].proj_vs_mps = resampled_opponent_vs[j] 387 | oppwpnts_list[i].vd_mps = resampled_opponent_vd[j] 388 | if not whole_lap: 389 | oppwpnts_list[i].d_var = sigma_d[j] 390 | else: 391 | oppwpnts_list[i].d_var = 0 392 | oppwpnts_list[i].vs_var = sigma_vs[j] 393 | opp_traj_marker_array = self._visualize_opponent_wpnts(oppwpnts_list=oppwpnts_list) 394 | 395 | return oppwpnts_list , opp_traj_marker_array 396 | 397 | 398 | 399 | def make_initial_opponent_trajectory_msg(self, ego_s_original:list): 400 | 401 | #make trajectory with velocity 100 for the first half lap 402 | resampled_wpnts_xy_original = self.converter.get_cartesian(ego_s_original , np.zeros(len(ego_s_original)).tolist()) 403 | oppwpnts_list = [] 404 | i=0 405 | 406 | for i in range(len(ego_s_original)): 407 | oppwpnts = OppWpnt() 408 | oppwpnts.x_m = resampled_wpnts_xy_original[0][i] 409 | oppwpnts.y_m = resampled_wpnts_xy_original[1][i] 410 | oppwpnts.s_m = ego_s_original[i] 411 | oppwpnts.d_m = 0 412 | oppwpnts.proj_vs_mps = 100 413 | oppwpnts.vd_mps = 0 414 | oppwpnts_list.append(oppwpnts) 415 | return oppwpnts_list 416 | 417 | 418 | 419 | def make_opponent_trajectory_msg(self, oppwpnts_list: list, lap_count: int, raw_oppenent_traj_msg: ObstacleArray): 420 | 421 | """Make the opponent trajectory message and return it""" 422 | 423 | opponent_trajectory_msg = OpponentTrajectory() 424 | opponent_trajectory_msg.header.seq = lap_count 425 | opponent_trajectory_msg.header.stamp = rospy.Time.now() 426 | opponent_trajectory_msg.header.frame_id = "opponent_trajectory" 427 | opponent_trajectory_msg.lap_count = lap_count 428 | opponent_trajectory_msg.oppwpnts = oppwpnts_list 429 | 430 | return opponent_trajectory_msg 431 | 432 | 433 | 434 | def _visualize_opponent_wpnts(self, oppwpnts_list: list): 435 | 436 | """Visualize the resampled opponent trajectory as a marker array""" 437 | opp_traj_marker_array = MarkerArray() 438 | 439 | i=0 440 | for i in range(len(oppwpnts_list)): 441 | marker_height = oppwpnts_list[i].proj_vs_mps/self.max_velocity 442 | 443 | marker = Marker(header=rospy.Header(frame_id="map"), id = i, type = Marker.CYLINDER) 444 | marker.pose.position.x = oppwpnts_list[i].x_m 445 | marker.pose.position.y = oppwpnts_list[i].y_m 446 | marker.pose.position.z = marker_height/2 447 | marker.pose.orientation.w = 1.0 448 | marker.scale.x = min(max(5 * oppwpnts_list[i].d_var, 0.07),0.7) 449 | marker.scale.y = min(max(5 * oppwpnts_list[i].d_var, 0.07),0.7) 450 | marker.scale.z = marker_height 451 | marker.color.a = 1.0 452 | marker.color.r = 1.0 453 | marker.color.g = 1.0 454 | marker.color.b = 0.0 455 | opp_traj_marker_array.markers.append(marker)#frenpy 456 | 457 | 458 | return opp_traj_marker_array 459 | 460 | 461 | 462 | if __name__ == '__main__': 463 | 464 | 465 | node = GaussianProcessOppTraj() 466 | node.get_gp_opp_traj() 467 | rospy.spin() -------------------------------------------------------------------------------- /src/opponent_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import rospy 5 | from f110_msgs.msg import ObstacleArray, OpponentTrajectory, Obstacle, OppWpnt, WpntArray,ProjOppTraj, ProjOppPoint 6 | from nav_msgs.msg import Odometry 7 | from visualization_msgs.msg import Marker, MarkerArray 8 | import time 9 | from std_msgs.msg import String 10 | from frenet_converter.frenet_converter import FrenetConverter 11 | 12 | 13 | class Opponent_Trajectory: 14 | def __init__(self): 15 | # Initialize the node 16 | rospy.init_node('opponent_trajectory', anonymous=True) 17 | 18 | self.opponent = [0,0,0,0,False,True,0] #s, d, vs, vd, is_static, is_visible, timestamp 19 | self.position_in_map_frenet = [] # current position in frenet coordinates 20 | self.opponent_trajectory_list = [] # add all the points where the opponent was to this list 21 | self.opponent_as_obstacle = Obstacle() 22 | self.opponent_trajectory_list_of_obstacles = [] 23 | self.oppwpnts = OppWpnt() 24 | self.oppwpnts_list = [] 25 | self.converter = None 26 | self.opponent_positions = [] 27 | self.track_length = 70 #initialize track_length with a feasible value 28 | self.opp_traj_count = 0 29 | self.overtake = False 30 | 31 | 32 | 33 | # Time 34 | self.ros_time = rospy.Time() 35 | 36 | # Publisher 37 | self.proj_opponent_trajectory_pub = rospy.Publisher('/proj_opponent_trajectory', ProjOppTraj, queue_size=10) 38 | self.marker_pub = rospy.Publisher('/opponent_marker', MarkerArray, queue_size=10) 39 | # Subscriber 40 | rospy.Subscriber("/perception/obstacles", ObstacleArray, self.obstacle_cb) 41 | rospy.Subscriber('/car_state/odom_frenet', Odometry, self.car_state_frenet_cb) # car frenet coordinates 42 | rospy.Subscriber('/global_waypoints', WpntArray, self.glb_wpnts_cb) # global waypoints 43 | rospy.Subscriber('/opponent_trajectory', OpponentTrajectory, self.opp_traj_cb) # global waypoints 44 | #subscribe to state machine 45 | rospy.Subscriber('/state_machine', String, self.state_machine_cb) 46 | 47 | # Retrieve ROS parameters 48 | self.loop_rate = rospy.get_param('~loop_rate', 25) 49 | 50 | rospy.wait_for_message("/global_waypoints", WpntArray) 51 | self.converter = self.initialize_converter() 52 | 53 | #callbacks 54 | def state_machine_cb(self, data: String): 55 | if data.data == "OVERTAKE": 56 | self.overtake = True 57 | else: 58 | self.overtake = False 59 | 60 | def obstacle_cb(self, data: ObstacleArray): 61 | self.obstacle_array = data 62 | 63 | def car_state_frenet_cb(self, data: Odometry): 64 | s = data.pose.pose.position.x 65 | d = data.pose.pose.position.y 66 | vs = data.twist.twist.linear.x 67 | vd = data.twist.twist.linear.y 68 | self.position_in_map_frenet = np.array([s,d,vs,vd]) 69 | 70 | def glb_wpnts_cb(self, data): 71 | self.waypoints = np.array([[wpnt.x_m, wpnt.y_m] for wpnt in data.wpnts]) 72 | self.glb_wpnts = data 73 | self.track_length = data.wpnts[-1].s_m 74 | 75 | def opp_traj_cb(self, data: OpponentTrajectory): 76 | if not self.overtake: 77 | self.opponent_trajectory = data 78 | self.opp_traj_count += 1 79 | 80 | def initialize_converter(self) -> bool: 81 | """ 82 | Initialize the FrenetConverter object""" 83 | rospy.wait_for_message("/global_waypoints", WpntArray) 84 | 85 | # Initialize the FrenetConverter object 86 | converter = FrenetConverter(self.waypoints[:, 0], self.waypoints[:, 1]) 87 | rospy.loginfo("[Tracking] initialized FrenetConverter object") 88 | 89 | return converter 90 | 91 | 92 | #Main Loop 93 | def make_opponent_trajectory(self): 94 | 95 | """Make the opponent trajectory and publish it""" 96 | 97 | 98 | rate = rospy.Rate(self.loop_rate) 99 | self.lap_count = 0 100 | first_point = True 101 | self.global_wpnts = rospy.wait_for_message("/global_waypoints", WpntArray) 102 | ego_s_sorted = [wnpt.s_m for wnpt in self.global_wpnts.wpnts] 103 | bound_right = [wnpt.d_right for wnpt in self.global_wpnts.wpnts]#all positive 104 | bound_left = [wnpt.d_left for wnpt in self.global_wpnts.wpnts]#all positive 105 | self.treshold = self.track_length/2 #treshold for checking if we are going "backwards" in the s direction 106 | number_of_wpnts = len(ego_s_sorted) 107 | raw_opponent_traj = np.zeros((number_of_wpnts,11)) 108 | projected_opponent_traj = ProjOppTraj() 109 | first_point = True 110 | traversed_distance = 0 111 | index = None 112 | points_in_lap = 0 113 | track_length = self.track_length 114 | marker_count = 0 115 | marker_max = 5 116 | mrk_array_msg = MarkerArray() 117 | initial_lap = True 118 | consecutive_points_off_opptraj = 0 119 | 120 | rospy.wait_for_message("/perception/obstacles", ObstacleArray) 121 | while not rospy.is_shutdown(): 122 | #sample data 123 | skip = False 124 | obstacle_array = self.obstacle_array 125 | position_in_map_frenet = self.position_in_map_frenet.copy() 126 | 127 | if index is not None and self.lap_count >= 1: 128 | time_diff = rospy.Time.now().to_sec() - raw_opponent_traj[index][6] 129 | #time_diff = 0 #for ROSBAG 130 | if time_diff > 0.5: #if the timestamp is older than 0.5 seconds we discard the measurement (variable) 131 | if points_in_lap > 5: #publish some extra information about the opponent trajectory (variable) 132 | projected_opponent_traj.nrofpoints = points_in_lap 133 | self.lap_count += trav_dist/track_length 134 | projected_opponent_traj.lapcount = self.lap_count 135 | projected_opponent_traj.opp_is_on_trajectory = True 136 | self.proj_opponent_trajectory_pub.publish(projected_opponent_traj) 137 | projected_opponent_traj = ProjOppTraj() 138 | raw_opponent_traj = np.zeros((number_of_wpnts,11)) 139 | 140 | traversed_distance = 0 141 | points_in_lap = 1 142 | first_point = True 143 | index = None 144 | else: 145 | projected_opponent_traj = ProjOppTraj() 146 | raw_opponent_traj = np.zeros((number_of_wpnts,11)) 147 | 148 | traversed_distance = 0 149 | points_in_lap = 1 150 | first_point = True 151 | index = None 152 | 153 | opponent = self.find_nearest_dyn_obstacle(obstacle_array=obstacle_array, position_in_map_frenet=position_in_map_frenet, track_length=track_length) 154 | if opponent is not None: 155 | old_index = index 156 | index = self.get_index(ego_s_sorted, opponent[0]) 157 | 158 | if index != old_index: #if the index is the same as the old index we dont update the opponent trajectory (this also gets rid of the delta time = 0 problem in simulation) 159 | raw_opponent_traj[index] = opponent #s, d, vs, vd, is_static, is_visible, timestamp, s_var, d_var, vs_var, vd_var 160 | 161 | 162 | trav_dist, proj_opponent, ignore_datapoint_flag, first_point, publish_flag = self.project_opponent_trajectory(old_index=old_index, index=index, 163 | raw_opponent_traj=raw_opponent_traj, 164 | proj_opponent_traj=projected_opponent_traj, 165 | track_length=track_length, 166 | trav_dist=traversed_distance, 167 | bound_left=bound_left, bound_right=bound_right, 168 | first_point=first_point) 169 | 170 | if ignore_datapoint_flag: 171 | index = old_index 172 | 173 | 174 | else: 175 | if self.lap_count >= 1 and self.opp_traj_count > 1: 176 | consecutive_points_off_opptraj = self.check_if_opponent_is_on_predicted_trajectory(proj_opponent=proj_opponent, 177 | consecutive_points_off_opptraj=consecutive_points_off_opptraj 178 | ,opponent_trajectory=self.opponent_trajectory) 179 | projected_opponent_traj.detections.append(proj_opponent) 180 | if first_point: 181 | projected_opponent_traj.detections.pop(0) 182 | traversed_distance += trav_dist 183 | first_point = False 184 | points_in_lap += 1 185 | 186 | mrk_msg = self._opp_to_marker(proj_opp=proj_opponent, id=marker_count, delete_bool=False) 187 | if len(mrk_array_msg.markers) > marker_max: 188 | del_mrk = mrk_array_msg.markers[0] 189 | mrk_array_msg.markers.pop(0) 190 | mrk_array_msg.markers.pop(0) 191 | 192 | del_mrk.action = Marker.DELETEALL 193 | mrk_array_msg.markers.append(del_mrk) 194 | mrk_array_msg.markers.append(mrk_msg) 195 | else: 196 | mrk_array_msg.markers.append(mrk_msg) 197 | self.marker_pub.publish(mrk_array_msg) 198 | marker_count += 1 199 | if consecutive_points_off_opptraj > 5 and traversed_distance < track_length/2: #if the opponent is off the predicted trajectory for more than 5 points we discard the measurement (variable) 200 | projected_opponent_traj.nrofpoints = points_in_lap 201 | projected_opponent_traj.lapcount = self.lap_count 202 | projected_opponent_traj.opp_is_on_trajectory = False 203 | opp_traj_count_old = self.opp_traj_count 204 | self.proj_opponent_trajectory_pub.publish(projected_opponent_traj) 205 | start_time = rospy.Time.now().to_sec() 206 | while(self.opp_traj_count == opp_traj_count_old and rospy.Time.now().to_sec() - start_time < 3): 207 | #Wait for 0.02 seconds to get new opponent trajectory 208 | time.sleep(0.02) 209 | obstacle_array = self.obstacle_array 210 | position_in_map_frenet = self.position_in_map_frenet.copy() 211 | opponent = self.find_nearest_dyn_obstacle(obstacle_array=obstacle_array, position_in_map_frenet=position_in_map_frenet, track_length=track_length) 212 | if opponent is not None: 213 | old_index = index 214 | index = self.get_index(ego_s_sorted, opponent[0]) 215 | 216 | if index != old_index: 217 | raw_opponent_traj[index] = opponent 218 | trav_dist, proj_opponent, ignore_datapoint_flag, first_point, publish_flag = self.project_opponent_trajectory(old_index=old_index, index=index, 219 | raw_opponent_traj=raw_opponent_traj, 220 | proj_opponent_traj=projected_opponent_traj, 221 | track_length=track_length, 222 | trav_dist=traversed_distance, 223 | bound_left=bound_left, bound_right=bound_right, 224 | first_point=first_point) 225 | if ignore_datapoint_flag: 226 | index = old_index 227 | else: 228 | projected_opponent_traj.detections.append(proj_opponent) 229 | if first_point: 230 | projected_opponent_traj.detections.pop(0) 231 | traversed_distance += trav_dist 232 | first_point = False 233 | points_in_lap += 1 234 | 235 | mrk_msg = self._opp_to_marker(proj_opp=proj_opponent, id=marker_count, delete_bool=False) 236 | if len(mrk_array_msg.markers) > marker_max: 237 | del_mrk = mrk_array_msg.markers[0] 238 | mrk_array_msg.markers.pop(0) 239 | mrk_array_msg.markers.pop(0) 240 | 241 | del_mrk.action = Marker.DELETEALL 242 | mrk_array_msg.markers.append(del_mrk) 243 | mrk_array_msg.markers.append(mrk_msg) 244 | else: 245 | mrk_array_msg.markers.append(mrk_msg) 246 | self.marker_pub.publish(mrk_array_msg) 247 | marker_count += 1 248 | consecutive_points_off_opptraj = 0 249 | 250 | 251 | 252 | 253 | #lap complete 254 | if traversed_distance > (track_length/2): 255 | self.lap_count += 0.5 256 | 257 | #delete first point of the projected opponent trajectory since the velocity is not known 258 | if initial_lap: 259 | initial_lap = False 260 | #checke for a large s gap between consecutive points 261 | for i in range(len(projected_opponent_traj.detections)-1): 262 | if (projected_opponent_traj.detections[i+1].s - projected_opponent_traj.detections[i].s)%self.track_length > 2.5: 263 | initial_lap = True 264 | start_index = i+1 265 | break 266 | if initial_lap: #start over from start_index 267 | #remove points before start_index 268 | for i in range(start_index): 269 | projected_opponent_traj.detections.pop(0) 270 | 271 | if len(projected_opponent_traj.detections) > 0: 272 | index = self.get_index(ego_s_sorted, projected_opponent_traj.detections[-1].s) 273 | points_in_lap = points_in_lap - start_index 274 | self.lap_count = 0 275 | #reset traversed distance 276 | traversed_distance = (projected_opponent_traj.detections[-1].s-projected_opponent_traj.detections[0].s)%self.track_length 277 | else: 278 | #reset everything 279 | projected_opponent_traj = ProjOppTraj() 280 | raw_opponent_traj = np.zeros((number_of_wpnts,11)) 281 | traversed_distance = 0 282 | points_in_lap = 0 283 | self.lap_count = 0 284 | index = None 285 | first_point = True 286 | 287 | if not initial_lap: 288 | projected_opponent_traj.nrofpoints = points_in_lap 289 | projected_opponent_traj.lapcount = self.lap_count 290 | projected_opponent_traj.opp_is_on_trajectory = True 291 | opp_traj_count_old = self.opp_traj_count 292 | 293 | # publish projected opponent trajectory 294 | self.proj_opponent_trajectory_pub.publish(projected_opponent_traj) 295 | 296 | #reset (make last point of old half lap the first point of the new half lap) 297 | projected_opponent_traj = ProjOppTraj() 298 | projected_opponent_traj.detections.append(proj_opponent) 299 | raw_opponent_traj = np.zeros((number_of_wpnts,11)) 300 | raw_opponent_traj[index] = opponent 301 | points_in_lap = 1 302 | traversed_distance = 0 303 | 304 | 305 | rate.sleep() 306 | 307 | #########################HELPER FUNCTIONS###################################### 308 | 309 | def find_nearest_dyn_obstacle(self, obstacle_array: ObstacleArray, position_in_map_frenet: np.ndarray, track_length: float) -> (np.ndarray, Obstacle): 310 | 311 | """Find the nearest dynamic obstacle and return the obstacle and the obstacle message""" 312 | 313 | if len(obstacle_array.obstacles) > 0 and len(position_in_map_frenet): 314 | allowed_distance = 4 315 | timestanp = obstacle_array.header.stamp 316 | closest_opp = track_length 317 | for obstacle in obstacle_array.obstacles: 318 | opponent_dist = (obstacle.s_start - position_in_map_frenet[0]) % track_length 319 | if opponent_dist <= closest_opp and not obstacle.is_static: 320 | closest_opp = opponent_dist 321 | opponent_static = obstacle.is_static 322 | opponent_s = obstacle.s_center 323 | opponent_d = obstacle.d_center 324 | opponent_vs = obstacle.vs 325 | opponent_vd = obstacle.vd 326 | opponent_visible = obstacle.is_visible 327 | s_var = obstacle.s_var 328 | d_var = obstacle.d_var 329 | vs_var = obstacle.vs_var 330 | vd_var = obstacle.vd_var 331 | opponent = [opponent_s, opponent_d, opponent_vs, opponent_vd, opponent_static, opponent_visible, 332 | timestanp, s_var, d_var, vs_var, vd_var] #s, d, vs, vd, is_static, is_visible, timestamp, s_var, d_var, vs_var, vd_var 333 | opponent[6] = opponent[6].to_sec() #convert timestamp to seconds (not supported by np array otherwise) 334 | allowed_distance = 6 #if we found an opponent we allow a bigger distance to the next opponent (variable) 335 | else: 336 | opponent = None 337 | 338 | if closest_opp > allowed_distance: #if the closest opponent is more than xm away we do not want to start trailing 339 | opponent = None 340 | else: 341 | opponent = None 342 | 343 | 344 | return opponent 345 | 346 | def check_if_opponent_is_on_predicted_trajectory(self, proj_opponent: ProjOppPoint, consecutive_points_off_opptraj: int, opponent_trajectory: OpponentTrajectory): 347 | 348 | """Check if the opponent is on the predicted trajectory""" 349 | #find corresponding oppwpnt to s value of opponent 350 | closest_oppwpnt = None 351 | for oppwpnt in self.opponent_trajectory.oppwpnts: 352 | if oppwpnt.s_m - proj_opponent.s > 0 and closest_oppwpnt is None: 353 | closest_oppwpnt = oppwpnt 354 | if closest_oppwpnt is not None: 355 | if abs(closest_oppwpnt.d_m - proj_opponent.d) < 0.3: #if the opponent is within 30cm of the predicted trajectory we assume that the prediction is correct (variable) 356 | consecutive_points_off_opptraj = 0 357 | else: 358 | consecutive_points_off_opptraj += 1 359 | return consecutive_points_off_opptraj 360 | 361 | 362 | def get_index(self, ego_s_sorted: list, opponent_s: float) -> int: 363 | 364 | """Get the index of the ego vehicle's s position that is closest to the opponent's s position """ 365 | 366 | index = 0 367 | for i in range(len(ego_s_sorted)): 368 | if ego_s_sorted[i] > opponent_s: 369 | index = i 370 | break 371 | return index 372 | 373 | def project_opponent_trajectory(self, old_index:int, index:int, raw_opponent_traj: np.ndarray, proj_opponent_traj: ProjOppTraj, trav_dist: float, track_length: float, first_point: bool = False, bound_left: list = None, bound_right: list = None) -> (float, np.ndarray, bool): 374 | 375 | 376 | """Make list of all opponent positions and velocities and return the list """ 377 | 378 | delta_time = 0 379 | opp_vs = 0 380 | opp_vd = 0 381 | trav_dist = 0 382 | publish_flag = False 383 | #For first point only do this 384 | projected_opponent = ProjOppPoint() 385 | projected_opponent.s = raw_opponent_traj[index][0] 386 | projected_opponent.d = raw_opponent_traj[index][1] 387 | projected_opponent.vs = 0 388 | projected_opponent.vd = 0 389 | projected_opponent.is_static = int(raw_opponent_traj[index][4]) 390 | projected_opponent.is_visible = int(raw_opponent_traj[index][5]) 391 | projected_opponent.time = raw_opponent_traj[index][6] 392 | projected_opponent.s_var = raw_opponent_traj[index][7] 393 | projected_opponent.d_var = raw_opponent_traj[index][8] 394 | projected_opponent.vs_var = raw_opponent_traj[index][9] 395 | projected_opponent.vd_var = raw_opponent_traj[index][10] 396 | 397 | discard_flag = False 398 | if(not first_point): 399 | diff_s = raw_opponent_traj[index][0] - raw_opponent_traj[old_index][0] 400 | diff_d = raw_opponent_traj[index][1] - raw_opponent_traj[old_index][1] 401 | delta_time, discard_flag = self._get_delta_time(old_index=old_index, index=index,opponent=raw_opponent_traj) 402 | 403 | #check if we are going "backwards" in the s direction we dicard this point to avoid duplicate points (no negativ s velicity possible for opponent) 404 | if diff_s < 0 and abs(diff_s) < self.treshold: 405 | discard_flag = True 406 | 407 | # around the origin 408 | elif diff_s < 0 and abs(diff_s) > self.treshold: 409 | diff_s = diff_s % track_length 410 | if len(proj_opponent_traj.detections) > 0: 411 | opp_vs = proj_opponent_traj.detections[-1].vs #do this since track_length is not very accurate (could also ignore this point) 412 | else: 413 | discard_flag = True 414 | opp_vd = diff_d / delta_time 415 | trav_dist += diff_s 416 | 417 | #normal case were s is increasing with time 418 | else: 419 | opp_vs = diff_s / delta_time 420 | opp_vd = diff_d / delta_time 421 | trav_dist += diff_s 422 | 423 | #Add projected velocity to the projected opponent 424 | projected_opponent.vs = opp_vs 425 | projected_opponent.vd = opp_vd 426 | 427 | #check if opp is on the track 428 | if projected_opponent.d<0: 429 | if abs(projected_opponent.d) > bound_right[index]: 430 | discard_flag = True 431 | else: 432 | if projected_opponent.d > bound_left[index]: 433 | discard_flag = True 434 | 435 | 436 | #leave out points where velocity or acceleration is over the threshold 437 | curr_vel = np.linalg.norm([raw_opponent_traj[index][2],raw_opponent_traj[index][3]]) 438 | old_vel = np.linalg.norm([raw_opponent_traj[old_index][2],raw_opponent_traj[old_index][3]]) 439 | delta_velocity= abs(curr_vel-old_vel)/delta_time 440 | if not discard_flag and (np.sqrt(opp_vs**2+opp_vd**2) > 15 or delta_velocity > 20): 441 | discard_flag = True 442 | 443 | if diff_s > 1 and self.lap_count>=1: #if detections are more than 1m apart (variable) 444 | discard_flag = True 445 | first_point = True 446 | publish_flag = True 447 | 448 | return trav_dist, projected_opponent, discard_flag, first_point, publish_flag 449 | 450 | 451 | 452 | 453 | def _get_delta_time(self, old_index:int, index:int, opponent: np.ndarray) -> float: 454 | 455 | """Get the delta time between two points in the opponent trajectory""" 456 | 457 | delta_time = opponent[index][6] - opponent[old_index][6] 458 | discard = False 459 | if delta_time < 0.01: 460 | discard = True 461 | delta_time = 0.04 462 | return delta_time, discard 463 | 464 | 465 | def _opp_to_marker(self, proj_opp: ProjOppPoint, id: int, delete_bool: bool = False) -> Marker: 466 | 467 | """Convert the opponent position to a marker""" 468 | cartesian_position = self.converter.get_cartesian(proj_opp.s, proj_opp.d) 469 | # Create a cylindrical marker to represent the opponent in RViz 470 | marker = Marker() 471 | marker.header.frame_id = "map" # Set the frame ID 472 | marker.type = Marker.CYLINDER 473 | marker.action = Marker.ADD if not delete_bool else Marker.DELETE 474 | marker.pose.position.x = cartesian_position[0] 475 | marker.pose.position.y = cartesian_position[1] 476 | marker.pose.position.z = 0.0 477 | marker.scale.x = 0.3 # Adjust the size of the cylinder as needed 478 | marker.scale.y = 0.3 479 | marker.scale.z = 1.0 480 | marker.color.r = 0.0 481 | marker.color.g = 1.0 482 | marker.color.b = 1.0 483 | marker.color.a = 1.0 484 | marker.id = id # Give each marker an ID 485 | return marker 486 | 487 | 488 | if __name__ == '__main__': 489 | 490 | 491 | node = Opponent_Trajectory() 492 | node.make_opponent_trajectory() 493 | rospy.spin() -------------------------------------------------------------------------------- /src/predictor_opponent_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import rospy 5 | from f110_msgs.msg import (ObstacleArray, OpponentTrajectory, Obstacle, Wpnt, OppWpnt, WpntArray, ProjOppTraj, ProjOppPoint) 6 | from nav_msgs.msg import Odometry 7 | from visualization_msgs.msg import Marker, MarkerArray 8 | import matplotlib.pyplot as plt 9 | from scipy.interpolate import interp1d 10 | from sklearn.gaussian_process import GaussianProcessRegressor 11 | from scipy.interpolate import UnivariateSpline 12 | from sklearn.gaussian_process.kernels import RBF, Matern, WhiteKernel, ConstantKernel 13 | import time 14 | from collections import defaultdict 15 | from sklearn.utils import check_random_state, check_array 16 | from scipy.optimize import fmin_cg 17 | from scipy.optimize import fmin_l_bfgs_b 18 | 19 | 20 | from frenet_converter.frenet_converter import FrenetConverter 21 | 22 | class GaussianProcessOppTraj(object): 23 | def __init__(self): 24 | #Node 25 | rospy.init_node('gaussian_process_opp_traj', anonymous=True) 26 | self.rate = rospy.Rate(10) 27 | self.opp_traj = OpponentTrajectory() 28 | self.opp_traj_gp = OpponentTrajectory() 29 | self.opp_positions_in_map_frenet = [] #testing with Rosbag 30 | 31 | 32 | 33 | #Subscribers 34 | rospy.Subscriber('/proj_opponent_trajectory', ProjOppTraj, self.proj_opp_traj_cb) 35 | rospy.Subscriber('/global_waypoints', WpntArray, self.glb_wpnts_cb) # global waypoints 36 | 37 | # rospy.Subscriber('/opp/car_state/odom_frenet', Odometry, self.opp_car_state_frenet_cb) #testing with Rosbag 38 | rospy.Subscriber('/opponent_trajectory', OpponentTrajectory, self.opp_traj_cb) #testing with Rosbag 39 | 40 | #Publishers 41 | self.opp_traj_gp_pub = rospy.Publisher('/opponent_trajectory', OpponentTrajectory, queue_size=10) 42 | self.opp_traj_marker_pub = rospy.Publisher('/opponent_traj_markerarray', MarkerArray, queue_size=10) 43 | 44 | #Frenet Converter 45 | rospy.wait_for_message("/global_waypoints", WpntArray) 46 | self.converter = self.initialize_converter() 47 | 48 | 49 | 50 | 51 | #Callback 52 | def proj_opp_traj_cb(self,data: ProjOppTraj): 53 | self.proj_opp_traj = data 54 | 55 | def glb_wpnts_cb(self, data): 56 | self.waypoints = np.array([[wpnt.x_m, wpnt.y_m] for wpnt in data.wpnts]) 57 | self.glb_wpnts = data 58 | self.track_length = data.wpnts[-1].s_m 59 | 60 | def opp_traj_cb(self, data: OpponentTrajectory): 61 | self.opponent_trajectory = data 62 | 63 | 64 | #Functions 65 | def initialize_converter(self) -> bool: 66 | 67 | """Initialize the FrenetConverter object""" 68 | 69 | rospy.wait_for_message("/global_waypoints", WpntArray) 70 | 71 | # Initialize the FrenetConverter object 72 | converter = FrenetConverter(self.waypoints[:, 0], self.waypoints[:, 1]) 73 | rospy.loginfo("[Tracking] initialized FrenetConverter object") 74 | 75 | return converter 76 | 77 | 78 | 79 | #Main Loop 80 | def get_gp_opp_traj(self): 81 | # Define the constant kernels with a lower bound for the constant_value parameter 82 | constant_kernel1_d = ConstantKernel(constant_value=0.5, constant_value_bounds=(1e-6, 1e3)) 83 | constant_kernel2_d = ConstantKernel(constant_value=0.2, constant_value_bounds=(1e-6, 1e3)) 84 | 85 | # Define the Gaussian Process kernel 86 | 87 | self.kernel_d = constant_kernel1_d * Matern(length_scale=1.0, nu=3/2) + constant_kernel2_d * WhiteKernel(noise_level=1) 88 | 89 | self.global_wpnts = rospy.wait_for_message("/global_waypoints", WpntArray) 90 | self.max_velocity = max([wnpt.vx_mps for wnpt in self.global_wpnts.wpnts]) 91 | ego_s_original = [wnpt.s_m for wnpt in self.global_wpnts.wpnts] 92 | ego_s_original.pop()#pop last point of original ego_s since it is double 93 | ego_s_doublelap = ego_s_original.copy() 94 | for i in range(len(ego_s_original)): 95 | ego_s_doublelap.append(ego_s_original[i]+self.track_length) 96 | self.opponent_trajectory = rospy.wait_for_message("/opponent_trajectory", OpponentTrajectory) 97 | proj_opp_traj = ProjOppTraj() 98 | 99 | while not rospy.is_shutdown(): 100 | proj_opp_traj = rospy.wait_for_message('/proj_opponent_trajectory', ProjOppTraj) 101 | oppwpnts_list = self.opponent_trajectory.oppwpnts 102 | self.lap_count = proj_opp_traj.lapcount 103 | opp_on_traj = proj_opp_traj.opp_is_on_trajectory 104 | nr_of_points = proj_opp_traj.nrofpoints 105 | if opp_on_traj == False and len(proj_opp_traj.detections) != 0: 106 | oppwpnts_list , opp_traj_marker_array = self.predict_opponent_trajectory(ego_s_doublelap=ego_s_doublelap, proj_opp_detections=proj_opp_traj.detections, oppwpnts_list=oppwpnts_list) 107 | opp_traj_gp_msg = self.make_opponent_trajectory_msg(oppwpnts_list=oppwpnts_list, lap_count=self.lap_count, raw_oppenent_traj_msg=proj_opp_traj) 108 | #Publish 109 | self.opp_traj_gp_pub.publish(opp_traj_gp_msg) 110 | self.opp_traj_marker_pub.publish(opp_traj_marker_array) 111 | 112 | 113 | 114 | 115 | #Helper Functions 116 | 117 | def predict_opponent_trajectory(self, ego_s_doublelap: list, proj_opp_detections:list, oppwpnts_list:list): 118 | 119 | """Make a prediction of how the opponent will return to his trajectory""" 120 | #expand oppwpnts_list to a double lap 121 | oppwpnts_list_doublelap = oppwpnts_list.copy() 122 | 123 | for i in range(len(oppwpnts_list)): 124 | new_oppwpnts = OppWpnt() 125 | new_oppwpnts.x_m = oppwpnts_list[i].x_m 126 | new_oppwpnts.y_m = oppwpnts_list[i].y_m 127 | new_oppwpnts.s_m = oppwpnts_list[i].s_m+self.track_length 128 | new_oppwpnts.d_m = oppwpnts_list[i].d_m 129 | new_oppwpnts.proj_vs_mps = oppwpnts_list[i].proj_vs_mps 130 | new_oppwpnts.vd_mps = oppwpnts_list[i].vd_mps 131 | oppwpnts_list_doublelap.append(new_oppwpnts) 132 | around_origin = False 133 | for i in range(len(proj_opp_detections)-1): 134 | if abs(proj_opp_detections[i].s - proj_opp_detections[i+1].s) > self.track_length/2 or around_origin: 135 | proj_opp_detections[i+1].s = proj_opp_detections[i+1].s+self.track_length 136 | around_origin = True 137 | 138 | nr_of_points_to_merge = 15 139 | gap = 7 140 | nr_of_detections = len(proj_opp_detections) 141 | last_s_of_detections = proj_opp_detections[-1].s 142 | 143 | first_index = nr_of_detections-min(nr_of_points_to_merge,nr_of_detections) 144 | 145 | opponent_s_sorted= [proj_opp_detections[i].s for i in range(first_index,nr_of_detections)] 146 | opponent_d_sorted= [proj_opp_detections[i].d for i in range(first_index,nr_of_detections)] 147 | opponent_vs_sorted= [proj_opp_detections[i].vs for i in range(first_index,nr_of_detections)] 148 | opponent_vd_sorted= [proj_opp_detections[i].vd for i in range(first_index,nr_of_detections)] 149 | counter = 0 150 | for i in range(len(oppwpnts_list_doublelap)): 151 | if oppwpnts_list_doublelap[i].s_m > last_s_of_detections+gap and counter < nr_of_points_to_merge: 152 | opponent_s_sorted.append(oppwpnts_list_doublelap[i].s_m) 153 | opponent_d_sorted.append(oppwpnts_list_doublelap[i].d_m) 154 | opponent_vs_sorted.append(oppwpnts_list_doublelap[i].proj_vs_mps) 155 | opponent_vd_sorted.append(oppwpnts_list_doublelap[i].vd_mps) 156 | counter = counter+1 157 | last_s_prediction = opponent_s_sorted[-1] 158 | first_s_prediction = opponent_s_sorted[0] 159 | 160 | opponent_s_sorted_reshape = np.array(opponent_s_sorted).reshape(-1, 1) 161 | opponent_d_sorted_reshape = np.array(opponent_d_sorted).reshape(-1, 1) 162 | opponent_vs_sorted_reshape = np.array(opponent_vs_sorted).reshape(-1, 1) 163 | opponent_vd_sorted_reshape = np.array(opponent_vd_sorted).reshape(-1, 1) 164 | 165 | s_pred = [] 166 | for i in range (len(ego_s_doublelap)): 167 | if ego_s_doublelap[i] <= last_s_prediction and ego_s_doublelap[i] >= first_s_prediction: 168 | s_pred.append(ego_s_doublelap[i]) 169 | 170 | 171 | def optimizer_wrapper(obj_func, initial_theta, bounds): 172 | solution, function_value, _ = fmin_l_bfgs_b(obj_func, initial_theta, bounds=bounds) 173 | return solution, function_value 174 | 175 | gpr_d = GaussianProcessRegressor(kernel=self.kernel_d, optimizer=optimizer_wrapper) 176 | 177 | 178 | # Fit the GPR model to the data 179 | gpr_d.fit(opponent_s_sorted_reshape, opponent_d_sorted_reshape) 180 | 181 | # Define a range of s values for prediction 182 | s_pred_reshape = np.array(s_pred).reshape(-1, 1) 183 | 184 | # Make predictions using the GPR model 185 | d_pred, sigma_d = gpr_d.predict(s_pred_reshape, return_std=True) 186 | 187 | resampled_opponent_d = d_pred 188 | #resampled_opponent_vs = np.interp(s_pred, opponent_s_sorted, opponent_vs_sorted) 189 | resampled_opponent_vd = np.interp(s_pred, opponent_s_sorted, opponent_vd_sorted) 190 | 191 | ego_s=[s_pred[i]%self.track_length for i in range(len(s_pred))] 192 | 193 | 194 | resampled_wpnts_xy = self.converter.get_cartesian(ego_s , resampled_opponent_d.tolist()) 195 | 196 | # replace all the entries where i have a corresponding ego_s with the predicted values 197 | i=0 198 | 199 | for i in range(len(oppwpnts_list)): 200 | for j in range(len(ego_s)): 201 | if abs(ego_s[j]-oppwpnts_list[i].s_m) < 1e-8: 202 | oppwpnts_list[i].x_m = resampled_wpnts_xy[0][j] 203 | oppwpnts_list[i].y_m = resampled_wpnts_xy[1][j] 204 | oppwpnts_list[i].d_m = resampled_opponent_d[j] 205 | #oppwpnts_list[i].proj_vs_mps = resampled_opponent_vs[j] 206 | oppwpnts_list[i].vd_mps = resampled_opponent_vd[j] 207 | oppwpnts_list[i].d_var = sigma_d[j] 208 | oppwpnts_list[i].vs_var = 69 209 | opp_traj_marker_array = self._visualize_opponent_wpnts(oppwpnts_list=oppwpnts_list) 210 | return oppwpnts_list , opp_traj_marker_array 211 | 212 | 213 | 214 | 215 | 216 | def make_opponent_trajectory_msg(self, oppwpnts_list: list, lap_count: int, raw_oppenent_traj_msg: ObstacleArray): 217 | 218 | """Make the opponent trajectory message and return it""" 219 | 220 | opponent_trajectory_msg = OpponentTrajectory() 221 | opponent_trajectory_msg.header.seq = lap_count 222 | opponent_trajectory_msg.header.stamp = rospy.Time.now() 223 | opponent_trajectory_msg.header.frame_id = "opponent_trajectory" 224 | opponent_trajectory_msg.lap_count = lap_count 225 | opponent_trajectory_msg.oppwpnts = oppwpnts_list 226 | 227 | return opponent_trajectory_msg 228 | 229 | 230 | 231 | def _visualize_opponent_wpnts(self, oppwpnts_list: list): 232 | 233 | """Visualize the resampled opponent trajectory as a marker array""" 234 | opp_traj_marker_array = MarkerArray() 235 | 236 | i=0 237 | for i in range(len(oppwpnts_list)): 238 | marker_height = oppwpnts_list[i].proj_vs_mps/self.max_velocity 239 | 240 | marker = Marker(header=rospy.Header(frame_id="map"), id = i, type = Marker.CYLINDER) 241 | marker.pose.position.x = oppwpnts_list[i].x_m 242 | marker.pose.position.y = oppwpnts_list[i].y_m 243 | marker.pose.position.z = marker_height/2 244 | marker.pose.orientation.w = 1.0 245 | marker.scale.x = min(max(5 * oppwpnts_list[i].d_var, 0.07),0.7) 246 | marker.scale.y = min(max(5 * oppwpnts_list[i].d_var, 0.07),0.7) 247 | marker.scale.z = marker_height 248 | if oppwpnts_list[i].vs_var == 69: 249 | marker.color.a = 0.8 250 | marker.color.r = 1.0 251 | marker.color.g = 0.0 252 | marker.color.b = 0.0 253 | else: 254 | marker.color.a = 1.0 255 | marker.color.r = 1.0 256 | marker.color.g = 1.0 257 | marker.color.b = 0.0 258 | opp_traj_marker_array.markers.append(marker)#frenpy 259 | 260 | 261 | return opp_traj_marker_array 262 | 263 | 264 | 265 | 266 | 267 | if __name__ == '__main__': 268 | 269 | 270 | node = GaussianProcessOppTraj() 271 | node.get_gp_opp_traj() 272 | rospy.spin() -------------------------------------------------------------------------------- /src/sqp_avoidance_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import time 3 | import rospy 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from f110_msgs.msg import Wpnt, WpntArray, Obstacle, ObstacleArray, OTWpntArray, OpponentTrajectory, OppWpnt 7 | from dynamic_reconfigure.msg import Config 8 | from visualization_msgs.msg import MarkerArray, Marker 9 | from std_msgs.msg import Float32MultiArray, Float32 10 | import matplotlib.pyplot as plt 11 | from scipy.optimize import minimize 12 | from frenet_converter.frenet_converter import FrenetConverter 13 | from std_msgs.msg import Bool 14 | from copy import deepcopy 15 | 16 | class SQPAvoidanceNode: 17 | """ 18 | This class implements a ROS node that creates a overtaking trajectory around osbtacles and opponents. 19 | 20 | It subscribes to the following topics: 21 | - `/perception/obstacles`: Subscribes to the obstacle array. 22 | - `/collision_prediction/obstacles`: Subscribes to the predicted obstacle array (ROCs). 23 | - `/car_state/odom_frenet`: Subscribes to the car state in Frenet coordinates. 24 | - `/global_waypoints`: Subscribes to the global waypoints. 25 | - `/global_waypoints_scaled`: Subscribes to the scaled global waypoints. 26 | - `/global_waypoints_updated`: Subscribes to the updated global waypoints. 27 | - `/local_waypoints`: Subscribes to the local waypoints. 28 | - `/opponent_waypoints`: Subscribes to the opponent waypoints. 29 | - `/ot_section_check`: Subscribes to the overtaking section check. 30 | - `/dynamic_sqp_tuner_node/parameter_updates`: Subscribes to the dynamic reconfigure updates. 31 | 32 | The node publishes the following topics: 33 | - `/planner/avoidance/markers_sqp`: Publishes the markers for the avoidance trajectory. 34 | - `/planner/avoidance/otwpnts`: Publishes the overtaking waypoints. 35 | - `/planner/avoidance/merger`: Publishes the merger region of the overtaking trajectory. 36 | - `/planner/pspliner_sqp/latency`: Publishes the latency of the SQP solver. (Only if measure is set to True) 37 | """ 38 | 39 | def __init__(self): 40 | # Initialize node 41 | rospy.init_node('sqp_avoidance_node') 42 | self.rate = rospy.Rate(20) 43 | 44 | # Params 45 | self.frenet_state = Odometry() 46 | self.local_wpnts = None 47 | self.lookahead = 15 48 | self.past_avoidance_d = [] 49 | # Scaled waypoints params 50 | self.scaled_wpnts = None 51 | self.scaled_wpnts_msg = WpntArray() 52 | self.scaled_vmax = None 53 | self.scaled_max_idx = None 54 | self.scaled_max_s = None 55 | self.scaled_delta_s = None 56 | # Updated waypoints params 57 | self.wpnts_updated = None 58 | self.max_s_updated = None 59 | self.max_idx_updated = None 60 | # Obstalces params 61 | self.obs = ObstacleArray() 62 | self.obs_perception = ObstacleArray() 63 | self.obs_predict = ObstacleArray() 64 | # Opponent waypoint params 65 | self.opponent_waypoints = OpponentTrajectory() 66 | self.max_opp_idx = None 67 | self.opponent_wpnts_sm = None 68 | # OT params 69 | self.last_ot_side = "" 70 | self.ot_section_check = False 71 | # Solver params 72 | self.min_radius = 0.05 # wheelbase / np.tan(max_steering) 73 | self.max_kappa = 1/self.min_radius 74 | self.width_car = 0.28 75 | self.avoidance_resolution = 20 76 | self.back_to_raceline_before = 5 77 | self.back_to_raceline_after = 5 78 | self.obs_traj_tresh = 2 79 | 80 | # Dynamic sovler params 81 | self.down_sampled_delta_s = None 82 | self.global_traj_kappas = None 83 | 84 | # ROS Parameters 85 | self.opponent_traj_topic = '/opponent_trajectory' 86 | self.measure = rospy.get_param("/measure", False) 87 | 88 | # Dynamic reconf params 89 | self.avoid_static_obs = True 90 | 91 | self.converter = None 92 | self.global_waypoints = None 93 | 94 | # Subscribers 95 | rospy.Subscriber("/perception/obstacles", ObstacleArray, self.obs_perception_cb) 96 | rospy.Subscriber("/collision_prediction/obstacles", ObstacleArray, self.obs_prediction_cb) 97 | rospy.Subscriber("/car_state/odom_frenet", Odometry, self.state_cb) 98 | rospy.Subscriber("/global_waypoints_scaled", WpntArray, self.scaled_wpnts_cb) 99 | rospy.Subscriber("/local_waypoints", WpntArray, self.local_wpnts_cb) 100 | rospy.Subscriber("/dynamic_sqp_tuner_node/parameter_updates", Config, self.dyn_param_cb) 101 | rospy.Subscriber("/global_waypoints", WpntArray, self.gb_cb) 102 | rospy.Subscriber("/global_waypoints_updated", WpntArray, self.updated_wpnts_cb) 103 | rospy.Subscriber(self.opponent_traj_topic, OpponentTrajectory, self.opponent_trajectory_cb) 104 | rospy.Subscriber("/ot_section_check", Bool, self.ot_sections_check_cb) 105 | # Publishers 106 | self.mrks_pub = rospy.Publisher("/planner/avoidance/markers_sqp", MarkerArray, queue_size=10) 107 | self.evasion_pub = rospy.Publisher("/planner/avoidance/otwpnts", OTWpntArray, queue_size=10) 108 | self.merger_pub = rospy.Publisher("/planner/avoidance/merger", Float32MultiArray, queue_size=10) 109 | if self.measure: 110 | self.measure_pub = rospy.Publisher("/planner/pspliner_sqp/latency", Float32, queue_size=10) 111 | 112 | self.converter = self.initialize_converter() 113 | 114 | ### Callbacks ### 115 | def obs_perception_cb(self, data: ObstacleArray): 116 | self.obs_perception = data 117 | self.obs_perception.obstacles = [obs for obs in data.obstacles if obs.is_static == True] 118 | if self.avoid_static_obs == True: 119 | self.obs.header = data.header 120 | self.obs.obstacles = self.obs_perception.obstacles + self.obs_predict.obstacles 121 | 122 | def obs_prediction_cb(self, data: ObstacleArray): 123 | self.obs_predict = data 124 | self.obs = self.obs_predict 125 | if self.avoid_static_obs == True: 126 | self.obs.obstacles = self.obs.obstacles + self.obs_perception.obstacles 127 | 128 | def state_cb(self, data: Odometry): 129 | self.frenet_state = data 130 | 131 | def gb_cb(self, data: WpntArray): 132 | self.global_waypoints = np.array([[wpnt.x_m, wpnt.y_m] for wpnt in data.wpnts]) 133 | 134 | # Everything is refered to the SCALED global waypoints 135 | def scaled_wpnts_cb(self, data: WpntArray): 136 | self.scaled_wpnts = np.array([[wpnt.s_m, wpnt.d_m] for wpnt in data.wpnts]) 137 | self.scaled_wpnts_msg = data 138 | v_max = np.max(np.array([wpnt.vx_mps for wpnt in data.wpnts])) 139 | if self.scaled_vmax != v_max: 140 | self.scaled_vmax = v_max 141 | self.scaled_max_idx = data.wpnts[-1].id 142 | self.scaled_max_s = data.wpnts[-1].s_m 143 | self.scaled_delta_s = data.wpnts[1].s_m - data.wpnts[0].s_m 144 | 145 | def updated_wpnts_cb(self, data: WpntArray): 146 | self.wpnts_updated = data.wpnts[:-1] 147 | self.max_s_updated = self.wpnts_updated[-1].s_m 148 | self.max_idx_updated = self.wpnts_updated[-1].id 149 | 150 | def local_wpnts_cb(self, data: WpntArray): 151 | self.local_wpnts = np.array([[wpnt.s_m, wpnt.d_m] for wpnt in data.wpnts]) 152 | 153 | def opponent_trajectory_cb(self, data: OpponentTrajectory): 154 | self.opponent_waypoints = data.oppwpnts 155 | self.max_opp_idx = len(data.oppwpnts)-1 156 | self.opponent_wpnts_sm = np.array([wpnt.s_m for wpnt in data.oppwpnts]) 157 | 158 | def ot_sections_check_cb(self, data: Bool): 159 | self.ot_section_check = data.data 160 | 161 | # Callback triggered by dynamic spline reconf 162 | def dyn_param_cb(self, params: Config): 163 | self.evasion_dist = rospy.get_param("dynamic_sqp_tuner_node/evasion_dist", 0.65) 164 | self.obs_traj_tresh = rospy.get_param("dynamic_sqp_tuner_node/obs_traj_tresh", 1.5) 165 | self.spline_bound_mindist = rospy.get_param("dynamic_sqp_tuner_node/spline_bound_mindist", 0.2) 166 | self.lookahead = rospy.get_param("dynamic_sqp_tuner_node/lookahead_dist", 15) 167 | self.avoidance_resolution = rospy.get_param("dynamic_sqp_tuner_node/avoidance_resolution", 20) 168 | self.back_to_raceline_before = rospy.get_param("dynamic_sqp_tuner_node/back_to_raceline_before", 5) 169 | self.back_to_raceline_after = rospy.get_param("dynamic_sqp_tuner_node/back_to_raceline_after", 5) 170 | self.avoid_static_obs = rospy.get_param("dynamic_sqp_tuner_node/avoid_static_obs", True) 171 | 172 | print( 173 | f"[Planner] Dynamic reconf triggered new spline params: \n" 174 | f" Evasion apex distance: {self.evasion_dist} [m],\n" 175 | f" Obstacle trajectory treshold: {self.obs_traj_tresh} [m]\n" 176 | f" Spline boundary mindist: {self.spline_bound_mindist} [m]\n" 177 | f" Lookahead distance: {self.lookahead} [m]\n" 178 | f" Avoid static obstacles: {self.avoid_static_obs}\n" 179 | f" Avoidance resolution: {self.avoidance_resolution}\n" 180 | f" Back to raceline before: {self.back_to_raceline_before} [m]\n" 181 | f" Back to raceline after: {self.back_to_raceline_after} [m]\n" 182 | ) 183 | 184 | def loop(self): 185 | # Wait for critical Messages and services 186 | rospy.loginfo("[OBS Spliner] Waiting for messages and services...") 187 | rospy.wait_for_message("/global_waypoints_scaled", WpntArray) 188 | rospy.wait_for_message("/car_state/odom", Odometry) 189 | rospy.wait_for_message("/dynamic_sqp_tuner_node/parameter_updates", Config) 190 | rospy.wait_for_message("/local_waypoints",WpntArray) 191 | rospy.loginfo("[OBS Spliner] Ready!") 192 | 193 | while not rospy.is_shutdown(): 194 | start_time = time.perf_counter() 195 | obs = deepcopy(self.obs) 196 | mrks = MarkerArray() 197 | frenet_state = self.frenet_state 198 | self.current_d = frenet_state.pose.pose.position.y 199 | self.cur_s = frenet_state.pose.pose.position.x 200 | 201 | # Obstacle pre-processing 202 | obs.obstacles = sorted(obs.obstacles, key=lambda obs: obs.s_start) 203 | considered_obs = [] 204 | for obs in obs.obstacles: 205 | if abs(obs.d_center) < self.obs_traj_tresh and (obs.s_start - self.cur_s) % self.scaled_max_s < self.lookahead: 206 | considered_obs.append(obs) 207 | 208 | # If there is an obstacle and we are in OT section 209 | if len(considered_obs) > 0 and self.ot_section_check == True: 210 | evasion_x, evasion_y, evasion_s, evasion_d, evasion_v = self.sqp_solver(considered_obs, frenet_state.pose.pose.position.x) 211 | # Publish merge reagion if evasion track has been found 212 | if len(evasion_s) > 0: 213 | self.merger_pub.publish(Float32MultiArray(data=[considered_obs[-1].s_end%self.scaled_max_s, evasion_s[-1]%self.scaled_max_s])) 214 | 215 | # IF there is no point in overtaking anymore delte all markers 216 | else: 217 | mrks = MarkerArray() 218 | del_mrk = Marker(header=rospy.Header(stamp=rospy.Time.now())) 219 | del_mrk.action = Marker.DELETEALL 220 | mrks.markers = [] 221 | mrks.markers.append(del_mrk) 222 | self.mrks_pub.publish(mrks) 223 | 224 | # publish latency 225 | if self.measure: 226 | self.measure_pub.publish(Float32(data=time.perf_counter() - start_time)) 227 | self.rate.sleep() 228 | 229 | def sqp_solver(self, considered_obs: list, cur_s: float): 230 | danger_flag = False 231 | # Get the initial guess of the overtaking side (see spliner) 232 | initial_guess_object = self.group_objects(considered_obs) 233 | initial_guess_object_start_idx = np.abs(self.scaled_wpnts - initial_guess_object.s_start).argmin() 234 | initial_guess_object_end_idx = np.abs(self.scaled_wpnts - initial_guess_object.s_end).argmin() 235 | # Get array of indexes of the global waypoints overlapping with the ROC 236 | gb_idxs = np.array(range(initial_guess_object_start_idx, initial_guess_object_start_idx + (initial_guess_object_end_idx - initial_guess_object_start_idx)%self.scaled_max_idx))%self.scaled_max_idx 237 | # If the ROC is too short, we take the next 20 waypoints 238 | if len(gb_idxs) < 20: 239 | gb_idxs = [int(initial_guess_object.s_center / self.scaled_delta_s + i) % self.scaled_max_idx for i in range(20)] 240 | 241 | side, initial_apex = self._more_space(initial_guess_object, self.scaled_wpnts_msg.wpnts, gb_idxs) 242 | kappas = np.array([self.scaled_wpnts_msg.wpnts[gb_idx].kappa_radpm for gb_idx in gb_idxs]) 243 | max_kappa = np.max(np.abs(kappas)) 244 | outside = "left" if np.sum(kappas) < 0 else "right" 245 | 246 | # Enlongate the ROC if our initial guess suggests that we are overtaking on the outside 247 | if side == outside: 248 | for i in range(len(considered_obs)): 249 | considered_obs[i].s_end = considered_obs[i].s_end + (considered_obs[i].s_end - considered_obs[i].s_start)%self.max_s_updated * max_kappa * (self.width_car + self.evasion_dist) 250 | 251 | min_s_obs_start = self.scaled_max_s 252 | max_s_obs_end = 0 253 | for obs in considered_obs: 254 | if obs.s_start < min_s_obs_start: 255 | min_s_obs_start = obs.s_start 256 | if obs.s_end > max_s_obs_end: 257 | max_s_obs_end = obs.s_end 258 | # Check if it is a really wide obstacle 259 | if obs.d_left > 3 or obs.d_right < -3: 260 | danger_flag = True 261 | 262 | # Get local waypoints to check where we are and where we are heading 263 | # If we are closer than threshold to the opponent use the first two local waypoints as start points 264 | start_avoidance = max((min_s_obs_start - self.back_to_raceline_before), cur_s) 265 | end_avoidance = max_s_obs_end + self.back_to_raceline_after 266 | 267 | # Get a downsampled version for s avoidance points 268 | s_avoidance = np.linspace(start_avoidance, end_avoidance, self.avoidance_resolution) 269 | self.down_sampled_delta_s = s_avoidance[1] - s_avoidance[0] 270 | # Get the closest scaled waypoint for every s avoidance point (down sampled) 271 | scaled_wpnts_indices = np.array([np.abs(self.scaled_wpnts[:, 0] - s % self.scaled_max_s).argmin() for s in s_avoidance]) 272 | # Get the scaled waypoints for every s avoidance point idx 273 | corresponding_scaled_wpnts = [self.scaled_wpnts_msg.wpnts[i] for i in scaled_wpnts_indices] 274 | # Get the boundaries for every s avoidance point 275 | bounds = np.array([(-wpnt.d_right + self.spline_bound_mindist, wpnt.d_left - self.spline_bound_mindist) for wpnt in corresponding_scaled_wpnts]) 276 | 277 | # Calculate curvature at each point using numerical differentiation 278 | # k = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2) 279 | x_global_points = np.array([wpnt.x_m for wpnt in corresponding_scaled_wpnts]) 280 | y_global_points = np.array([wpnt.y_m for wpnt in corresponding_scaled_wpnts]) 281 | x_prime = np.diff(x_global_points) 282 | x_prime = np.where(x_prime == 0, 1e-6, x_prime) # Avoid division by zero 283 | y_prime = np.diff(y_global_points) 284 | y_prime = np.where(y_prime == 0, 1e-6, y_prime) # Avoid division by zero 285 | x_prime_prime = np.diff(x_prime) 286 | y_prime_prime = np.diff(y_prime) 287 | x_prime = x_prime[:-1] # Make it the same length as x_prime_prime 288 | y_prime = y_prime[:-1] # Make it the same length as y_prime_prime 289 | self.global_traj_kappas = (x_prime*y_prime_prime - y_prime*x_prime_prime) / ((x_prime**2 + y_prime**2)**(3/2)) 290 | 291 | # Create a list of indices which overlap with the obstacles 292 | # Get the centerline of the obstacles and enforce a min distance to the obstacles 293 | self.obs_downsampled_indices = np.array([]) 294 | self.obs_downsampled_center_d = np.array([]) 295 | self.obs_downsampled_min_dist = np.array([]) 296 | 297 | for obs in considered_obs: 298 | obs_idx_start = np.abs(s_avoidance - obs.s_start).argmin() 299 | obs_idx_end = np.abs(s_avoidance - obs.s_end).argmin() 300 | 301 | if obs_idx_start < len(s_avoidance) - 2: # Sanity check 302 | if obs.is_static == True or obs_idx_end == obs_idx_start: 303 | if obs_idx_end == obs_idx_start: 304 | obs_idx_end = obs_idx_start + 1 305 | self.obs_downsampled_indices = np.append(self.obs_downsampled_indices, np.arange(obs_idx_start, obs_idx_end + 1)) 306 | self.obs_downsampled_center_d = np.append(self.obs_downsampled_center_d, np.full(obs_idx_end - obs_idx_start + 1, (obs.d_left + obs.d_right) / 2)) 307 | self.obs_downsampled_min_dist = np.append(self.obs_downsampled_min_dist, np.full(obs_idx_end - obs_idx_start + 1, (obs.d_left - obs.d_right) / 2 + self.width_car + self.evasion_dist)) 308 | else: 309 | indices = np.arange(obs_idx_start, obs_idx_end + 1) 310 | self.obs_downsampled_indices = np.append(self.obs_downsampled_indices, indices) 311 | opp_wpnts_idx = [np.abs(self.opponent_wpnts_sm - s_avoidance[int(idx)]%self.max_opp_idx).argmin() for idx in indices] 312 | d_opp_downsampled_array = np.array([self.opponent_waypoints[opp_idx].d_m for opp_idx in opp_wpnts_idx]) 313 | self.obs_downsampled_center_d = np.append(self.obs_downsampled_center_d, d_opp_downsampled_array) 314 | self.obs_downsampled_min_dist = np.append(self.obs_downsampled_min_dist, np.full(obs_idx_end - obs_idx_start + 1, self.width_car + self.evasion_dist)) 315 | else: 316 | rospy.loginfo("[OBS Spliner] Obstacle end index is smaller than start index") 317 | rospy.loginfo("[OBS Spliner] len obs: " + str(len(considered_obs)) + "obs_start:" + str(obs.s_start) + "obs_end:" + str(obs.s_end) + " obs_idx_start: " + str(obs_idx_start) + " obs_idx_end: " + str(obs_idx_end) + " len s_avoidance: " + str(len(s_avoidance)) + "s avoidance 0:" + str(s_avoidance[0]) + " s avoidance -1: " + str(s_avoidance[-1])) 318 | 319 | 320 | self.obs_downsampled_indices = self.obs_downsampled_indices.astype(int) 321 | 322 | # Get the min radius 323 | # Clip speed between 1 and 7 m/s 324 | clipped_speed = np.clip(self.frenet_state.twist.twist.linear.x, 1, 6.5) 325 | # Get the minimum of clipped speed and the updated speed of the first waypoints 326 | radius_speed = min([clipped_speed, self.wpnts_updated[(scaled_wpnts_indices[0])%self.max_idx_updated].vx_mps]) 327 | # Interpolate the min_radius with speeds between 0.2 and 7 m 328 | self.min_radius = np.interp(radius_speed, [1, 6, 7], [0.2, 2, 4]) 329 | self.max_kappa = 1/self.min_radius 330 | 331 | if len(self.past_avoidance_d) == 0: 332 | initial_guess = np.full(len(s_avoidance), initial_apex) 333 | 334 | elif len(self.past_avoidance_d) > 0: 335 | initial_guess = self.past_avoidance_d 336 | else: 337 | #TODO: Remove -> print("this happend") 338 | if self.last_ot_side == "left": 339 | initial_guess = np.full(len(s_avoidance), 2) 340 | else: 341 | initial_guess = np.full(len(s_avoidance), -2) 342 | 343 | result = self.solve_sqp(initial_guess, bounds) 344 | 345 | # if len(self.obs_downsampled_indices) < 2 or danger_flag == True: 346 | # result.success = False 347 | 348 | if result.success == True: 349 | # Create a new s array for the global waypoints as close to delta s as possible 350 | n_global_avoidance_points = int((end_avoidance - start_avoidance) / self.scaled_delta_s) 351 | s_array = np.linspace(start_avoidance, end_avoidance, n_global_avoidance_points) 352 | # Interpolate corresponding d values 353 | evasion_d = np.interp(s_array, s_avoidance, result.x) 354 | # Solve rap around problem 355 | evasion_s = np.mod(s_array, self.scaled_max_s) 356 | # Get the corresponding x and y values 357 | resp = self.converter.get_cartesian(evasion_s, evasion_d) 358 | evasion_x = resp[0, :] 359 | evasion_y = resp[1, :] 360 | # Get the corresponding v values 361 | downsampled_v = np.array([wpnt.vx_mps for wpnt in corresponding_scaled_wpnts]) 362 | evasion_v = np.interp(s_array, s_avoidance, downsampled_v) 363 | # Create a new evasion waypoint message 364 | evasion_wpnts_msg = OTWpntArray(header=rospy.Header(stamp=rospy.Time.now(), frame_id="map")) 365 | evasion_wpnts = [] 366 | evasion_wpnts = [Wpnt(id=len(evasion_wpnts), s_m=s, d_m=d, x_m=x, y_m=y, vx_mps= v) for x, y, s, d, v in zip(evasion_x, evasion_y, evasion_s, evasion_d, evasion_v)] 367 | evasion_wpnts_msg.wpnts = evasion_wpnts 368 | self.past_avoidance_d = result.x 369 | mean_d = np.mean(evasion_d) 370 | if mean_d > 0: 371 | self.last_ot_side = "left" 372 | else: 373 | self.last_ot_side = "right" 374 | # print("[OBS Spliner] SQP solver successfull") 375 | 376 | else: 377 | evasion_x = [] 378 | evasion_y = [] 379 | evasion_s = [] 380 | evasion_d = [] 381 | evasion_v = [] 382 | evasion_wpnts_msg = OTWpntArray(header=rospy.Header(stamp=rospy.Time.now(), frame_id="map")) 383 | evasion_wpnts_msg.wpnts = [] 384 | self.past_avoidance_d = [] 385 | 386 | self.evasion_pub.publish(evasion_wpnts_msg) 387 | self.visualize_sqp(evasion_s, evasion_d, evasion_x, evasion_y, evasion_v) 388 | 389 | return evasion_x, evasion_y, evasion_s, evasion_d, evasion_v 390 | 391 | 392 | ### Optimal Trajectory Solver ### 393 | def objective_function(self, d): 394 | return np.sum((d) ** 2) * 10 + np.sum(np.diff(np.diff(d))**2) * 100 + (np.diff(d)[0] ** 2) * 1000 395 | 396 | ## Constraint functions ## 397 | def start_on_raceline_constraint(self, d): # And end on raceline 398 | return np.array([0.02 - abs(d[0] - self.current_d), 0.02 - abs(d[-2]), 0.02 - abs(d[-1])]) 399 | 400 | def obstacle_constraint(self, d): 401 | distance_to_obstacle = np.abs(d[self.obs_downsampled_indices] - self.obs_downsampled_center_d) 402 | violation = distance_to_obstacle - self.obs_downsampled_min_dist 403 | return violation 404 | 405 | # Prevents points from jumping trhough obstacles due to resoultion isses 406 | def consecutive_points_constraint(self, d): 407 | # Extract the relevant points 408 | points = d[self.obs_downsampled_indices] 409 | 410 | # Check the condition for each pair of consecutive points 411 | violations = [] 412 | for i in range(len(points) - 1): 413 | if not ((points[i] > self.obs_downsampled_center_d[i] and points[i+1] > self.obs_downsampled_center_d[i+1]) or 414 | (points[i] < self.obs_downsampled_center_d[i] and points[i+1] < self.obs_downsampled_center_d[i+1])): 415 | violations.append(-1) # Add a violation as a negative value if the condition is not met 416 | else: 417 | violations.append(1) 418 | return violations 419 | 420 | def turning_radius_constraint(self, d): 421 | # Calculate curvature at each point using numerical differentiation 422 | # k = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2) 423 | # x' = self.down_sampled_delta_s, x'' = 0 424 | 425 | y_prime = np.diff(d) 426 | y_prime = np.where(y_prime == 0, 1e-6, y_prime) # Avoid division by zero 427 | y_prime_prime = np.diff(y_prime) 428 | y_prime = y_prime[:-1] # Make it the same length as y_prime_prime 429 | 430 | kappa = (self.down_sampled_delta_s * y_prime_prime) / ((self.down_sampled_delta_s ** 2) ** (3/2)) 431 | # np.diff losses last two points so we delete them from self.global_traj_kappas 432 | total_kappa = self.global_traj_kappas - kappa 433 | violation = self.max_kappa - abs(total_kappa) 434 | return violation 435 | 436 | # The arctan of of (d[1]-d[0])/ delta_s_sample_points < than 45 degrees 437 | def first_point_constraint(self, d): 438 | return np.array([self.down_sampled_delta_s - abs(d[1]-d[0])]) 439 | 440 | def combined_equality_constraints(self, d): 441 | return self.start_on_raceline_constraint(d) 442 | 443 | def combined_inequality_constraints(self, d): 444 | return np.concatenate([self.obstacle_constraint(d), self.consecutive_points_constraint(d), self.turning_radius_constraint(d), self.first_point_constraint(d)]) # 445 | 446 | def solve_sqp(self, d_array, track_boundaries): 447 | result = minimize( 448 | self.objective_function, d_array, method='SLSQP', jac='10-point', 449 | bounds=track_boundaries, 450 | constraints=[ 451 | {'type': 'eq', 'fun': self.combined_equality_constraints}, 452 | {'type': 'ineq', 'fun': self.combined_inequality_constraints} 453 | ], 454 | options={'ftol': 1e-1, 'maxiter': 20, 'disp': False}, 455 | ) 456 | return result 457 | 458 | def group_objects(self, obstacles: list): 459 | # Group obstacles that are close to each other 460 | initial_guess_object = obstacles[0] 461 | for obs in obstacles: 462 | if obs.d_left > initial_guess_object.d_left: 463 | initial_guess_object.d_left = obs.d_left 464 | if obs.d_right < initial_guess_object.d_right: 465 | initial_guess_object.d_right = obs.d_right 466 | if obs.s_start < initial_guess_object.s_start: 467 | initial_guess_object.s_start = obs.s_start 468 | if obs.s_end > initial_guess_object.s_end: 469 | initial_guess_object.s_end = obs.s_end 470 | initial_guess_object.s_center = (initial_guess_object.s_start + initial_guess_object.s_end) / 2 471 | return initial_guess_object 472 | 473 | def _more_space(self, obstacle: Obstacle, gb_wpnts, gb_idxs): 474 | left_boundary_mean = np.mean([gb_wpnts[gb_idx].d_left for gb_idx in gb_idxs]) 475 | right_boundary_mean = np.mean([gb_wpnts[gb_idx].d_right for gb_idx in gb_idxs]) 476 | left_gap = abs(left_boundary_mean - obstacle.d_left) 477 | right_gap = abs(right_boundary_mean + obstacle.d_right) 478 | min_space = self.evasion_dist + self.spline_bound_mindist 479 | 480 | if right_gap > min_space and left_gap < min_space: 481 | # Compute apex distance to the right of the opponent 482 | d_apex_right = obstacle.d_right - self.evasion_dist 483 | # If we overtake to the right of the opponent BUT the apex is to the left of the raceline, then we set the apex to 0 484 | if d_apex_right > 0: 485 | d_apex_right = 0 486 | return "right", d_apex_right 487 | 488 | elif left_gap > min_space and right_gap < min_space: 489 | # Compute apex distance to the left of the opponent 490 | d_apex_left = obstacle.d_left + self.evasion_dist 491 | # If we overtake to the left of the opponent BUT the apex is to the right of the raceline, then we set the apex to 0 492 | if d_apex_left < 0: 493 | d_apex_left = 0 494 | return "left", d_apex_left 495 | else: 496 | candidate_d_apex_left = obstacle.d_left + self.evasion_dist 497 | candidate_d_apex_right = obstacle.d_right - self.evasion_dist 498 | 499 | if abs(candidate_d_apex_left) <= abs(candidate_d_apex_right): 500 | # If we overtake to the left of the opponent BUT the apex is to the right of the raceline, then we set the apex to 0 501 | if candidate_d_apex_left < 0: 502 | candidate_d_apex_left = 0 503 | return "left", candidate_d_apex_left 504 | else: 505 | # If we overtake to the right of the opponent BUT the apex is to the left of the raceline, then we set the apex to 0 506 | if candidate_d_apex_right > 0: 507 | candidate_d_apex_right = 0 508 | return "right", candidate_d_apex_right 509 | 510 | ### Visualize SQP Rviz### 511 | def visualize_sqp(self, evasion_s, evasion_d, evasion_x, evasion_y, evasion_v): 512 | mrks = MarkerArray() 513 | if len(evasion_s) == 0: 514 | pass 515 | else: 516 | resp = self.converter.get_cartesian(evasion_s, evasion_d) 517 | for i in range(len(evasion_s)): 518 | mrk = Marker(header=rospy.Header(stamp=rospy.Time.now(), frame_id="map")) 519 | mrk.type = mrk.CYLINDER 520 | mrk.scale.x = 0.1 521 | mrk.scale.y = 0.1 522 | mrk.scale.z = evasion_v[i] / self.scaled_vmax 523 | mrk.color.a = 1.0 524 | mrk.color.g = 0.13 525 | mrk.color.r = 0.63 526 | mrk.color.b = 0.94 527 | 528 | mrk.id = i 529 | mrk.pose.position.x = evasion_x[i] 530 | mrk.pose.position.y = evasion_y[i] 531 | mrk.pose.position.z = evasion_v[i] / self.scaled_vmax / 2 532 | mrk.pose.orientation.w = 1 533 | mrks.markers.append(mrk) 534 | self.mrks_pub.publish(mrks) 535 | 536 | def initialize_converter(self) -> bool: 537 | """ 538 | Initialize the FrenetConverter object""" 539 | rospy.wait_for_message("/global_waypoints", WpntArray) 540 | 541 | # Initialize the FrenetConverter object 542 | converter = FrenetConverter(self.global_waypoints[:, 0], self.global_waypoints[:, 1]) 543 | rospy.loginfo("[Spliner] initialized FrenetConverter object") 544 | 545 | return converter 546 | 547 | 548 | if __name__ == "__main__": 549 | SQPAvoidance = SQPAvoidanceNode() 550 | SQPAvoidance.loop() -------------------------------------------------------------------------------- /src/update_waypoints.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import numpy as np 5 | import rospy 6 | from nav_msgs.msg import Odometry 7 | from f110_msgs.msg import WpntArray 8 | from visualization_msgs.msg import MarkerArray, Marker 9 | from frenet_conversion.srv import Frenet2GlobArr 10 | from std_msgs.msg import String 11 | import time 12 | from dynamic_reconfigure.msg import Config 13 | 14 | 15 | class UpdateWaypoints: 16 | """ 17 | Update the scaled waypoints to get a better estimate of ones positon and speed. 18 | Since this has to be done fast, it is done in the callbacks. 19 | """ 20 | 21 | def __init__(self): 22 | # Initialize the node 23 | rospy.init_node('waypoint_updater', anonymous=True) 24 | 25 | # Subscriber 26 | rospy.Subscriber("/car_state/odom_frenet", Odometry, self.odom_cb) 27 | rospy.Subscriber("/state_machine", String, self.state_machine_cb) 28 | rospy.Subscriber("/dynamic_collision_tuner_node/parameter_updates", Config, self.dyn_param_cb) 29 | 30 | # Waypoint publisher 31 | self.wpnts_updated_pub = rospy.Publisher("/global_waypoints_updated", WpntArray, queue_size=10) 32 | self.marker_pub = rospy.Publisher("/updated_waypoints_marker", MarkerArray, queue_size=10) 33 | 34 | # Init and params 35 | self.frenet2glob = rospy.ServiceProxy("convert_frenet2globarr_service", Frenet2GlobArr) 36 | 37 | # Adaptive rate would be nice 38 | self.loop_rate = 1 #Hz 39 | 40 | # Parameters 41 | self.state = "GB_TRACK" 42 | self.hysteresis_time = 2.0 # s 43 | self.gb_track_start_time = None 44 | self.max_speed_scaled = 10.0 # m/s 45 | self.speed_offset = 0 # m/s 46 | 47 | # Callback data 48 | self.wpnts_scaled_msg = WpntArray() 49 | self.wpnts_updated_msg = WpntArray() 50 | self.s_points_array = np.array([]) 51 | self.update_waypoints = True 52 | 53 | ### Callbacks ### 54 | def state_machine_cb(self, data: String): 55 | self.state = data.data 56 | if self.state == "GB_TRACK" and self.gb_track_start_time is None: 57 | self.gb_track_start_time = time.time() 58 | elif self.state != "GB_TRACK": 59 | self.gb_track_start_time = None 60 | 61 | def odom_cb(self, data: Odometry): 62 | car_odom = data 63 | if self.update_waypoints == True: 64 | if self.s_points_array.any(): 65 | ego_position = car_odom.pose.pose.position.x 66 | ego_speed = car_odom.twist.twist.linear.x 67 | ego_approx_indx = np.abs(self.s_points_array - ego_position).argmin() 68 | # Hysteresis added to prevent the waypoints from being updated to soon afer switching to GB_TRACK 69 | if self.state == "GB_TRACK" and self.gb_track_start_time is not None and time.time() - self.gb_track_start_time >= self.hysteresis_time: 70 | self.wpnts_updated_msg.wpnts[ego_approx_indx].vx_mps = ego_speed + self.speed_offset 71 | if ego_approx_indx == 0 or ego_approx_indx == (len(self.wpnts_updated_msg.wpnts) - 1): # First and last waypoint are the same 72 | self.wpnts_updated_msg.wpnts[0].vx_mps 73 | self.wpnts_updated_msg.wpnts[-1].vx_mps 74 | else: 75 | pass 76 | 77 | def dyn_param_cb(self, params: Config): 78 | """ 79 | Notices the change in the parameters and changes spline params 80 | """ 81 | self.update_waypoints = rospy.get_param("dynamic_collision_tuner_node/update_waypoints", True) 82 | self.speed_offset = rospy.get_param("dynamic_collision_tuner_node/speed_offset") 83 | 84 | print( 85 | f"[Coll. Pred.] Toggled update waypoints" 86 | f"[Coll. Pred.] Speed offset: {self.speed_offset}" 87 | ) 88 | 89 | ### Helper functions ### 90 | def visualize_waypoints(self): 91 | marker_array = MarkerArray() 92 | for i in range(len(self.wpnts_updated_msg.wpnts)): 93 | marker = Marker(header = rospy.Header(frame_id="map"), id = i, type = Marker.CYLINDER) 94 | marker.pose.position.x = self.wpnts_updated_msg.wpnts[i].x_m 95 | marker.pose.position.y = self.wpnts_updated_msg.wpnts[i].y_m 96 | 97 | marker.pose.orientation.w = 1.0 98 | marker.scale.x = 0.1 99 | marker.scale.y = 0.1 100 | marker.scale.z = self.wpnts_updated_msg.wpnts[i].vx_mps/self.max_speed_scaled 101 | marker.pose.position.z = marker.scale.z/2 102 | 103 | marker.color.a = 1 104 | marker.color.g = 1.0 105 | 106 | marker_array.markers.append(marker) 107 | 108 | self.marker_pub.publish(marker_array) 109 | 110 | ### Main loop ### 111 | def loop(self): 112 | """ 113 | Publish the updated waypoints and visualize them. 114 | """ 115 | rate = rospy.Rate(self.loop_rate) 116 | rospy.loginfo("[Update Wpnts] Update Wpnts wating...") 117 | self.wpnts_scaled_msg = rospy.wait_for_message("/global_waypoints_scaled", WpntArray) 118 | self.wpnts_updated_msg = self.wpnts_scaled_msg 119 | self.global_wpnts_msg = rospy.wait_for_message("/global_waypoints", WpntArray) 120 | self.max_speed_scaled = max([self.global_wpnts_msg.wpnts[i].vx_mps for i in range(len(self.global_wpnts_msg.wpnts))]) 121 | self.s_points_array = np.array([wpnt.s_m for wpnt in self.wpnts_updated_msg.wpnts]) 122 | rospy.loginfo("[Update Wptns] Update Wpnts ready!") 123 | 124 | while not rospy.is_shutdown(): 125 | self.wpnts_updated_msg.header.stamp = rospy.Time.now() 126 | self.wpnts_updated_pub.publish(self.wpnts_updated_msg) 127 | self.visualize_waypoints() 128 | 129 | rate.sleep() 130 | 131 | if __name__ == '__main__': 132 | """ 133 | 134 | """ 135 | node = UpdateWaypoints() 136 | node.loop() --------------------------------------------------------------------------------