publish_test_obstacles.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Author: christoph.roesmann@tu-dortmund.de
4 
5 import rospy, math
6 from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
7 from geometry_msgs.msg import PolygonStamped, Point32
8 
9 
11  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
12  #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
13  rospy.init_node("test_obstacle_msg")
14 
15 
16  obstacle_msg = ObstacleArrayMsg()
17  obstacle_msg.header.stamp = rospy.Time.now()
18  obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
19 
20  # Add point obstacle
21  obstacle_msg.obstacles.append(ObstacleMsg())
22  obstacle_msg.obstacles[0].id = 0
23  obstacle_msg.obstacles[0].polygon.points = [Point32()]
24  obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
25  obstacle_msg.obstacles[0].polygon.points[0].y = 0
26  obstacle_msg.obstacles[0].polygon.points[0].z = 0
27 
28 
29  # Add line obstacle
30  obstacle_msg.obstacles.append(ObstacleMsg())
31  obstacle_msg.obstacles[1].id = 1
32  line_start = Point32()
33  line_start.x = -2.5
34  line_start.y = 0.5
35  #line_start.y = -3
36  line_end = Point32()
37  line_end.x = -2.5
38  line_end.y = 2
39  #line_end.y = -4
40  obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
41 
42  # Add polygon obstacle
43  obstacle_msg.obstacles.append(ObstacleMsg())
44  obstacle_msg.obstacles[1].id = 2
45  v1 = Point32()
46  v1.x = -1
47  v1.y = -1
48  v2 = Point32()
49  v2.x = -0.5
50  v2.y = -1.5
51  v3 = Point32()
52  v3.x = 0
53  v3.y = -1
54  obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
55 
56 
57  r = rospy.Rate(10) # 10hz
58  t = 0.0
59  while not rospy.is_shutdown():
60 
61  # Vary y component of the point obstacle
62  obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
63  t = t + 0.1
64 
65  pub.publish(obstacle_msg)
66 
67  r.sleep()
68 
69 
70 
71 if __name__ == '__main__':
72  try:
74  except rospy.ROSInterruptException:
75  pass
76 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08