原理：根据不同轮子转弯半径不同，推算出前轮的转角和后轮的速度分配

原理讲解视频：https://www.bilibili.com/video/BV1BE41177i4?t=2314

#!/usr/bin/env pythonimport rospy
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import mathclass CmdVel2Gazebo:def __init__(self):rospy.init_node('cmdvel2gazebo', anonymous=True)rospy.Subscriber('/smart/cmd_vel', Twist, self.callback, queue_size=1)self.pub_steerL = rospy.Publisher('/smart/front_left_steering_position_controller/command', Float64, queue_size=1)self.pub_steerR = rospy.Publisher('/smart/front_right_steering_position_controller/command', Float64, queue_size=1)self.pub_rearL = rospy.Publisher('/smart/rear_left_velocity_controller/command', Float64, queue_size=1)self.pub_rearR = rospy.Publisher('/smart/rear_right_velocity_controller/command', Float64, queue_size=1)# initial velocity and tire angle are 0self.x = 0self.z = 0# car Wheelbase (in m)self.L = 1.868# car Treadself.T_front = 1.284self.T_rear = 1.284 #1.386# how many seconds delay for the dead man's switchself.timeout=rospy.Duration.from_sec(0.2);self.lastMsg=rospy.Time.now()# maximum steer angle of the "inside" tireself.maxsteerInside=0.6;# turning radius for maximum steer angle just with the inside tire# tan(maxsteerInside) = wheelbase/radius --> solve for max radius at this anglerMax = self.L/math.tan(self.maxsteerInside);# radius of inside tire is rMax, so radius of the ideal middle tire (rIdeal) is rMax+treadwidth/2rIdeal = rMax+(self.T_front/2.0)# maximum steering angle for ideal middle tire# tan(angle) = wheelbase/radiusself.maxsteer=math.atan2(self.L,rIdeal)# looprate = rospy.Rate(10) # run at 10Hzwhile not rospy.is_shutdown():self.publish()rate.sleep()def callback(self,data):# w = v / rself.x = data.linear.x / 0.3# constrain the ideal steering angle such that the ackermann steering is maxed outself.z = max(-self.maxsteer,min(self.maxsteer,data.angular.z))self.lastMsg = rospy.Time.now()def publish(self):# now that these values are published, we# reset the velocity, so that if we don't hear new# ones for the next timestep that we time out; note# that the tire angle will not change# NOTE: we only set self.x to be 0 after 200ms of timeoutdelta_last_msg_time = rospy.Time.now() - self.lastMsgmsgs_too_old = delta_last_msg_time > self.timeoutif msgs_too_old:self.x = 0msgRear = Float64()msgRear.data = self.xself.pub_rearL.publish(msgRear)self.pub_rearR.publish(msgRear)msgSteer = Float64()msgSteer.data = 0self.pub_steerL.publish(msgSteer)self.pub_steerR.publish(msgSteer)return# The self.z is the delta angle in radians of the imaginary front wheel of ackerman model.if self.z != 0:T_rear = self.T_rearT_front = self.T_frontL=self.L# self.v is the linear *velocity*r = L/math.fabs(math.tan(self.z)) #求绝对值# 求出各个轮子的转弯半径rL_rear = r-(math.copysign(1,self.z)*(T_rear/2.0))rR_rear = r+(math.copysign(1,self.z)*(T_rear/2.0))rL_front = r-(math.copysign(1,self.z)*(T_front/2.0))rR_front = r+(math.copysign(1,self.z)*(T_front/2.0))msgRearR = Float64()# the right tire will go a little faster when we turn left (positive angle)# amount is proportional to the radius of the outside/idealmsgRearR.data = self.x*rR_rear/r;msgRearL = Float64()# the left tire will go a little slower when we turn left (positive angle)# amount is proportional to the radius of the inside/idealmsgRearL.data = self.x*rL_rear/r;self.pub_rearL.publish(msgRearL)self.pub_rearR.publish(msgRearR)msgSteerL = Float64()msgSteerR = Float64()# the left tire's angle is solved directly from geometrymsgSteerL.data = math.atan2(L,rL_front)*math.copysign(1,self.z)self.pub_steerL.publish(msgSteerL)# the right tire's angle is solved directly from geometrymsgSteerR.data = math.atan2(L,rR_front)*math.copysign(1,self.z)self.pub_steerR.publish(msgSteerR)else:# if we aren't turningmsgRear = Float64()msgRear.data = self.x;self.pub_rearL.publish(msgRear)# msgRear.data = 0;self.pub_rearR.publish(msgRear)msgSteer = Float64()msgSteer.data = self.zself.pub_steerL.publish(msgSteer)self.pub_steerR.publish(msgSteer)if __name__ == '__main__':try:CmdVel2Gazebo()except rospy.ROSInterruptException:pass

阿克曼转速度指令 cmd_vel相关推荐

移动机器人常见底盘及其运动学
Overview 1 通常移动机器人依赖电机驱动车轮实现行走功能.机器人底盘结构不同,其运动学也完全不同.根据不同类型车轮,常见的底盘结构差速运动模型.滑移运动模型.阿克曼运动模型.全向轮运动模型等等 ...
rostopic发送cmd_vel指令
记录一下rostopic的形式在终端发布cmd_vel指令,方便以后其他测试 rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: ...
ROS综合学习记录（一）---cmd_vel转换为阿克曼模型的速度变换
文章目录 0.问题描述: 1.问题分析: 2.解决方案: 2.1Ackermann差速控制输出方法 2.1.1理解阿克曼模型 2.1.2适用阿克曼模型的teb_local_planner 2.2改正c ...
「ROS Kinetic」发布速度消息cmd_vel
文章目录 Twist 终端发布Twist消息控制机器人节点发布Twist消息控制机器人 Twist 发布Twist消息控制机器人,其实这个Twist消息,它的Topic是/cmd_vel,base ...
ROSNOTE 发送cmd_vel话题
机器人发送cmd_vel话题,转化为机器人的转速知识点一:原文链接:https://blog.csdn.net/SimileciWH/article/details/84976255 在使用ROS导 ...
ROS订阅/cmd_vel话题，转化成移动机器人左右轮的转速
1,理论模型在使用ROS导航时,move_base发布的时 /cmd_vel 话题,但是为了驱动小车,可能我们的车的底盘并不支持ros系统,因此需要通过串口通信交换数据,因此我们必须自己订阅 /cm ...
ROS机器人021-机器人命令行发送cmd_vel话题及/cmd_vel geometry_msgs/Twist示例
1.启动gazebo并加载空地图 roslaunch mbot_gazebo nav_gmapping_view_mbot_gazebolaserandcamera_room.launch 2.命令行 ...
ROS学习记录16【SLAM】仿真学习5——将cmd_vel转换为ackman小车的速度
零.前言在ROS的机制下,绝大多数的速度模型都是:Twist,当然我们有符合阿克曼模型的:ackermann_msgs,不用那么麻烦,直接将Twist.linear.x作为后轮前进的速度.twist ...
ROS：rosbag play系列指令（详解）
Rosbag play系列指令 Rosbag play bagFile01.bag bagFile02.bag - 播放多个录制文件: 由于我们的bag录制文件并不是同一时间录制,因此bag录制文件会 ...

阿克曼转速度指令 cmd_vel

原理：根据不同轮子转弯半径不同，推算出前轮的转角和后轮的速度分配

阿克曼转速度指令 cmd_vel相关推荐

最新文章

热门文章