publish_dynamic_obstacle.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Author: franz.albers@tu-dortmund.de
4 
5 import rospy, math, tf
6 from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
7 from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance
8 from tf.transformations import quaternion_from_euler
9 
10 
12  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
13  #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
14  rospy.init_node("test_obstacle_msg")
15 
16  y_0 = -3.0
17  vel_x = 0.0
18  vel_y = 0.3
19  range_y = 6.0
20 
21  obstacle_msg = ObstacleArrayMsg()
22  obstacle_msg.header.stamp = rospy.Time.now()
23  obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
24 
25  # Add point obstacle
26  obstacle_msg.obstacles.append(ObstacleMsg())
27  obstacle_msg.obstacles[0].id = 99
28  obstacle_msg.obstacles[0].polygon.points = [Point32()]
29  obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
30  obstacle_msg.obstacles[0].polygon.points[0].y = 0
31  obstacle_msg.obstacles[0].polygon.points[0].z = 0
32 
33  yaw = math.atan2(vel_y, vel_x)
34  q = tf.transformations.quaternion_from_euler(0,0,yaw)
35  obstacle_msg.obstacles[0].orientation = Quaternion(*q)
36 
37  obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
38  obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
39  obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
40  obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
41  obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
42  obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
43 
44  r = rospy.Rate(10) # 10hz
45  t = 0.0
46  while not rospy.is_shutdown():
47 
48  # Vary y component of the point obstacle
49  if (vel_y >= 0):
50  obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
51  else:
52  obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y
53 
54  t = t + 0.1
55 
56  pub.publish(obstacle_msg)
57 
58  r.sleep()
59 
60 
61 
62 if __name__ == '__main__':
63  try:
65  except rospy.ROSInterruptException:
66  pass
67 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10