写在前面的话

上一篇博客：键盘控制车子四轮转向

这篇文章通过订阅车轮的速度和转向计算得到整车的速度和转向，同时发布轮式里程计信息，需要和 四轮转向键盘控制改进版 ros2（python） 进行配套使用

大部分的车子的底盘控制器其实不需要自己写轮式里程计，因为ros2有很多官方的底盘控制器，比如两轮差速等常见底盘控制器，这些官方底盘控制器会自带轮式里程计。本文用的不是官方的四轮转向底盘，所以需要再写一个轮式里程计。（耗时1-2个星期）

参考教程

官方教程

1 里程计设置
2 Publishing Odometry Information over ROS

参考代码（c++）

Autonomous-Ackerman-Simulation/four_ws_navigation/four_wheel_steering_controller/src

关键代码解析

订阅车轮速度

订阅话题/forward_velocity_controller/commands，回调函数self.velocity_callback

self.wheel_v_sub = self.create_subscription(Float64MultiArray, '/forward_velocity_controller/commands', self.velocity_callback, 10)

def velocity_callback(self, msg):
    # 处理 '/forward_velocity_controller/commands' 话题的消息
    # self.get_logger().info('Received data from /forward_velocity_controller/commands: %s' % list(msg.data))
    self.wheel_v_array = list(msg.data)

订阅车轮转向

订阅话题/forward_position_controller/commands，回调函数self.position_callback

self.wheel_w_sub = self.create_subscription(Float64MultiArray, '/forward_position_controller/commands', self.position_callback, 10)

def position_callback(self, msg):
    # 处理 '/forward_position_controller/commands' 话题的消息
    # self.get_logger().info('Received data from /forward_position_controller/commands: %s' % list(msg.data))
    self.wheel_w_array = list(msg.data)

订阅四轮转向控制模式

订阅话题/keyboard_control_mode，回调函数self.control_mode_callback

self.mode_sub = self.create_subscription(Int32, '/keyboard_control_mode', self.control_mode_callback, 10)

def control_mode_callback(self, msg):
    # 处理 '/control_4ws_mode 话题的消息
    # self.get_logger().info('Received data from /control_4ws_mode: %s' % msg.data)
    self.control_mode = msg.data

积累速度和转向角

self.current_time = self.get_clock().now()
dt = (self.current_time - self.last_time).nanoseconds / 1e9


self.vx, self.vy, self.vth = self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)
self.get_logger().info(f'\n self.x:{
      
      self.x}, self.y:{
      
      self.y}, self.th:{
      
      self.th}, dt:{
      
      dt}')
self.get_logger().info(f'\n self.vx:{
      
      self.vx}, self.vy:{
      
      self.vy}, self.vth:{
      
      self.vth}')
#====================== publish odom ====================#

