teb教程7

融合自定义的障碍物

简介：本部分讲解怎样考虑其他节点发布的多边形的障碍物。

1.在一些应用当中，可能不想依赖于代价地图或者想添加其他的除了点状的障碍物。你可以发送你自己的障碍物列表到teb_local_planner包里面，通过指定话题/obstacles.

下面的消息类型costmap_converter/ObstacleArrayMsg是costmap_converter包的一部分。说明了以下障碍物的类型：

点状障碍物：提供了单个顶点的几何形状；

圆圈障碍物：提供了单个顶点且非零半径的几何形状；

线障碍物：提供了两个顶点的几何形状；

多边形障碍物：提供了不只2个顶点的多边形；

2.写一个简单的障碍物发不器

创建一个简单的python节点类发不一些障碍物；对于规划部分，会运行test_optim_node节点。如下publish_obstacles.py

    #!/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=)

     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[].id =
     obstacle_msg.obstacles[].polygon.points = [Point32()]
     obstacle_msg.obstacles[].polygon.points[].x = 1.5
     obstacle_msg.obstacles[].polygon.points[].y =
     obstacle_msg.obstacles[].polygon.points[].z = 

     # Add line obstacle
     obstacle_msg.obstacles.append(ObstacleMsg())
     obstacle_msg.obstacles[].id =
     line_start = Point32()
     line_start.x = -2.5
     line_start.y = 0.5
     line_end = Point32()
     line_end.x = -2.5
     line_end.y =
     obstacle_msg.obstacles[].polygon.points = [line_start, line_end]

     # Add polygon obstacle
     obstacle_msg.obstacles.append(ObstacleMsg())
     obstacle_msg.obstacles[].id =
     v1 = Point32()
     v1.x = -
     v1.y = -
     v2 = Point32()
     v2.x = -0.5
     v2.y = -1.5
     v3 = Point32()
     v3.x =
     v3.y = -
     obstacle_msg.obstacles[].polygon.points = [v1, v2, v3]

     r = rospy.Rate() # 10hz
     t = 0.0
     while not rospy.is_shutdown():

       # Vary y component of the point obstacle
       obstacle_msg.obstacles[].polygon.points[].y = *math.sin(t)
       t = t + 0.1

       pub.publish(obstacle_msg)

       r.sleep()

   if __name__ == '__main__':
     try:
       publish_obstacle_msg()
     except rospy.ROSInterruptException:
       pass

如何运行：

roslaunch teb_local_planner test_optim_node.launch
roslaunch mypublisher publish_obstacles.py

相关参数：

在规划中，与自定义障碍物相关的参数

~<name>/min_obstacle_dist: Desired minimal distance from obstacles

~<name>/include_costmap_obstacles: Deactivate costmap obstacles completely

~<name>/costmap_obstacles_behind_robot_dist: Maximum distance behind the robot searched for occupied costmap cells.

~<name>/obstacle_poses_affected: Specify how many trajectory configurations/poses should be taken into account next to the closest one.

~<name>/weight_obstacle: Optimization weight for keeping a distance to obstacles.

~<name>/footprint_model: The robot footprint model