├── 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 |
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 |
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()
--------------------------------------------------------------------------------