delta_x = (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dt
delta_y = (self.vx * math.sin(self.th) + self.vy * math.cos(self.th))*dt
delta_th = self.vth*dt

self.th += delta_th
self.x += delta_x
self.y += delta_y

# self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}')
self.last_time = self.current_time

发布里程计

self.odom_pub = self.create_publisher(Odometry, '/odom', 50)
self.odom_broadcaster = TransformBroadcaster(self)

odom_quat = Quaternion()
# magnitude = math.sqrt(odom_quat.x**2 + odom_quat.y**2 
#                       + math.sin(self.th / 2)**2 + math.cos(self.th / 2)**2)

# 归一化四元数
# odom_quat.z /= magnitude
# odom_quat.w /= magnitude

odom_quat.z = math.sin(self.th / 2)
odom_quat.w = math.cos(self.th / 2)

# Publish the transform
odom_trans = TransformStamped()
odom_trans.header.stamp = self.current_time.to_msg()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_link'
odom_trans.transform.translation.x = self.x
odom_trans.transform.translation.y = self.y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = odom_quat

self.odom_broadcaster.sendTransform(odom_trans)

# Publish the odometry message
odom = Odometry()
odom.header.stamp = self.current_time.to_msg()
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = odom_quat
odom.child_frame_id = 'base_link'
odom.twist.twist.linear = Vector3(x=self.vx, y=self.vy, z=0.0)
odom.twist.twist.angular = Vector3(x=0.0, y=0.0, z=self.vth)
self.odom_pub.publish(odom)

完整代码

Class Commdar(Node) 通过整车速度和转向控制车轮速度和车轮转向
Class OdometryPublisher(Node) 的 forward_kinematics 函数通过车轮速度和车轮转向计算整车速度和转向

#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
from nav_msgs.msg import Odometry
import pty
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from tf2_ros import TransformStamped
import math
from std_msgs.msg import Int32



class Commander(Node):
    '''
    wheel_separation:
        Shortest distance between the left and right wheels. If this parameter is wrong, 
        the robot will not behave correctly in curves.
    wheel_base:
        Distance between front and rear wheels, in meters.
    wheel_radius:
        Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. 
        If this parameter is wrong the robot will move faster or slower then expected.
    '''

    def __init__(self):
        super().__init__('commander')
        
        self.wheel_seperation = 1.2
        self.wheel_base = 1
        self.wheel_radius = 0.3
        self.wheel_steering_y_offset = 0.03
        self.steering_track = self.wheel_seperation - 2*self.wheel_steering_y_offset #1.14

        self.pos = np.array([0,0,0,0], float)
        self.vel = np.array([0,0,0,0], float) #left_front, right_front, left_rear, right_rear
                
        self.pub_pos = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', 10)
        self.pub_vel = self.create_publisher(Float64MultiArray, '/forward_velocity_controller/commands', 10)
        self.sub_vel = self.create_subscription(Twist,'/keyboard_vel_msg', self.vel_callback, 10)
        self.sub_mode = self.create_subscription(Int32, '/keyboard_control_mode', self.mode_callback, 10)
        self.sub_vel
        self.sub_mode
        self.vel_msg = Twist()
        self.vel_msg.linear.x = 0.0
        self.vel_msg.linear.y = 0.0
        self.vel_msg.angular.x = 0.0 
        self.vel_msg.angular.y = 0.0 
        self.vel_msg.angular.z = 0.0
        self.control_mode = Int32()
        self.control_mode.data = 1
        self.create_timer(timer_period_sec=0.02, callback=self.timer_callback)
        


    def vel_callback(self, msg):
        self.vel_msg = msg
        return
    
    def mode_callback(self, msg):
        self.control_mode = msg.data
        return 

    def timer_callback(self):
        vel_msg = self.vel_msg
        
        mode_selection = self.control_mode
        
        # opposite phase
        if(mode_selection == 1):
            
            vel_steerring_offset = vel_msg.angular.z * self.wheel_steering_y_offset / self.wheel_radius
            sign = math.copysign(1.0, vel_msg.linear.x)            

            self.vel[0] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offset
            self.vel[1] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius + vel_steerring_offset
            self.vel[2] = sign*math.hypot(vel_msg.linear.x - vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius - vel_steerring_offset
            self.vel[3] = sign*math.hypot(vel_msg.linear.x + vel_msg.angular.z*self.steering_track/2, vel_msg.angular.z*self.wheel_base/2)/self.wheel_radius + vel_steerring_offset

            if abs(2.0 * vel_msg.linear.x) > abs(vel_msg.angular.z * self.steering_track) and math.fabs(vel_msg.linear.y) < 0.001:
                front_left_steering = math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x - vel_msg.angular.z * self.steering_track))
                front_right_steering = math.atan(vel_msg.angular.z * self.wheel_base / (2.0 * vel_msg.linear.x + vel_msg.angular.z * self.steering_track))
                self.pos[0] = front_left_steering
                self.pos[1] = front_right_steering
            elif abs(vel_msg.linear.x) > 0.001 and math.fabs(vel_msg.linear.y) < 0.001:
                front_left_steering = math.copysign(math.pi / 2, vel_msg.angular.z)
                front_right_steering = math.copysign(math.pi / 2, vel_msg.angular.z)
                self.pos[0] = front_left_steering
                self.pos[1] = front_right_steering

            # self.pos[0] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x + vel_msg.angular.z*self.steering_track+0.001))
            # self.pos[1] = math.atan(vel_msg.angular.z*self.wheel_base/(2*vel_msg.linear.x - vel_msg.angular.z*self.steering_track+0.001))
            self.pos[2] = -self.pos[0]
            self.pos[3] = -self.pos[1]
            

        # in-phase
        elif(mode_selection == 2):

            V = math.hypot(vel_msg.linear.x, vel_msg.linear.y)
            sign = np.sign(vel_msg.linear.x)
            
            if(vel_msg.linear.x != 0):
                ang = vel_msg.linear.y / vel_msg.linear.x
            else:
                ang = 0
            
            self.pos[0] = math.atan(ang)
            self.pos[1] = math.atan(ang)
            self.pos[2] = self.pos[0]
            self.pos[3] = self.pos[1]
            
            self.vel[:] = sign*V            
            
        # pivot turn
        elif(mode_selection == 3):
            omega_const = 6  # to make robot rotates a little bit slower
            
            self.pos[0] = -math.atan(self.wheel_base/self.steering_track)
            self.pos[1] = math.atan(self.wheel_base/self.steering_track)
            self.pos[2] = math.atan(self.wheel_base/self.steering_track)
            self.pos[3] = -math.atan(self.wheel_base/self.steering_track)
            
            self.vel[0] = -vel_msg.angular.z/(omega_const * self.wheel_radius)
            self.vel[1] = vel_msg.angular.z/(omega_const * self.wheel_radius)
            self.vel[2] = self.vel[0]
            self.vel[3] = self.vel[1]

        else:
            self.pos[:] = 0
            self.vel[:] = 0

        # fixed by xcg: change the sign of the position
        self.pos = -self.pos
        pos_array = Float64MultiArray(data=self.pos) 
        vel_array = Float64MultiArray(data=self.vel) 
        self.pub_pos.publish(pos_array)
        self.pub_vel.publish(vel_array)
        
        self.pos[:] = 0
        self.vel[:] = 0



