【Carla + ROS2 control vehicle movement】

Background

This simple tutorial for receive the commands via ROS2 Topic and self-defined message to control vehicle behavior. This framework can be used to your scenario. I will write the problem I met, hope it have some referenced meanings to you.
My scenario setting is that a vehicle radical move, for testing the braking algorithm to avoid collision .

environment:
ubuntu22.04
ROS2 humble
Carla0.9.15

Code snippets

1、configure ROS2
write self-defined msg to control vehicle and return vehicle status, simple example as follow:
YourPublishMsg.msg

std_msgs/Header header
float32 vehicle_speed

YourSubscriptionMsg.msg

std_msgs/Header header
bool brake

2. main code
   Perhaps some dependence isn’t necessary, I cut it from my whole project, you can throw it, so I’m not sure this snippet can running without error. Your ROS2 work space should has two packages, one is the message, another is your realize program. Remember configure your CMakeLists.txt, package.xml or setup.py. If some wrong happen, a great possibility refer the environment configure.

import carla
import rclpy
import random
import math
import threading
from rclpy.time import Time  
from std_msgs.msg import Header

from rclpy.executors import MultiThreadedExecutor   
from YourPackageName.msg import YourSubscriptionMsg,YourPublishMsg
from rclpy.node import Node

class CarlaControl(Node):
    def __init__(self):
        super().__init__('NodeName')
        self.clock = self.get_clock() 
        self.publisher_speed = self.create_publisher(YourPublishMsg, "TopicName", 10)
        self.subscription = self.create_subscription(YourSubscriptionMsg,"TopicName",self.subscription_callback,10)  
        self.subscription  # prevent unused variable warning
		# you can choose one of the method of control
        self.ego_control = carla.VehicleControl()
        self.ego_ackermann_control = carla.VehicleAckermannControl()
        self.ego_ackermann_control.speed = 2.77777 # the unit of measurement is m/s
        self.ego_ackermann_control.acceleration = 6.0 # m/s^2 
        self.is_autopilot = True

    def simulation(self):
        client = carla.Client('localhost', 2000)
        client.set_timeout(2.0)
        traffic_manager = client.get_trafficmanager()
        world = client.load_world("Town02") # I am used to testing with small map
        try:
            original_settings = world.get_settings()
            settings = world.get_settings()
			# Synchronous mode + fixed time-step.
            traffic_manager.set_synchronous_mode(True)
            settings.fixed_delta_seconds = delta # every simulated second including 1/fixed time-step frame
            settings.synchronous_mode = True
            world.apply_settings(settings)

            blueprint_library = world.get_blueprint_library()
 			vehicle_bp = blueprint_library.find('vehicle.tesla.model3') 
            ego_vehicle_transform = random.choice(world.get_map().get_spawn_points())
            ego_vehicle = world.spawn_actor(vehicle_bp, ego_vehicle_transform)
            actor_list.append(ego_vehicle)
            
            traffic_manager.ignore_vehicles_percentage(ego_vehicle, 100) # ignore the vehicle collision , second parameter is percentage
            traffic_manager.ignore_lights_percentage(ego_vehicle, 100) # ignore the traffic lights
            traffic_manager.set_desired_speed(ego_vehicle,18) # the unit of measurement is km/h
            
            while True:
                world.tick() 
                ego_vehicle.set_autopilot(self.is_autopilot)

				# if receive the brake message, colse the autopilot, use normal control or ackermancontrol to stop vehicle running. 
                if self.is_autopilot == False: 
                    # ego_vehicle.apply_control(self.ego_control)
                    ego_vehicle.apply_ackermann_control(self.ego_ackermann_control)

                # publish and print the speed 
                ret_speed = self.publish_and_display_speed(ego_vehicle)
                
                spectator = world.get_spectator()
                transform = ego_vehicle.get_transform()
                spectator_transform = carla.Transform(transform.location + carla.Location(z=30), carla.Rotation(pitch=-90, yaw=270, roll=0.0))
                spectator.set_transform(spectator_transform)
        finally:
            world.apply_settings(original_settings)
            traffic_manager.set_synchronous_mode(False)
            for actor in actor_list:
                actor.destroy()

    def subscription_callback(self, control):  

        self.get_logger().info(f'I heard: {control.braking}') 
        if control.braking == True: 
            self.is_autopilot = False # Close the autopilot model
            # self.ego_control.throttle = 0.0
            # self.ego_control.brake = 1.0    
            # target speed is 0 with the -6 acceleration.
            self.ego_ackermann_control.speed = 0.0
            self.ego_ackermann_control.acceleration = -6.0
        else:
            self.is_autopilot = True

    def publish_and_display_speed(self,vehicle):
        v = vehicle.get_velocity() #carla.Vector3D - m/s
        speed_kmh = 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2) # km/h
        # create your feedback message，intializw the items.
        vehicle_status =  YourPublishMsg()
        vehicle_status.header = Header(stamp=self.clock.now().to_msg(), frame_id=" ")
        vehicle_status.vehicle_speed = speed_kmh  
        self.publisher_speed.publish(vehicle_status)
        self.get_logger().info(f'vehicle Speed:   {speed_kmh:15.0f} km/h')
        return speed_kmh

    def destroy_node(self):
        super().destroy_node()
    
    def spin(self):
        rclpy.spin(self, self.executor)

