export_to_svg.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 """
4 ========================================================================================
5 # This small script subscribes to the FeedbackMsg message of teb_local_planner
6 # and converts the current scene to a svg-image
7 # publish_feedback must be turned on such that the planner publishes this information.
8 # Author: christoph.roesmann@tu-dortmund.de
9 
10 It is recommendable to start this node after initialization of TEB is completed.
11 
12 Requirements:
13 svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
14 =======================================================================================
15 """
16 import roslib;
17 import rospy
18 import svgwrite
19 import math
20 import time
21 import random
22 from svgwrite import cm, mm
23 from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
24 from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
25 
26 
27 # ================= PARAMETERS ==================
28 # TODO: In case of a more general node, change parameter to ros-parameter
29 # Drawing parameters:
30 SCALE = 200 # Overall scaling: 100 pixel = 1 m
31 MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
32 SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
33 GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction.
34 GRID_X_MAX = 2
35 GRID_Y_MIN = -2
36 GRID_Y_MAX = 1
37 
38 # TEB parameters:
39 OBSTACLE_DIST = 50 *SCALE/100 # cm
40 
41 
42 # ================= FUNCTIONS ===================
43 
44 def sign(number):
45  """
46  Signum function: get sign of a number
47 
48  @param number: get sign of this number
49  @type number: numeric type (eg. integer)
50  @return: sign of number
51  @rtype: integer {1, -1, 0}
52  """
53  return cmp(number,0)
54 
55 def arrowMarker(color='green', orientation='auto'):
56  """
57  Create an arrow marker with svgwrite
58 
59  @return: arrow marker
60  @rtype: svg_write marker object
61  """
62  arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
63  arrow.viewbox(width=10, height=10)
64  arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
65  svg.defs.add(arrow)
66  return arrow
67 
68 def quaternion2YawDegree(orientation):
69  """
70  Get yaw angle [degree] from quaternion representation
71 
72  @param orientation: orientation in quaternions to read from
73  @type orientation: geometry_msgs/Quaternion
74  @return: yaw angle [degree]
75  @rtype: float
76  """
77  yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
78  return yawRad*180/math.pi
79 
80 
82  """
83  Callback for receiving TEB and obstacle information
84 
85  @param data: Received feedback message
86  @type data: visualization_msgs/Marker
87 
88  @globalparam tebList: Received TEB List
89  @globaltype tebList: teb_local_planner/FeedbackMsg
90  """
91  # TODO: Remove global variables
92  global feedbackMsg
93 
94  if not feedbackMsg:
95  feedbackMsg = data
96  rospy.loginfo("TEB feedback message received...")
97 
98 
99 # ================ MAIN FUNCTION ================
100 
101 if __name__ == '__main__':
102  rospy.init_node('export_to_svg', anonymous=True)
103 
104  topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
105 
106  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1)
107 
108  rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
109 
110  rate = rospy.Rate(10.0)
111  feedbackMsg = []
112 
113  timestr = time.strftime("%Y%m%d_%H%M%S")
114  filename_string = "teb_svg_" + timestr + '.svg'
115 
116  rospy.loginfo("SVG will be written to '%s'.", filename_string)
117 
118  random.seed(0)
119 
120  svg=svgwrite.Drawing(filename=filename_string, debug=True)
121 
122  # Create viewbox -> this box defines the size of the visible drawing
123  svg.viewbox(GRID_X_MIN*SCALE-1*SCALE,GRID_Y_MIN*SCALE-1*SCALE,GRID_X_MAX*SCALE-GRID_X_MIN*SCALE+2*SCALE,GRID_Y_MAX*SCALE-GRID_Y_MIN*SCALE+2*SCALE)
124 
125  # Draw grid:
126  hLines = svg.add(svg.g(id='hLines', stroke='black'))
127  hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
128  for y in range(GRID_Y_MAX):
129  hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
130  for y in range(-GRID_Y_MIN):
131  hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
132  vLines = svg.add(svg.g(id='vline', stroke='black'))
133  vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
134  for x in range(GRID_X_MAX):
135  vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
136  for x in range(-GRID_X_MIN):
137  vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
138 
139 
140  # Draw legend:
141  legend = svg.g(id='legend', font_size=25)
142  stringGeometry = "Geometry: 1 Unit = 1.0m"
143  legendGeometry = svg.text(stringGeometry)
144  legend.add(legendGeometry)
145  legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
146  svg.add(legend)
147 
148 
149  #arrow = arrowMarker() # Init arrow marker
150 
151  rospy.loginfo("Initialization completed.\nWaiting for feedback message...")
152 
153  # -------------------- WAIT FOR CALLBACKS --------------------------
154  while not rospy.is_shutdown():
155  if feedbackMsg:
156  break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
157  rate.sleep()
158  # ------------------------------------------------------------------
159 
160  if not feedbackMsg.trajectories:
161  rospy.loginfo("Received message does not contain trajectories. Shutting down...")
162  sys.exit()
163 
164  if len(feedbackMsg.trajectories[0].trajectory) < 2:
165  rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
166  sys.exit()
167 
168  # iterate trajectories
169  for index, traj in enumerate(feedbackMsg.trajectories):
170 
171  #color
172  traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
173 
174  # Iterate through TEB positions -> Draw Paths
175  points = []
176  for point in traj.trajectory:
177  points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
178  # svgwrite rotates clockwise!
179 
180 
181  if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
182  line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
183  stroke_linejoin='round', opacity=1.0 ) )
184  else:
185  line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
186  stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
187  #marker_points = points[::7]
188  #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
189  #arrow = arrowMarker(traj_color)
190  #markerline.set_markers( (arrow, arrow, arrow) )
191  #line.set_markers( (arrow, arrow, arrow) )
192  #line['marker-start'] = arrow.get_funciri()
193 
194 
195  # Add Start and Goal Point
196  start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
197  goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
198  start_position = start_pose.position
199  goal_position = goal_pose.position
200  svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
201  svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
202  svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
203  svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
204 
205  # draw start arrow
206  start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
207  start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
208  start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) )
209  start_arrow.scale(3)
210  svg.add(start_arrow)
211 
212  # draw goal arrow
213  goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
214  goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
215  goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) )
216  goal_arrow.scale(3)
217  svg.add(goal_arrow)
218 
219  # Draw obstacles
220  for obstacle in feedbackMsg.obstacles:
221  if len(obstacle.polygon.points) == 1: # point obstacle
222  point = obstacle.polygon.points[0]
223  svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
224  svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
225  svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
226  if len(obstacle.polygon.points) == 2: # line obstacle
227  line_start = obstacle.polygon.points[0]
228  line_end = obstacle.polygon.points[1]
229  svg.add(svg.line(start=(line_start.x*SCALE,-line_start.y*SCALE), end=(line_end.x*SCALE,-line_end.y*SCALE), stroke='black', fill='gray', stroke_width=1, opacity=1.0))
230  svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
231  if len(obstacle.polygon.points) > 2: # polygon obstacle
232  vertices = []
233  for point in obstacle.polygon.points:
234  vertices.append((point.x*SCALE, -point.y*SCALE))
235  svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
236  svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
237 
238 
239 
240  # Save svg to file (svg_output.svg) and exit node
241  svg.save()
242 
243  rospy.loginfo("Drawing completed.")
def arrowMarker(color='green', orientation='auto')
def quaternion2YawDegree(orientation)
def sign(number)


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