class OdometryPublisher(Node):
    def __init__(self):
        super().__init__('odometry_publisher')
        self.wheel_v_sub = self.create_subscription(Float64MultiArray, '/forward_velocity_controller/commands', self.velocity_callback, 10)
        self.wheel_w_sub = self.create_subscription(Float64MultiArray, '/forward_position_controller/commands', self.position_callback, 10)
        self.mode_sub = self.create_subscription(Int32, '/keyboard_control_mode', self.control_mode_callback, 10)
        self.wheel_v_sub  # 防止被 GC 回收
        self.wheel_w_sub
        self.mode_sub

        self.odom_pub = self.create_publisher(Odometry, '/odom', 50)
        self.odom_broadcaster = TransformBroadcaster(self)
        self.get_logger().info('Subscribed to /forward_velocity(position)_controller/commands topic')

        self.x = 0.0
        self.y = 0.0
        self.th = 0.0

        self.vx = 0.0
        self.vy = 0.0
        self.vth = 0.0

        self.wheel_base_ = 1.0 
        self.wheel_seperation = 1.2
        self.steering_track_ = 1.14
        self.wheel_steering_y_offset_ = 0.03
        self.wheel_radius_ = 0.3 #轮子半径30cm


        self.wheel_v_array = [0,0,0,0]
        self.wheel_w_array = [0,0,0,0]
        self.control_mode = 1

        self.current_time = self.get_clock().now()
        self.last_time = self.get_clock().now()

        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
    
    def velocity_callback(self, msg):
        # 处理 '/forward_velocity_controller/commands' 话题的消息
        # self.get_logger().info('Received data from /forward_velocity_controller/commands: %s' % list(msg.data))
        self.wheel_v_array = list(msg.data)

    def position_callback(self, msg):
        # 处理 '/forward_position_controller/commands' 话题的消息
        # self.get_logger().info('Received data from /forward_position_controller/commands: %s' % list(msg.data))
        self.wheel_w_array = list(msg.data)
    
    def control_mode_callback(self, msg):
        # 处理 '/control_4ws_mode 话题的消息
        # self.get_logger().info('Received data from /control_4ws_mode: %s' % msg.data)
        self.control_mode = msg.data
    
    def forward_kinematics(self, control_mode, wheel_velocity, wheel_angular):
        # transform wheel_velocity and wheel_angular to the whole car
        fl_speed = wheel_velocity[0]#车轮线速度，没除车轮半径
        fr_speed = wheel_velocity[1]
        rl_speed = wheel_velocity[2]
        rr_speed = wheel_velocity[3]
        fl_steering = wheel_angular[0]#轮子转角
        fr_steering = wheel_angular[1]
        rl_steering = wheel_angular[2]
        rr_steering = wheel_angular[3]

        # 检查转向角是否为 NaN
        if any(math.isnan(steering) for steering in [fl_steering, fr_steering, rl_steering, rr_steering]):
            return

        # 计算前轮转向位置
        front_steering = 0.0
        if abs(fl_steering) > 0.001 or abs(fr_steering) > 0.001:
            # self.get_logger().info(f'\n fl_steering:{fl_steering}, math.tan(fl_steering):{math.tan(fl_steering)}, fr_steering:{fr_steering}')
            if control_mode != 3:
                front_steering = math.atan(
                    2 * math.tan(fl_steering) * math.tan(fr_steering) / (math.tan(fl_steering) + math.tan(fr_steering))#pivot模式下正负为0
                )
            else:
                front_steering = 0.0
        # 计算后轮转向位置
        rear_steering = 0.0
        if abs(rl_steering) > 0.001 or abs(rr_steering) > 0.001:
            if control_mode != 3:
                rear_steering = math.atan(
                    2 * math.tan(rl_steering) * math.tan(rr_steering) / (math.tan(rl_steering) + math.tan(rr_steering))
                )
            else:
                rear_steering = 0.0


        front_tmp = math.cos(front_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_
        front_left_tmp = front_tmp / math.sqrt(1 - self.steering_track_ * front_tmp * math.cos(front_steering)
                                            + (self.steering_track_ * front_tmp / 2)**2)
        front_right_tmp = front_tmp / math.sqrt(1 + self.steering_track_ * front_tmp * math.cos(front_steering)
                                            + (self.steering_track_ * front_tmp / 2)**2)
        fl_speed_tmp = fl_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_left_tmp))
        fr_speed_tmp = fr_speed * (1 / (1 - self.wheel_steering_y_offset_ * front_right_tmp))
        front_linear_speed = self.wheel_radius_ * math.copysign(1.0, fl_speed_tmp + fr_speed_tmp) * math.sqrt(
            (fl_speed**2 + fr_speed**2) / (2 + (self.steering_track_ * front_tmp)**2 / 2.0))

        rear_tmp = math.cos(rear_steering) * (math.tan(front_steering) - math.tan(rear_steering)) / self.wheel_base_
        rear_left_tmp = rear_tmp / math.sqrt(1 - self.steering_track_ * rear_tmp * math.cos(rear_steering)
                                            + (self.steering_track_ * rear_tmp / 2)**2)
        rear_right_tmp = rear_tmp / math.sqrt(1 + self.steering_track_ * rear_tmp * math.cos(rear_steering)
                                            + (self.steering_track_ * rear_tmp / 2)**2)
        rl_speed_tmp = rl_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_left_tmp))
        rr_speed_tmp = rr_speed * (1 / (1 - self.wheel_steering_y_offset_ * rear_right_tmp))
        rear_linear_speed = self.wheel_radius_ * math.copysign(1.0, rl_speed_tmp + rr_speed_tmp) * math.sqrt(
            (rl_speed_tmp**2 + rr_speed_tmp**2) / (2 + (self.steering_track_ * rear_tmp)**2 / 2.0))

        if fl_speed == 0:
            return 0.0, 0.0, 0.0

        if control_mode == 1:#opposite phase
            self.angular_ = (front_linear_speed * front_tmp + rear_linear_speed * rear_tmp) / 2.0
            self.linear_x_ = (front_linear_speed * math.cos(front_steering) + rear_linear_speed * math.cos(rear_steering)) / 2.0
            self.linear_y_ = (front_linear_speed * math.sin(front_steering) - self.wheel_base_ * self.angular_ / 2.0 +
                            rear_linear_speed * math.sin(rear_steering) + self.wheel_base_ * self.angular_ / 2.0) / 2.0
            # print('opposite phase mode...')
        elif control_mode == 2:#in-phase
            c_sign = math.copysign(1.0, rl_speed)
            self.linear_x_ = c_sign * math.sqrt(fl_speed**2 / (1 + math.tan(fl_steering)**2))
            self.linear_y_ = math.tan(fl_steering) * self.linear_x_
            self.angular_ = 0.0
            print('in-phase mode...')
        elif control_mode == 3:
            self.linear_x_ = 0.0
            self.linear_y_ = 0.0
            R = math.sqrt((self.wheel_base_ / 2) ** 2 + (self.steering_track_ / 2) ** 2) + self.wheel_steering_y_offset_
            self.angular_ = - self.wheel_radius_ * fl_speed / R
            print('pivot turn mode...')
        else:
            self.linear_x_ = 0.0
            self.linear_y_ = 0.0
            self.angular_ = 0.0
            print('other mode...')
        # print('\nn调用函数了马???')
        self.get_logger().info(f'\n self.linear_x_:{
      
      self.linear_x_}, self.linear_y_:{
      
      self.linear_y_}, self.angular_:{
      
      self.angular_}, control_mode:{
      
      control_mode}')
            
        return self.linear_x_, self.linear_y_, self.angular_

    def timer_callback(self):
        self.current_time = self.get_clock().now()
        dt = (self.current_time - self.last_time).nanoseconds / 1e9
        
        
        self.vx, self.vy, self.vth = self.forward_kinematics(self.control_mode, self.wheel_v_array, self.wheel_w_array)
        self.get_logger().info(f'\n self.x:{
      
      self.x}, self.y:{
      
      self.y}, self.th:{
      
      self.th}, dt:{
      
      dt}')
        self.get_logger().info(f'\n self.vx:{
      
      self.vx}, self.vy:{
      
      self.vy}, self.vth:{
      
      self.vth}')
        #====================== publish odom ====================#
        
        delta_x = (self.vx * math.cos(self.th) - self.vy * math.sin(self.th))*dt
        delta_y = (self.vx * math.sin(self.th) + self.vy * math.cos(self.th))*dt
        delta_th = self.vth*dt
        
        self.th += delta_th
        self.x += delta_x
        self.y += delta_y
        
        # self.get_logger().info(f'\n self.x:{self.x}, self.y:{self.y}, self.th:{self.th}')
        self.last_time = self.current_time

        odom_quat = Quaternion()
        # magnitude = math.sqrt(odom_quat.x**2 + odom_quat.y**2 
        #                       + math.sin(self.th / 2)**2 + math.cos(self.th / 2)**2)

        # 归一化四元数
        # odom_quat.z /= magnitude
        # odom_quat.w /= magnitude

        odom_quat.z = math.sin(self.th / 2)
        odom_quat.w = math.cos(self.th / 2)

        # Publish the transform
        odom_trans = TransformStamped()
        odom_trans.header.stamp = self.current_time.to_msg()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'base_link'
        odom_trans.transform.translation.x = self.x
        odom_trans.transform.translation.y = self.y
        odom_trans.transform.translation.z = 0.0
        odom_trans.transform.rotation = odom_quat

        self.odom_broadcaster.sendTransform(odom_trans)

        # Publish the odometry message
        odom = Odometry()
        odom.header.stamp = self.current_time.to_msg()
        odom.header.frame_id = 'odom'
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = odom_quat
        odom.child_frame_id = 'base_link'
        odom.twist.twist.linear = Vector3(x=self.vx, y=self.vy, z=0.0)
        odom.twist.twist.angular = Vector3(x=0.0, y=0.0, z=self.vth)
        self.odom_pub.publish(odom)
        


