融合自定义的障碍物
简介:本部分讲解怎样考虑其他节点发布的多边形的障碍物。
1.在一些应用当中,可能不想依赖于代价地图或者想添加其他的除了点状的障碍物。你可以发送你自己的障碍物列表到teb_local_planner包里面,通过指定话题/obstacles.
下面的消息类型costmap_converter/ObstacleArrayMsg是costmap_converter包的一部分。说明了以下障碍物的类型:
点状障碍物:提供了单个顶点的几何形状;
圆圈障碍物:提供了单个顶点且非零半径的几何形状;
线障碍物:提供了两个顶点的几何形状;
多边形障碍物:提供了不只2个顶点的多边形;
2.写一个简单的障碍物发不器
创建一个简单的python节点类发不一些障碍物;对于规划部分,会运行test_optim_node节点。如下publish_obstacles.py
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/961ddebeb323a10fe0623af514929fc1.gif)
1 #!/usr/bin/env python 2 import rospy, math 3 from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg 4 from geometry_msgs.msg import PolygonStamped, Point32 5 6 def publish_obstacle_msg(): 7 rospy.init_node("test_obstacle_msg") 8 9 pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) 10 11 obstacle_msg = ObstacleArrayMsg() 12 obstacle_msg.header.stamp = rospy.Time.now() 13 obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map 14 15 # Add point obstacle 16 obstacle_msg.obstacles.append(ObstacleMsg()) 17 obstacle_msg.obstacles[0].id = 0 18 obstacle_msg.obstacles[0].polygon.points = [Point32()] 19 obstacle_msg.obstacles[0].polygon.points[0].x = 1.5 20 obstacle_msg.obstacles[0].polygon.points[0].y = 0 21 obstacle_msg.obstacles[0].polygon.points[0].z = 0 22 23 # Add line obstacle 24 obstacle_msg.obstacles.append(ObstacleMsg()) 25 obstacle_msg.obstacles[1].id = 1 26 line_start = Point32() 27 line_start.x = -2.5 28 line_start.y = 0.5 29 line_end = Point32() 30 line_end.x = -2.5 31 line_end.y = 2 32 obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] 33 34 # Add polygon obstacle 35 obstacle_msg.obstacles.append(ObstacleMsg()) 36 obstacle_msg.obstacles[1].id = 2 37 v1 = Point32() 38 v1.x = -1 39 v1.y = -1 40 v2 = Point32() 41 v2.x = -0.5 42 v2.y = -1.5 43 v3 = Point32() 44 v3.x = 0 45 v3.y = -1 46 obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] 47 48 r = rospy.Rate(10) # 10hz 49 t = 0.0 50 while not rospy.is_shutdown(): 51 52 # Vary y component of the point obstacle 53 obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) 54 t = t + 0.1 55 56 pub.publish(obstacle_msg) 57 58 r.sleep() 59 60 if __name__ == '__main__': 61 try: 62 publish_obstacle_msg() 63 except rospy.ROSInterruptException: 64 pass
如何运行:
roslaunch teb_local_planner test_optim_node.launch
roslaunch mypublisher publish_obstacles.py
相关参数:
在规划中,与自定义障碍物相关的参数
![](https://i-blog.csdnimg.cn/blog_migrate/8f900a89c6347c561fdf2122f13be562.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/961ddebeb323a10fe0623af514929fc1.gif)
~<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