From 5b25708669b97aa5c6912528d10818d25cc3d0e7 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Thu, 25 Jan 2024 19:06:31 +0100 Subject: [PATCH 1/2] fix: test --- code/planning/src/behavior_agent/behaviours/lane_change.py | 2 +- code/planning/src/local_planner/collision_check.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 4433cc2e..1a3b7015 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -70,7 +70,7 @@ def initialise(self): self.change_detected = False self.change_distance = np.inf self.virtual_change_distance = np.inf - self.curr_behavior_pub.publish(bs.lc_init.name) + self.curr_behavior_pub.publish(bs.lc_app_init.name) def update(self): """ diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index 7091bdcd..53146d2d 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -41,7 +41,7 @@ def __init__(self): # Publisher for emergency stop self.emergency_pub = self.new_publisher( Bool, - f"/paf/{self.role_name}/emergency_deactivated", + f"/paf/{self.role_name}/emergency", qos_profile=1) # Publisher for distance to collision self.collision_pub = self.new_publisher( From 6344cf8846c891b03e647a314540ff7f971d4981 Mon Sep 17 00:00:00 2001 From: robertik10 Date: Fri, 26 Jan 2024 15:48:37 +0100 Subject: [PATCH 2/2] fix(#166): fixed the problem! --- build/docker-compose.yml | 2 +- code/acting/launch/acting.launch | 2 +- .../Test_Data/console_message.perspective | 265 ++++++++++++++++++ code/acting/src/acting/vehicle_controller.py | 63 +++-- 4 files changed, 306 insertions(+), 26 deletions(-) create mode 100644 code/acting/src/acting/Test_Data/console_message.perspective diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 2e9b1d57..ac6c07ea 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,7 +61,7 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" logging: driver: "local" diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 3a705e5d..99674f6b 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -30,7 +30,7 @@ - + diff --git a/code/acting/src/acting/Test_Data/console_message.perspective b/code/acting/src/acting/Test_Data/console_message.perspective new file mode 100644 index 00000000..198b116c --- /dev/null +++ b/code/acting/src/acting/Test_Data/console_message.perspective @@ -0,0 +1,265 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000004800000030000006840000037d0000004800000055000006840000037d00000000000000000a000000004800000055000006840000037d')", + "type": "repr(QByteArray.hex)", + "pretty-print": " H 0 } H U } H U }" + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000063d000002fffc0100000002fb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c00650057006900640067006500740100000000000003bc0000019300fffffffb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c0069007300680065007200570069006400670065007401000003c20000027b0000023000ffffff0000063d0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " Lrqt_console__Console__1__ConsoleWidget Xrqt_publisher__Publisher__1__PublisherWidget 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_console/Console': [1], 'rqt_publisher/Publisher': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_console__Console__1": { + "keys": {}, + "groups": { + "dock_widget__ConsoleWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Console'", + "type": "repr" + }, + "dockable": { + "repr": "False", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "exclude_filters": { + "repr": "['severity', 'message', 'message', 'message', 'message', 'message', 'message']", + "type": "repr" + }, + "filter_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff000000010000000200000070000000700100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " p p " + }, + "highlight_filters": { + "repr": "'message'", + "type": "repr" + }, + "message_limit": { + "repr": "20000", + "type": "repr" + }, + "paused": { + "repr": "False", + "type": "repr" + }, + "settings_exist": { + "repr": "True", + "type": "repr" + }, + "show_highlighted_only": { + "repr": "False", + "type": "repr" + }, + "table_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000100000002000001d3000000e90100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + } + }, + "groups": { + "exclude_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "itemlist": { + "repr": "''", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_1": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'lidar'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_2": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Vision'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_3": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'TF'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_4": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Stanley'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_5": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Created'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_6": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Removed'", + "type": "repr" + } + }, + "groups": {} + }, + "highlight_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "''", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + }, + "plugin__rqt_publisher__Publisher__1": { + "keys": {}, + "groups": { + "dock_widget__PublisherWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Message Publisher'", + "type": "repr" + }, + "dockable": { + "repr": "False", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "publishers": { + "repr": "\"[{'topic_name': '/paf/hero/emergency', 'type_name': 'std_msgs/Bool', 'rate': 1.0, 'enabled': False, 'publisher_id': 0, 'counter': 19, 'expressions': {'/paf/hero/emergency/data': 'True'}}]\"", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index e210ec76..4089af17 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -69,7 +69,7 @@ def __init__(self): self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/emergency", - self.__emergency_break, + self.__set_emergency, qos_profile=QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) ) @@ -151,11 +151,14 @@ def run(self): def loop(timer_event=None) -> None: """ Collects all data received and sends a CarlaEgoVehicleControl msg. + The Leaderboard expects a msg every 0.05 seconds + OTHERWISE IT WILL LAG REALLY BADLY :param timer_event: Timer event from ROS :return: """ - if self.__emergency: # emergency is already handled in - # __emergency_break() + if self.__emergency: + # emergency is already handled in __emergency_brake() + self.__emergency_brake(True) return if (self.controller_testing): if (self.controller_selected_debug == 2): @@ -214,9 +217,11 @@ def __map_steering(self, steering_angle: float) -> float: self.pidpoint_publisher.publish(steering_float) # TODO needed? return steering_float - def __emergency_break(self, data) -> None: + def __set_emergency(self, data) -> None: """ - Executes emergency stop + In case of an emergency set the emergency flag to True ONCE + The emergency flag can be ONLY be set to False if velocity is > 0.1 + by __get_velocity :param data: :return: """ @@ -224,32 +229,30 @@ def __emergency_break(self, data) -> None: return if self.__emergency: # emergency was already triggered return - self.logerr("Emergency breaking engaged") + + self.logerr("Emergency braking engaged") self.__emergency = True - message = CarlaEgoVehicleControl() - message.throttle = 1 - message.steer = 1 # todo: maybe use 30 degree angle - message.brake = 1 - message.reverse = True - message.hand_brake = True - message.manual_gear_shift = False - message.header.stamp = roscomp.ros_timestamp(self.get_time(), - from_sec=True) - self.control_publisher.publish(message) - def __get_velocity(self, data: CarlaSpeedometer) -> None: + def __emergency_brake(self, active) -> None: """ - Ends emergency if vehicle velocity reaches 0. Sends emergency msg - with data = False after stopping. + Executes emergency stop :param data: :return: """ - self.__velocity = data.speed - if not self.__emergency: # nothing to do in this case + if not self.__emergency: return - if data.speed < 0.1: # vehicle has come to a stop + message = CarlaEgoVehicleControl() + if active: + message.throttle = 1 + message.steer = 1 + message.brake = 1 + message.reverse = True + message.hand_brake = True + message.manual_gear_shift = False + message.header.stamp = roscomp.ros_timestamp(self.get_time(), + from_sec=True) + else: self.__emergency = False - message = CarlaEgoVehicleControl() message.throttle = 0 message.steer = 0 message.brake = 1 @@ -258,8 +261,20 @@ def __get_velocity(self, data: CarlaSpeedometer) -> None: message.manual_gear_shift = False message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) - self.control_publisher.publish(message) + self.control_publisher.publish(message) + def __get_velocity(self, data: CarlaSpeedometer) -> None: + """ + Ends emergency if vehicle velocity reaches 0. Sends emergency msg + with data = False after stopping. + :param data: + :return: + """ + self.__velocity = data.speed + if not self.__emergency: # nothing to do in this case + return + if data.speed < 0.1: # vehicle has come to a stop + self.__emergency_brake(False) self.loginfo("Emergency breaking disengaged " "(Emergency breaking has been executed successfully)") for _ in range(7): # publish 7 times just to be safe