From 3f9bcae7f7a2b4a17f7d1f7953bf4d1393f87bb0 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Thu, 7 Jul 2022 13:56:00 +0200 Subject: [PATCH 1/5] 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 - make ROS2 launch.py files robust against empty spawn points --- carla_ad_agent/src/carla_ad_agent/local_planner.py | 6 ++++++ .../src/carla_ad_agent/vehicle_pid_controller.py | 10 ++++++++-- ...carla_ros_bridge_with_example_ego_vehicle.launch.py | 2 +- .../launch/carla_example_ego_vehicle.launch.py | 2 +- .../launch/carla_spawn_objects.launch.py | 2 +- 5 files changed, 17 insertions(+), 5 deletions(-) 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 diff --git a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py index d396a799..a0b7b392 100644 --- a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py +++ b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): 'timeout': launch.substitutions.LaunchConfiguration('timeout'), 'vehicle_filter': launch.substitutions.LaunchConfiguration('vehicle_filter'), 'role_name': launch.substitutions.LaunchConfiguration('role_name'), - 'spawn_point': launch.substitutions.LaunchConfiguration('spawn_point') + 'spawn_point': str(launch.substitutions.LaunchConfiguration('spawn_point')) }.items() ), launch.actions.IncludeLaunchDescription( diff --git a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py index 7b523775..37a968df 100644 --- a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py +++ b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): ), launch_arguments={ 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file'), - 'spawn_point_ego_vehicle': launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle'), + 'spawn_point_ego_vehicle': str(launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle')), 'spawn_sensors_only': launch.substitutions.LaunchConfiguration('spawn_sensors_only') }.items() ), diff --git a/carla_spawn_objects/launch/carla_spawn_objects.launch.py b/carla_spawn_objects/launch/carla_spawn_objects.launch.py index ba878484..a7bec232 100644 --- a/carla_spawn_objects/launch/carla_spawn_objects.launch.py +++ b/carla_spawn_objects/launch/carla_spawn_objects.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file') }, { - 'spawn_point_ego_vehicle': launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle') + 'spawn_point_ego_vehicle': str(launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle')) }, { 'spawn_sensors_only': launch.substitutions.LaunchConfiguration('spawn_sensors_only') From b51b0a0edd19bd3c9d11cdf39db6058b7039289d Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Mon, 11 Jul 2022 08:45:27 +0200 Subject: [PATCH 2/5] Revert invalid launch file changes previous launch file changes were actually invalid --- .../launch/carla_ros_bridge_with_example_ego_vehicle.launch.py | 2 +- carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py | 2 +- carla_spawn_objects/launch/carla_spawn_objects.launch.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py index a0b7b392..d396a799 100644 --- a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py +++ b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): 'timeout': launch.substitutions.LaunchConfiguration('timeout'), 'vehicle_filter': launch.substitutions.LaunchConfiguration('vehicle_filter'), 'role_name': launch.substitutions.LaunchConfiguration('role_name'), - 'spawn_point': str(launch.substitutions.LaunchConfiguration('spawn_point')) + 'spawn_point': launch.substitutions.LaunchConfiguration('spawn_point') }.items() ), launch.actions.IncludeLaunchDescription( diff --git a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py index 37a968df..7b523775 100644 --- a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py +++ b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): ), launch_arguments={ 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file'), - 'spawn_point_ego_vehicle': str(launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle')), + 'spawn_point_ego_vehicle': launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle'), 'spawn_sensors_only': launch.substitutions.LaunchConfiguration('spawn_sensors_only') }.items() ), diff --git a/carla_spawn_objects/launch/carla_spawn_objects.launch.py b/carla_spawn_objects/launch/carla_spawn_objects.launch.py index a7bec232..ba878484 100644 --- a/carla_spawn_objects/launch/carla_spawn_objects.launch.py +++ b/carla_spawn_objects/launch/carla_spawn_objects.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): 'objects_definition_file': launch.substitutions.LaunchConfiguration('objects_definition_file') }, { - 'spawn_point_ego_vehicle': str(launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle')) + 'spawn_point_ego_vehicle': launch.substitutions.LaunchConfiguration('spawn_point_ego_vehicle') }, { 'spawn_sensors_only': launch.substitutions.LaunchConfiguration('spawn_sensors_only') From a7ece333e4fc11e2c3a633dd74f9a0663a22cbd3 Mon Sep 17 00:00:00 2001 From: Felipe Gomes de Melo Date: Mon, 27 Jun 2022 18:08:48 -0300 Subject: [PATCH 3/5] Remove empty launch args default_value --- .../launch/carla_ros_bridge_with_example_ego_vehicle.launch.py | 2 +- carla_ros_bridge/test/ros_bridge_client_ros2_test.py | 2 +- carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py | 2 +- carla_spawn_objects/launch/carla_spawn_objects.launch.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py index d396a799..ba91eee0 100644 --- a/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py +++ b/carla_ros_bridge/launch/carla_ros_bridge_with_example_ego_vehicle.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='spawn_point', - default_value='' + default_value='None' ), launch.actions.DeclareLaunchArgument( name='town', diff --git a/carla_ros_bridge/test/ros_bridge_client_ros2_test.py b/carla_ros_bridge/test/ros_bridge_client_ros2_test.py index e89abeb0..0828db92 100644 --- a/carla_ros_bridge/test/ros_bridge_client_ros2_test.py +++ b/carla_ros_bridge/test/ros_bridge_client_ros2_test.py @@ -77,7 +77,7 @@ def generate_test_description(): ), launch.actions.DeclareLaunchArgument( name='spawn_point', - default_value='' + default_value='None' ), launch.actions.DeclareLaunchArgument( name='objects_definition_file', diff --git a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py index 7b523775..ee24a672 100644 --- a/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py +++ b/carla_spawn_objects/launch/carla_example_ego_vehicle.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='spawn_point_ego_vehicle', - default_value='' + default_value='None' ), launch.actions.DeclareLaunchArgument( name='spawn_sensors_only', diff --git a/carla_spawn_objects/launch/carla_spawn_objects.launch.py b/carla_spawn_objects/launch/carla_spawn_objects.launch.py index ba878484..f8aa1c1a 100644 --- a/carla_spawn_objects/launch/carla_spawn_objects.launch.py +++ b/carla_spawn_objects/launch/carla_spawn_objects.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): ), launch.actions.DeclareLaunchArgument( name='spawn_point_ego_vehicle', - default_value='' + default_value='None' ), launch.actions.DeclareLaunchArgument( name='spawn_sensors_only', From 28c20ad6ccf7dbbc3d0d0cd0a012c630a07dcb43 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 16 Nov 2022 14:15:28 +0100 Subject: [PATCH 4/5] 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 63bc98b0ad30c87b23e6183ed1314effe978a3b1 Mon Sep 17 00:00:00 2001 From: Bernd Gassmann Date: Wed, 16 Nov 2022 15:16:25 +0100 Subject: [PATCH 5/5] Fix carla_ad_agent Don't execute planner step if no odometry message was received --- carla_ad_agent/src/carla_ad_agent/local_planner.py | 5 +++++ .../src/carla_ad_agent/vehicle_pid_controller.py | 10 ++-------- 2 files changed, 7 insertions(+), 8 deletions(-) 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