Home » TEB轨迹规划算法教程 » TEB轨迹规划算法教程-增加动态障碍

TEB轨迹规划算法教程-增加动态障碍

TEB轨迹规划算法教程-增加动态障碍

说明:

  • 介绍如何在teb_local_planner中包含动态障碍。
  • 在之前的教程中,您学习了如何通过一系列costmap_converter/ObstacleMsg消息提供用户提供的障碍。
  • 消息定义包括称为速度的geometry_msgs/TwistWithCovariance字段,该字段捕获当前质心的速度。
  • 目前,teb_local_planner包只处理linear.x和linear.y。

编写一个简单的动态障碍发布者

  • 在下文中,我们将创建一个发布动态障碍的小python节点。
  • 对于规划部分,我们将运行教程设置和测试优化中描述的test_optim_node。
  • 或者,您也可以根据配置并运行机器人导航,使用正确配置的导航启动文件测试您的发布者。
  • 创建一个名为publish_dynamic_obstacles.py的python节点:
#!/usr/bin/env python

import rospy, math, tf
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
from geometry_msgs.msg import Point32, QuaternionStamped, Quaternion, TwistWithCovariance
from tf.transformations import quaternion_from_euler


def publish_obstacle_msg():
  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
  rospy.init_node("test_obstacle_msg")

  y_0 = -3.0
  vel_x = 0.0
  vel_y = 0.3
  range_y = 6.0

  obstacle_msg = ObstacleArrayMsg() 
  obstacle_msg.header.stamp = rospy.Time.now()
  obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
  
  # Add point obstacle
  obstacle_msg.obstacles.append(ObstacleMsg())
  obstacle_msg.obstacles[0].id = 99
  obstacle_msg.obstacles[0].polygon.points = [Point32()]
  obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
  obstacle_msg.obstacles[0].polygon.points[0].y = 0
  obstacle_msg.obstacles[0].polygon.points[0].z = 0

  yaw = math.atan2(vel_y, vel_x)
  q = tf.transformations.quaternion_from_euler(0,0,yaw)
  obstacle_msg.obstacles[0].orientation = Quaternion(*q)

  obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
  obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
  obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.z = 0

  r = rospy.Rate(10) # 10hz
  t = 0.0
  while not rospy.is_shutdown():
    
    # Vary y component of the point obstacle
    if (vel_y >= 0):
      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
    else:
      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y

    t = t + 0.1
    
    pub.publish(obstacle_msg)
    
    r.sleep()


if __name__ == '__main__': 
  try:
    publish_obstacle_msg()
  except rospy.ROSInterruptException:
    pass
  • 现在结合rviz运行teb_local_planner test_optim_node:
roslaunch teb_local_planner test_optim_node.launch
  • 在下文中,我们假设包含python脚本的包名为mypublisher。
  • 确保您的脚本标记为可执行文件(chmod + x publish_dynamic_obstacles.py)。
  • 打开一个新终端并运行新创建的python脚本:
roslaunch mypublisher publish_dynamic_obstacles.py
  • 您现在应该看到一个可视化(rviz),它应该与前一个教程中的类似,
  • 特别是您应该能够看到默认的test_optim_node障碍物(交互式标记)和新的移动(动态)障碍物
  • 但是,teb_local_planner/test_optim_node中尚未启用动态障碍功能。
  • 我们将在下一节中完成此操作。
  • 在继续之前,请将交互式标记移动到远离计划场景的位置,如下图所示:

请输入图片描述

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: teb轨迹规划算法教程