Go to the documentation of this file.00001
00002 import yaml
00003 import rospy
00004 import move_base_msgs.msg as move_base_msgs
00005 import geometry_msgs.msg as geometry_msgs
00006 import visualization_msgs.msg as viz_msgs
00007 import std_msgs.msg as std_msgs
00008
00009 id_count = 1
00010
00011 def get_waypoints(filename):
00012 with open(filename, 'r') as f:
00013 data = yaml.load(f)
00014
00015 return data['waypoints']
00016
00017 def create_geo_pose(p):
00018 pose = geometry_msgs.Pose()
00019
00020 pose.position.x = p['pose']['position']['x']
00021 pose.position.y = p['pose']['position']['y']
00022 pose.position.z = p['pose']['position']['z']
00023 pose.orientation.x = p['pose']['orientation']['x']
00024 pose.orientation.y = p['pose']['orientation']['y']
00025 pose.orientation.z = p['pose']['orientation']['z']
00026 pose.orientation.w = p['pose']['orientation']['w']
00027 return pose
00028
00029 def create_move_base_goal(p):
00030 target = geometry_msgs.PoseStamped()
00031 target.header.frame_id = p['frame_id']
00032 target.header.stamp = rospy.Time.now()
00033 target.pose = create_geo_pose(p)
00034 goal = move_base_msgs.MoveBaseGoal(target)
00035 return goal
00036
00037 def create_viz_markers(waypoints):
00038 marray= viz_msgs.MarkerArray()
00039 for w in waypoints:
00040 m_arrow = create_arrow(w)
00041 m_text = create_text(w)
00042 marray.markers.append(m_arrow)
00043 marray.markers.append(m_text)
00044 return marray
00045
00046 def create_marker(w):
00047 global id_count
00048 m = viz_msgs.Marker()
00049 m.header.frame_id = w['frame_id']
00050 m.ns = w['name']
00051 m.id = id_count
00052 m.action = viz_msgs.Marker.ADD
00053 m.pose = create_geo_pose(w)
00054 m.scale = geometry_msgs.Vector3(1.0,0.3,0.3)
00055 m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
00056
00057 id_count = id_count + 1
00058 return m
00059
00060 def create_arrow(w):
00061 m = create_marker(w)
00062 m.type = viz_msgs.Marker.ARROW
00063 m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
00064 return m
00065
00066 def create_text(w):
00067 m = create_marker(w)
00068 m.type = viz_msgs.Marker.TEXT_VIEW_FACING
00069 m.pose.position.z = 2.5
00070 m.text = w['name']
00071 return m