Skip to content

Commit

Permalink
Merge pull request #174 from una-auxme:166-bug-lag-when-emergency-bra…
Browse files Browse the repository at this point in the history 
…king-in-leaderboard-eval

166 bug lag when emergency braking in leaderboard eval
  • Loading branch information
@samuelkuehnel
samuelkuehnel committed Jan 29, 2024
2 parents 19686b0 + 6344cf8 commit 26aab61
Show file tree
Hide file tree
Showing 6 changed files with 308 additions and 28 deletions.
2 changes: 1 addition & 1 deletion build/docker-compose.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
2 changes: 1 addition & 1 deletion code/acting/launch/acting.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
</node>

<node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">
<param name="control_loop_rate" value="$(arg control_loop_rate)" />
<param name="control_loop_rate" value="$(arg control_loop_rate)" /> <!-- leaderboard expects commands every 0.05 seconds OTHERWISE IT LAGS REALLY BADLY-->
<param name="role_name" value="$(arg role_name)" />
</node>

Expand Down
265 changes: 265 additions & 0 deletions code/acting/src/acting/Test_Data/console_message.perspective
View file
Original file line number Diff line number Diff line change
@@ -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": {}
}
}
}
}
}
}
}
63 changes: 39 additions & 24 deletions code/acting/src/acting/vehicle_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -214,42 +217,42 @@ 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:
"""
if not data.data: # not an emergency
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
Expand All @@ -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
Expand Down
Loading

0 comments on commit 26aab61

Please sign in to comment.