export_to_svg.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 """
00004 ========================================================================================
00005 # This small script subscribes to the FeedbackMsg message of teb_local_planner
00006 # and converts the current scene to a svg-image
00007 # publish_feedback must be turned on such that the planner publishes this information.
00008 # Author: christoph.roesmann@tu-dortmund.de
00009 
00010 It is recommendable to start this node after initialization of TEB is completed.
00011 
00012 Requirements:
00013 svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
00014 =======================================================================================
00015 """
00016 import roslib;
00017 import rospy
00018 import svgwrite
00019 import math
00020 import time
00021 import random
00022 from svgwrite import cm, mm
00023 from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
00024 from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
00025 
00026 
00027 # ================= PARAMETERS ==================
00028 # TODO: In case of a more general node, change parameter to ros-parameter
00029 # Drawing parameters: 
00030 SCALE = 200      # Overall scaling: 100 pixel = 1 m
00031 MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
00032 SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
00033 GRID_X_MIN = -2  # Define, how many cells your grid should contain in each direction.
00034 GRID_X_MAX = 2
00035 GRID_Y_MIN = -2
00036 GRID_Y_MAX = 1
00037 
00038 # TEB parameters:
00039 OBSTACLE_DIST = 50 *SCALE/100 # cm
00040 
00041 
00042 # ================= FUNCTIONS ===================
00043 
00044 def sign(number):
00045     """
00046     Signum function: get sign of a number
00047 
00048     @param number: get sign of this number
00049     @type  number: numeric type (eg. integer)
00050     @return:  sign of number
00051     @rtype:   integer {1, -1, 0}  
00052     """
00053     return cmp(number,0)
00054 
00055 def arrowMarker(color='green', orientation='auto'):
00056     """
00057     Create an arrow marker with svgwrite
00058 
00059     @return:  arrow marker
00060     @rtype:   svg_write marker object 
00061     """
00062     arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
00063     arrow.viewbox(width=10, height=10)
00064     arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
00065     svg.defs.add(arrow)
00066     return arrow
00067 
00068 def quaternion2YawDegree(orientation):
00069     """
00070     Get yaw angle [degree] from quaternion representation
00071 
00072     @param orientation: orientation in quaternions to read from
00073     @type  orientation: geometry_msgs/Quaternion
00074     @return: yaw angle [degree]
00075     @rtype:  float  
00076     """
00077     yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
00078     return yawRad*180/math.pi
00079 
00080 
00081 def feedback_callback(data):
00082     """
00083     Callback for receiving TEB and obstacle information
00084 
00085     @param data: Received feedback message
00086     @type  data: visualization_msgs/Marker
00087 
00088     @globalparam tebList: Received TEB List
00089     @globaltype  tebList: teb_local_planner/FeedbackMsg
00090     """
00091     # TODO: Remove global variables
00092     global feedbackMsg
00093     
00094     if not feedbackMsg:
00095       feedbackMsg = data
00096       rospy.loginfo("TEB feedback message received...") 
00097                
00098    
00099 # ================ MAIN FUNCTION ================
00100 
00101 if __name__ == '__main__':
00102     rospy.init_node('export_to_svg', anonymous=True)
00103     
00104     topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
00105 
00106     rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 
00107 
00108     rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
00109 
00110     rate = rospy.Rate(10.0)
00111     feedbackMsg = []
00112 
00113     timestr = time.strftime("%Y%m%d_%H%M%S")
00114     filename_string = "teb_svg_" + timestr + '.svg'
00115   
00116     rospy.loginfo("SVG will be written to '%s'.", filename_string)
00117     
00118     random.seed(0)
00119 
00120     svg=svgwrite.Drawing(filename=filename_string, debug=True)
00121     
00122     # Create viewbox -> this box defines the size of the visible drawing
00123     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)
00124 
00125     # Draw grid:
00126     hLines = svg.add(svg.g(id='hLines', stroke='black'))
00127     hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
00128     for y in range(GRID_Y_MAX):
00129         hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
00130     for y in range(-GRID_Y_MIN):
00131         hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
00132     vLines = svg.add(svg.g(id='vline', stroke='black'))
00133     vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
00134     for x in range(GRID_X_MAX):
00135         vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
00136     for x in range(-GRID_X_MIN):
00137         vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
00138 
00139 
00140     # Draw legend:
00141     legend = svg.g(id='legend', font_size=25)
00142     stringGeometry = "Geometry: 1 Unit = 1.0m"
00143     legendGeometry = svg.text(stringGeometry)
00144     legend.add(legendGeometry)
00145     legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
00146     svg.add(legend)
00147 
00148 
00149     #arrow = arrowMarker() # Init arrow marker
00150 
00151     rospy.loginfo("Initialization completed.\nWaiting for feedback message...") 
00152 
00153     # -------------------- WAIT FOR CALLBACKS --------------------------  
00154     while not rospy.is_shutdown():
00155         if feedbackMsg:
00156             break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
00157         rate.sleep()
00158     # ------------------------------------------------------------------
00159     
00160     if not feedbackMsg.trajectories:
00161       rospy.loginfo("Received message does not contain trajectories. Shutting down...")
00162       sys.exit()
00163     
00164     if len(feedbackMsg.trajectories[0].trajectory) < 2:
00165       rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
00166       sys.exit()
00167         
00168     # iterate trajectories
00169     for index, traj in enumerate(feedbackMsg.trajectories):
00170       
00171         #color
00172         traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
00173    
00174         # Iterate through TEB positions -> Draw Paths
00175         points = []
00176         for point in traj.trajectory:
00177             points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
00178             # svgwrite rotates clockwise!
00179             
00180 
00181         if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
00182           line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
00183                           stroke_linejoin='round', opacity=1.0 ) )
00184         else:
00185           line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
00186                           stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
00187         #marker_points = points[::7]
00188         #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
00189         #arrow = arrowMarker(traj_color)
00190         #markerline.set_markers( (arrow, arrow, arrow) )
00191         #line.set_markers( (arrow, arrow, arrow) )
00192         #line['marker-start'] = arrow.get_funciri()
00193     
00194   
00195     # Add Start and Goal Point
00196     start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
00197     goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
00198     start_position = start_pose.position
00199     goal_position = goal_pose.position
00200     svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
00201     svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
00202     svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
00203     svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
00204     
00205     # draw start arrow
00206     start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
00207     start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
00208     start_arrow.rotate( quaternion2YawDegree(start_pose.orientation)  )
00209     start_arrow.scale(3)
00210     svg.add(start_arrow)
00211     
00212     # draw goal arrow
00213     goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
00214     goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
00215     goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation)  )
00216     goal_arrow.scale(3)
00217     svg.add(goal_arrow)
00218     
00219     # Draw obstacles
00220     for obstacle in feedbackMsg.obstacles:
00221       if len(obstacle.polygon.points) == 1: # point obstacle
00222           point = obstacle.polygon.points[0]
00223           svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
00224           svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
00225           svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
00226       if len(obstacle.polygon.points) == 2: # line obstacle
00227           line_start = obstacle.polygon.points[0]
00228           line_end = obstacle.polygon.points[1]
00229           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))
00230           svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
00231       if len(obstacle.polygon.points) > 2: # polygon obstacle
00232           vertices = []
00233           for point in obstacle.polygon.points:
00234             vertices.append((point.x*SCALE, -point.y*SCALE))
00235           svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
00236           svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
00237           
00238           
00239     
00240     # Save svg to file (svg_output.svg) and exit node            
00241     svg.save() 
00242     
00243     rospy.loginfo("Drawing completed.") 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34