def main(args=None):
    rclpy.init(args=args)
    node = CarlaControl()
    try:
        # Create a multiple thread excutor for run simulation thread. 
        executor = MultiThreadedExecutor()  
        executor.add_node(node)
        spin_thread = threading.Thread(target=node.spin)
        spin_thread.start()
        node.simulation()

    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
        spin_thread.join()

Issue recording

1、When I use ros2 topic echo XX to check my topic publish, I found out it hint "The message type ‘XXX’ is invalid. for solve it, source install/setup.bash in you ROS2 work space path

2、Assign the time-stamp wrong:

vehicle_status.header.stamp = self.clock.now() File
“/opt/ros/humble/local/lib/python3.10/dist-packages/std_msgs/msg/_header.py”,
line 133, in stamp
assert \ AssertionError: The ‘stamp’ field must be a sub message of type ‘Time’

answer: https://robotics.stackexchange.com/questions/96084/timestamp-message-ros2-python.
self.clock.now().to_msg() to solve it.

3、I found out the program can’t enter the subscription callback, check the program structure, my original main function like this,

  rclpy.init(args=args)
  node = CarlaControl()
  try:
      rclpy.spin(node)
  except KeyboardInterrupt:
      pass
  finally:
      node.destroy_node()
      rclpy.shutdown()

I guess it’s because the spin() didn’t run, I found a article prove my assumption.

subscribe一个topic之后，并不会直接进入callback函数， 而是往下执行，直到遇见了ros::spin()。这也就是说，
ros::spin()后面的内容不会执行，除非ros::ok() return
false（用户按下ctrl+c，或者调用了ros::shutdown()函数）。还有就是，如果程序中没有ros::spin(),
那么就永远不会进入callback函数了。

from https://www.cnblogs.com/chrislzy/p/14933825.html

So, I use while loop to make my simulation work, simulation() as a class function in the Node class. then node = CarlaControl() make the node work. hence, the rclpy.spin(node) never run -> didn’t enter subscription function.
Solution:
use multiple thread to run simulation. I get this idea from the calra-ros-bridge example. If you want to know more, provide this program pathcarla-ros-bridge/src/ros-bridge/carla_manual_control/src/carla_manual_control/carla_manual_control.py

And the ROS2 executors tutorials as follow:
https://docs.ros.org/en/humble/Concepts/Intermediate/About-Executors.html

I mention the calra-ros-bridge, but why I didn’t use it.
First of all, the real-time capability is very important, the interface provide by the calra-ros-bridge went through more packaged, I thought the running speed will slower then the original ros interface.
Second, this function is simple, no need to research how to use calra-ros-bridge 😃.
And I’m not sure, but when I building the calra-ros-bridge, it remind me the version is not correct, seems it just support under the Carla 0.9.13 version.

Thank you for your attention. bye.
原创请勿转载