class Joy_subscriber(Node):

    def __init__(self):
        super().__init__('joy_subscriber')
        self.subscription = self.create_subscription(
            Joy,
            'joy',
            self.listener_callback,
            10)
        self.subscription

    def listener_callback(self, data):
        global vel_msg, mode_selection

        if(data.buttons[0] == 1):   # in-phase # A button of Xbox 360 controller
            mode_selection = 2
        elif(data.buttons[4] == 1): # opposite phase # LB button of Xbox 360 controller
            mode_selection = 1
        elif(data.buttons[5] == 1): # pivot turn # RB button of Xbox 360 controller
            mode_selection = 3
        else:
            mode_selection = 4

        vel_msg.linear.x = data.axes[1]*7.5
        vel_msg.linear.y = data.axes[0]*7.5
        vel_msg.angular.z = data.axes[3]*10



if __name__ == '__main__':
    rclpy.init(args=sys.argv)
    
    commander = Commander()#需要, 缺少无法发布odom话题
    # joy_subscriber = Joy_subscriber()
    odom_pub = OdometryPublisher()

    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(commander)
    executor.add_node(odom_pub)
    # executor.add_node(joy_subscriber)
    
    executor_thread = threading.Thread(target=executor.spin, daemon=True)
    executor_thread.start()
    rate = commander.create_rate(2)
    try:
        while rclpy.ok():
            rate.sleep()
    except KeyboardInterrupt:
        pass
    
    rclpy.shutdown()
    executor_thread.join()

完整视频演示

四轮转向轮式里程计设计 G果-CSDN