融合动态障碍物

简介：考虑怎样把其他节点发布的动态障碍物考虑进来

1.本部分演示了动态障碍物该如何被包含到teb_local_planner中。

2.写一个简单的动态障碍物的发布器publish_dynamic_obstacles.py

#!/usr/bin/env python2

3import rospy, math, tf4 fromcostmap_converter.msg import ObstacleArrayMsg, ObstacleMsg5 fromgeometry_msgs.msg import Point32, QuaternionStamped, Quaternion, TwistWithCovariance6 fromtf.transformations import quaternion_from_euler7

8

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

13 y_0 = -3.0

14 vel_x = 0.0

15 vel_y = 0.3

16 range_y = 6.0

17

18 obstacle_msg =ObstacleArrayMsg()19 obstacle_msg.header.stamp =rospy.Time.now()20 obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map21

22# Add point obstacle23obstacle_msg.obstacles.append(ObstacleMsg())24 obstacle_msg.obstacles[0].id = 99

25 obstacle_msg.obstacles[0].polygon.points =[Point32()]26 obstacle_msg.obstacles[0].polygon.points[0].x = -1.5

27 obstacle_msg.obstacles[0].polygon.points[0].y = 0

28 obstacle_msg.obstacles[0].polygon.points[0].z = 0

29

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

34 obstacle_msg.obstacles[0].velocities.twist.linear.x =vel_x35 obstacle_msg.obstacles[0].velocities.twist.linear.y =vel_y36 obstacle_msg.obstacles[0].velocities.twist.linear.z = 0

37 obstacle_msg.obstacles[0].velocities.twist.angular.x = 0

38 obstacle_msg.obstacles[0].velocities.twist.angular.y = 0

39 obstacle_msg.obstacles[0].velocities.twist.angular.z = 0

40

41 r = rospy.Rate(10) # 10hz42 t = 0.0

43 whilenot rospy.is_shutdown():44

45# Vary y component of the point obstacle46 if (vel_y >= 0):47 obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y48 else:49 obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y -range_y50

51 t = t + 0.1

52

53pub.publish(obstacle_msg)54

55r.sleep()56

57

58 if __name__ == '__main__':59 try:60publish_obstacle_msg()61except rospy.ROSInterruptException:62 pass

View Code

运行：

roslaunch teb_local_planner test_optim_node.launch

roslaunch mypublisher publish_dynamic_obstacles.py

3.设置规划器来考虑动态障碍物

启动rosrun rqt_reconfigure rqt_reconfigure；选中参数include_dynamic_obstacles，teb local planner使用一个常速度模型来预测障碍物将来的行为。现在的轨迹是根据时间和空间来避免障碍物而不是仅仅考当前障碍物的位置来避免。对于速度模型的估计精确性是很重要的，常速度假设是合理且满足的。

如果调节参数visualize_with_time_as_z_axis，可以可视化规划的和预测的速度的时间变化。设置该参数值为0.1。在rviz中的z轴被解释为时间轴，且可被扩展。也可以看到homotopy-class-planning把动态障碍物的预测考虑进去。

相关的参数

~/min_obstacle_dist: Desired minimal distance from (staticand dynamic) obstacles~/inflation_dist: Non-zero cost region around (static) obstacles~/include_dynamic_obstacles: Specify whether the motion of dynamic obstacles should be included (constant-velocity-model) or not.~/dynamic_obstacle_inflation_dist: Non-zero cost region around (dynamic) obstacles~/include_costmap_obstacles: Deactivate costmap obstacles completely~/costmap_obstacles_behind_robot_dist: Maximum distance behind the robot searched foroccupied costmap cells.~/obstacle_poses_affected: Specify how many trajectory configurations/poses should be taken into account next to the closest one.~/weight_obstacle: Optimization weight for keeping a distance to staticobstacles.~/weight_inflation: Optimization weight for inflation costs of staticobstacles.~/weight_dynamic_obstacle: Optimization weight forkeeping a distance to dynamic obstacles.~/weight_dynamic_obstacle_inflation: Optimization weight forinflation costs of dynamic obstacles.~/footprint_model: The robot footprint model

View Code