publish_test_obstacles.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Author: christoph.roesmann@tu-dortmund.de
00004 
00005 import rospy, math
00006 from teb_local_planner.msg import ObstacleMsg
00007 from geometry_msgs.msg import PolygonStamped, Point32
00008 
00009 
00010 def publish_obstacle_msg():
00011   pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleMsg, queue_size=1)
00012   #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleMsg, queue_size=1)
00013   rospy.init_node("test_obstacle_msg")
00014 
00015 
00016   obstacle_msg = ObstacleMsg() 
00017   obstacle_msg.header.stamp = rospy.Time.now()
00018   obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
00019   
00020   # Add point obstacle
00021   obstacle_msg.obstacles.append(PolygonStamped())
00022   obstacle_msg.obstacles[0].polygon.points = [Point32()]
00023   obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
00024   obstacle_msg.obstacles[0].polygon.points[0].y = 0
00025   obstacle_msg.obstacles[0].polygon.points[0].z = 0
00026 
00027 
00028   # Add line obstacle
00029   obstacle_msg.obstacles.append(PolygonStamped())
00030   line_start = Point32()
00031   line_start.x = -2.5
00032   line_start.y = 0.5
00033   #line_start.y = -3
00034   line_end = Point32()
00035   line_end.x = -2.5
00036   line_end.y = 2
00037   #line_end.y = -4
00038   obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
00039   
00040   # Add polygon obstacle
00041   obstacle_msg.obstacles.append(PolygonStamped())
00042   v1 = Point32()
00043   v1.x = -1
00044   v1.y = -1
00045   v2 = Point32()
00046   v2.x = -0.5
00047   v2.y = -1.5
00048   v3 = Point32()
00049   v3.x = 0
00050   v3.y = -1
00051   obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
00052   
00053 
00054   r = rospy.Rate(10) # 10hz
00055   t = 0.0
00056   while not rospy.is_shutdown():
00057     
00058     # Vary y component of the point obstacle
00059     obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
00060     t = t + 0.1
00061     
00062     pub.publish(obstacle_msg)
00063     
00064     r.sleep()
00065 
00066 
00067 
00068 if __name__ == '__main__': 
00069   try:
00070     publish_obstacle_msg()
00071   except rospy.ROSInterruptException:
00072     pass
00073 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15