ros_bridge ackermann控制

Posted 2022-11-18 想游泳的鱼

在ros_bridge中，除了使用manual_control包来直接使用键盘来控制仿真小车，还可以通过ackermann(阿克曼）来控制。在carla和autoware联合仿真的过程中，autoware最终的控制信息是要转换为ackermann控制指令，最后控制仿真车。
消息的整个流程如下图所示

/carla_ackerman_control_ego_vehicle

这个节点用来生成ackermann节点。

self.speed_controller = PID(Kp=0.0,
                                    Ki=0.0,
                                    Kd=0.0,
                                    sample_time=0.05,
                                    output_limits=(-1., 1.))
        self.accel_controller = PID(Kp=0.0,
                                    Ki=0.0,
                                    Kd=0.0,
                                    sample_time=0.05,
                                    output_limits=(-1, 1))

这个节点是使用PID来控制速度和加速度

rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/speed_Kp",
            rospy.get_param("/carla/ackermann_control/speed_Kp", 0.05))
        rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/speed_Ki",
            rospy.get_param("/carla/ackermann_control/speed_Ki", 0.00))
        rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/speed_Kd",
            rospy.get_param("/carla/ackermann_control/speed_Kd", 0.50))
        rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/accel_Kp",
            rospy.get_param("/carla/ackermann_control/accel_Kp", 0.05))
        rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/accel_Ki",
            rospy.get_param("/carla/ackermann_control/accel_Ki", 0.00))
        rospy.set_param(
            "/carla/" + self.role_name + "/ackermann_control/accel_Kd",
            rospy.get_param("/carla/ackermann_control/accel_Kd", 0.05))

        self.reconfigure_server = Server(
            EgoVehicleControlParameterConfig,
            namespace="/carla/" + self.role_name + "/ackermann_control",
            callback=(lambda config, level: CarlaAckermannControl.reconfigure_pid_parameters(
                self, config, level)))

这个用来设置ackermann的各种参数，并且定义了一个参数服务器

订阅消息

self.control_subscriber = rospy.Subscriber(
            "/carla/" + self.role_name + "/ackermann_cmd",
            AckermannDrive, self.ackermann_command_updated)

        # current status of the vehicle
        self.vehicle_status_subscriber = rospy.Subscriber(
            "/carla/" + self.role_name + "/vehicle_status",
            CarlaEgoVehicleStatus, self.vehicle_status_updated)

        # vehicle info
        self.vehicle_info_subscriber = rospy.Subscriber(
            "/carla/" + self.role_name + "/vehicle_info",
            CarlaEgoVehicleInfo, self.vehicle_info_updated)

这个是节点订阅的各种消息。如图所示，其中/carla/ego_vehicle/ackermann_cmd就是发过来的ackermann命令，节点对这个命令进行解析，然后转换成ros_bridge节点可以识别的消息格式：/carla/ego_vehicle/vehicle_conttrol_cmd，发给/carla_ros_bridge节点。

/carla/ego_vehicle/ackermann_cmd topic发布消息格式是ackermann_msgs/AckermannDrive

float32 steering_angle //转向角
float32 steering_angle_velocity //转向角的速度
float32 speed //向前的速度
float32 acceleration //加速度
float32 jerk //变加速度

如果想要控制车辆的行驶，就可以发送这个消息。autoware就是发送这个消息来控制车辆的行驶。
也可以手动发送

rostopic pub /carla/ego_vehicle/ackermann_cmd ackermann_msgs/AckermannDrive "steering_angle: 0.0, steering_angle_velocity: 0.0, speed: 10, acceleration: 0.0, jerk: 0.0" -r 10

图片中的/rostopic_1058_xxx 这个节点就是手动发送的数据
另外两个是订阅车的信息和车的状态

发布消息

 # to send command to carla
        self.carla_control_publisher = rospy.Publisher(
            "/carla/" + self.role_name + "/vehicle_control_cmd",
            CarlaEgoVehicleControl, queue_size=1)

        # report controller info
        self.control_info_publisher = rospy.Publisher(
            "/carla/" + self.role_name + "/ackermann_control/control_info",
            EgoVehicleControlInfo, queue_size=1)

ackermann节点发布两个消息：

• CarlaEgoVehicleContorl 车里的控制消息

• CarlaEgoVehicleInfo
  这个是车辆的静态信息，主要是定义车辆的物理属性

发布的/carla/eog_vehicle/vehicle_congrol_cmd消息是被/carla_ros_bridge 节点下的ego_vehicle.py文件接收

 self.control_subscriber = rospy.Subscriber(
            self.get_topic_prefix() + "/vehicle_control_cmd",
            CarlaEgoVehicleControl,
            lambda data: self.control_command_updated(data, manual_override=False))

    def control_command_updated(self, ros_vehicle_control, manual_override):
        """
        Receive a CarlaEgoVehicleControl msg and send to CARLA

        This function gets called whenever a ROS CarlaEgoVehicleControl is received.
        If the mode is valid (either normal or manual), the received ROS message is
        converted into carla.VehicleControl command and sent to CARLA.
        This bridge is not responsible for any restrictions on velocity or steering.
        It's just forwarding the ROS input to CARLA

        :param manual_override: manually override the vehicle control command
        :param ros_vehicle_control: current vehicle control input received via ROS
        :type ros_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl
        :return:
        """
        if manual_override == self.vehicle_control_override:
            vehicle_control = VehicleControl()
            vehicle_control.hand_brake = ros_vehicle_control.hand_brake
            vehicle_control.brake = ros_vehicle_control.brake
            vehicle_control.steer = ros_vehicle_control.steer
            vehicle_control.throttle = ros_vehicle_control.throttle
            vehicle_control.reverse = ros_vehicle_control.reverse
            self.carla_actor.apply_control(vehicle_control)
            self._vehicle_control_applied_callback(self.get_id())

注释书说的很清楚，接收CarlaEgoVehicleControl消息，发送给carla。
self.carla_actor.apply_control(vehicle_control)这个函数就是来控制仿真车辆。

在图中还有/carla_ego_vehicle_ego_vehicle节点，这个节点是用来生成车辆和传感器的。
是静态的，并不参与数据的传输和处理。车辆相关的传感器和控制之类的都是在/carla_ros_bridge
这个节点中，这个节点会通过world对象，拿到/carla_ego_vehicle_ego_vehicle节点生产的传感器
和车辆，在bridge.py文件中，把传感器和对应的ros_bridge对象进行绑定。
在bridge.py文件中，绑定的是在

def _create_actor(self, carla_actor):

函数中。

/carla_ros_bridge文件目录

车辆数据的绑定和订阅和发布消息就是在ego_vehicle.py文件中，图中的/carla_ego_vehicle/vehicle_info和
/carla_ego_vehicle/vehicle_status消息就是在ego_vehicle.py文件中发布的。