From 80ad3041365b0a1ec09abf045daa51548af48696 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 19 Oct 2022 17:19:35 +0200 Subject: [PATCH 1/3] Fill CarlaEgoVehicleControl message header Fill the message header of CarlaEgoVehicleControl messages with header received from odometry Some robustness increases: - make vehicle_pid_controller robust against missing speed/pose input --- CHANGELOG.md | 2 ++ carla_ad_agent/src/carla_ad_agent/local_planner.py | 6 ++++++ .../src/carla_ad_agent/vehicle_pid_controller.py | 10 ++++++++-- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 145615f7..f2892b4f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,7 @@ ## Latest +* Fill CarlaEgoVehicleControl message header + ## CARLA-ROS-Bridge 0.9.12 * Fixed scenario runner node shutdown for foxy diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py index 202eae51..184a7a82 100755 --- a/carla_ad_agent/src/carla_ad_agent/local_planner.py +++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py @@ -59,6 +59,7 @@ def __init__(self): self.data_lock = threading.Lock() + self._current_header = None self._current_pose = None self._current_speed = None self._target_speed = 0.0 @@ -100,6 +101,8 @@ def __init__(self): def odometry_cb(self, odometry_msg): with self.data_lock: + self._current_header = odometry_msg.header + self._current_header.frame_id = odometry_msg.child_frame_id self._current_pose = odometry_msg.pose.pose self._current_speed = math.sqrt(odometry_msg.twist.twist.linear.x ** 2 + odometry_msg.twist.twist.linear.y ** 2 + @@ -158,6 +161,7 @@ def run_step(self): # move using PID controllers control_msg = self._vehicle_controller.run_step( self._target_speed, self._current_speed, self._current_pose, target_pose) + control_msg.header = self._current_header # purge the queue of obsolete waypoints max_index = -1 @@ -176,6 +180,8 @@ def run_step(self): def emergency_stop(self): control_msg = CarlaEgoVehicleControl() + if self._current_header: + control_msg.header = self._current_header control_msg.steer = 0.0 control_msg.throttle = 0.0 control_msg.brake = 1.0 diff --git a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py index d2229935..e814ee63 100644 --- a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py +++ b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py @@ -55,8 +55,14 @@ def run_step(self, target_speed, current_speed, current_pose, waypoint): :return: control signal (throttle and steering) """ control = CarlaEgoVehicleControl() - throttle = self._lon_controller.run_step(target_speed, current_speed) - steering = self._lat_controller.run_step(current_pose, waypoint) + if current_speed: + throttle = self._lon_controller.run_step(target_speed, current_speed) + else: + throttle = 0.0 + if current_pose: + steering = self._lat_controller.run_step(current_pose, waypoint) + else: + sterring = 0.0 control.steer = -steering control.throttle = throttle control.brake = 0.0 From a6f1df50bc9d01f6a75448bb2e6d191cd210344a Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 16 Nov 2022 14:20:34 +0100 Subject: [PATCH 2/3] Fix typo --- carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py index e814ee63..6878d93d 100644 --- a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py +++ b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py @@ -62,7 +62,7 @@ def run_step(self, target_speed, current_speed, current_pose, waypoint): if current_pose: steering = self._lat_controller.run_step(current_pose, waypoint) else: - sterring = 0.0 + steering = 0.0 control.steer = -steering control.throttle = throttle control.brake = 0.0 From fae23641e9f0e7abda48958efd164d378c91fa1e Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 16 Nov 2022 15:23:47 +0100 Subject: [PATCH 3/3] Fix carla_ad_agent Don't execute planner step if no odometry message was received --- CHANGELOG.md | 2 +- carla_ad_agent/src/carla_ad_agent/local_planner.py | 5 +++++ .../src/carla_ad_agent/vehicle_pid_controller.py | 10 ++-------- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f2892b4f..81fc4365 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,6 @@ ## Latest -* Fill CarlaEgoVehicleControl message header +* carla_ad_agent: Fill CarlaEgoVehicleControl message header ## CARLA-ROS-Bridge 0.9.12 diff --git a/carla_ad_agent/src/carla_ad_agent/local_planner.py b/carla_ad_agent/src/carla_ad_agent/local_planner.py index 184a7a82..f2e56781 100755 --- a/carla_ad_agent/src/carla_ad_agent/local_planner.py +++ b/carla_ad_agent/src/carla_ad_agent/local_planner.py @@ -141,6 +141,11 @@ def run_step(self): self.emergency_stop() return + if (self._current_pose is None) or (self._current_speed is None) or (self._current_header is None): + self.loginfo("Waiting for first odometry message...") + self.emergency_stop() + return + # when target speed is 0, brake. if self._target_speed == 0.0: self.emergency_stop() diff --git a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py index 6878d93d..d2229935 100644 --- a/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py +++ b/carla_ad_agent/src/carla_ad_agent/vehicle_pid_controller.py @@ -55,14 +55,8 @@ def run_step(self, target_speed, current_speed, current_pose, waypoint): :return: control signal (throttle and steering) """ control = CarlaEgoVehicleControl() - if current_speed: - throttle = self._lon_controller.run_step(target_speed, current_speed) - else: - throttle = 0.0 - if current_pose: - steering = self._lat_controller.run_step(current_pose, waypoint) - else: - steering = 0.0 + throttle = self._lon_controller.run_step(target_speed, current_speed) + steering = self._lat_controller.run_step(current_pose, waypoint) control.steer = -steering control.throttle = throttle control.brake = 0.0