前言
局部路径规划器teb_local_planner详解4:关于代价地图中,我们介绍了如何使用costmap_converter
本章,我们来详细看下,对于不同的robot model ,teb是如何进行规划的。
car-like robots
一、轨迹优化项
添加non-holomomic约束:限制最大转向角
重要参数:
-
~<name>/min_turning_radius
: 最小转弯半径,最好稍微大点,因为最后会转换为软约束
相关说明: -
<name>/weight_kinematics_forward_drive
: 前向运动学权重,在car-like robot中失效
二、控制命令接口
1. geometry_msgs/Twist message
数据格式是机器人的线速度和角速度。
在前轮驱动的情况下,速度通常定义在后桥的中心。
这种情况,twist直接用。
2. ackemann
数据格式是线速度v
和方向盘转角phi
如下图所示:
重要参数
~<name>/cmd_angle_instead_rotvel
设置为true的话,我们使用的是方向盘转角而非旋转角速度。~<name>/wheelbase
轮间距同样要设定好,配合angle使用。单位为m
如果机器人底盘接收的是ackermann_msgs
而非twist
, 我们需要写个简单脚本来实现转换:
#!/usr/bin/env python
import rospy, math
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped
def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
if omega == 0 or v == 0:
return 0
radius = v / omega
return math.atan(wheelbase / radius)
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
v = data.linear.x
steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = steering
msg.drive.speed = v
pub.publish(msg)
if __name__ == '__main__':
try:
rospy.init_node('cmd_vel_to_ackermann_drive')
twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel')
ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
wheelbase = rospy.get_param('~wheelbase', 1.0)
frame_id = rospy.get_param('~frame_id', 'odom')
rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
rospy.loginfo("Node 'cmd_vel_to_ackermann_drive' started.\nListening to %s, publishing to %s. Frame id: %s, wheelbase: %f", "/cmd_vel", ackermann_cmd_topic, frame_id, wheelbase)
rospy.spin()
except rospy.ROSInterruptException:
pass
holonomic robot
重要参数:
max_vel_y
: 对于non-holonomic robots而言,这个数值必须设置为0. 现在可以设个正数acc_lim_y
: 同上,y轴加速度weight_kinematics_nh
: 面向non-holonomic robots的运动学约束的权重。现在可以降低该权重,1就可以
评论(0)
您还未登录,请登录后发表或查看评论