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

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

说明：

• 介绍如何增加静态障碍

障碍物：

• 对于某些应用程序，您可能不想依赖于代价地图，或者您希望添加点障碍。
• 在这种情况下，您可以使用特定主题（~/obstacles）将您自己的障碍列表发送到teb_local_planner包。
• 底层消息类型costmap_converter/ObstacleArrayMsg是costmap_converter包的一部分。
• 该消息指定以下障碍物类型：
  • Point obstacle: 提供具有单个顶点的图形
  • Circular obstacle: 提供具有单个顶点和非零半径值的图形。
  • Line obstacle: 提供具有两个顶点的图形
  • Polygon obstacle: 提供一个多于2个顶点的多边形; 多边形将被视为在最后一个和第一个顶点之间闭合。

写一个简单的障碍发布者

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

!/usr/bin/env python
import rospy, math
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
from geometry_msgs.msg import PolygonStamped, Point32

def publish_obstacle_msg():
  rospy.init_node("test_obstacle_msg")

  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)

  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 = 0
  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

  # Add line obstacle
  obstacle_msg.obstacles.append(ObstacleMsg())
  obstacle_msg.obstacles[1].id = 1
  line_start = Point32()
  line_start.x = -2.5
  line_start.y = 0.5
  line_end = Point32()
  line_end.x = -2.5
  line_end.y = 2
  obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
  
  # Add polygon obstacle
  obstacle_msg.obstacles.append(ObstacleMsg())
  obstacle_msg.obstacles[1].id = 2
  v1 = Point32()
  v1.x = -1
  v1.y = -1
  v2 = Point32()
  v2.x = -0.5
  v2.y = -1.5
  v3 = Point32()
  v3.x = 0
  v3.y = -1
  obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]

  r = rospy.Rate(10) # 10hz
  t = 0.0
  while not rospy.is_shutdown():
    
    # Vary y component of the point obstacle
    obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
    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_obstacles.py）。
• 打开一个新终端并运行新创建的python脚本：

roslaunch mypublisher publish_obstacles.py

• rviz中的可视化类似于下图：

相关参数

• 此处列出了影响使用自定义障碍进行规划的参数

〜<name>/min_obstacle_dist：与障碍物的最小距离
〜<name>/include_costmap_obstacles：完全停用代价地图障碍
〜<name>/costmap_obstacles_behind_robot_dist：机器人搜索占用的代价地图单元格后的最大距离。
〜<name>/obstacle_poses_affected：指定在最近的轨迹旁边应考虑多少轨迹配置/姿势。
〜<name>/weight_obstacle：保持与障碍物距离的最佳权重。
〜<name>/footprint_model：机器人足迹模型