Functions | Variables
export_to_svg Namespace Reference

Functions

def arrowMarker (color='green', orientation='auto')
 
def feedback_callback (data)
 
def quaternion2YawDegree (orientation)
 
def sign (number)
 

Variables

 anonymous
 
 feedback_callback
 
 FeedbackMsg
 
list feedbackMsg = []
 
string filename_string = "teb_svg_"
 
 goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
 
 goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
 
 goal_position = goal_pose.position
 
int GRID_X_MAX = 2
 
int GRID_X_MIN = -2
 
int GRID_Y_MAX = 1
 
int GRID_Y_MIN = -2
 
 hLines = svg.add(svg.g(id='hLines', stroke='black'))
 
 legend = svg.g(id='legend', font_size=25)
 
 legendGeometry = svg.text(stringGeometry)
 
 line
 
 line_end = obstacle.polygon.points[1]
 
 line_start = obstacle.polygon.points[0]
 
float MIN_POSE_DISTANCE = 0.3
 
int OBSTACLE_DIST = 50
 
 point = obstacle.polygon.points[0]
 
list points = []
 
 queue_size
 
 rate = rospy.Rate(10.0)
 
int SCALE = 200
 
float SCALE_VELOCITY_VEC = 0.4
 
 start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
 
 start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
 
 start_position = start_pose.position
 
string stringGeometry = "Geometry: 1 Unit = 1.0m"
 
 svg = svgwrite.Drawing(filename=filename_string, debug=True)
 
 timestr = time.strftime("%Y%m%d_%H%M%S")
 
string topic_name = "/test_optim_node/teb_feedback"
 
 traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
 
 tx
 
 ty
 
list vertices = []
 
 vLines = svg.add(svg.g(id='vline', stroke='black'))
 

Function Documentation

def export_to_svg.arrowMarker (   color = 'green',
  orientation = 'auto' 
)
Create an arrow marker with svgwrite

@return:  arrow marker
@rtype:   svg_write marker object 

Definition at line 55 of file export_to_svg.py.

def export_to_svg.feedback_callback (   data)
Callback for receiving TEB and obstacle information

@param data: Received feedback message
@type  data: visualization_msgs/Marker

@globalparam tebList: Received TEB List
@globaltype  tebList: teb_local_planner/FeedbackMsg

Definition at line 81 of file export_to_svg.py.

def export_to_svg.quaternion2YawDegree (   orientation)
Get yaw angle [degree] from quaternion representation

@param orientation:     orientation in quaternions to read from
@type  orientation:     geometry_msgs/Quaternion
@return: yaw angle [degree]
@rtype:  float  

Definition at line 68 of file export_to_svg.py.

def export_to_svg.sign (   number)
Signum function: get sign of a number

@param number: get sign of this number
@type  number: numeric type (eg. integer)
@return:  sign of number
@rtype:   integer {1, -1, 0}  

Definition at line 44 of file export_to_svg.py.

Variable Documentation

export_to_svg.anonymous

Definition at line 102 of file export_to_svg.py.

export_to_svg.feedback_callback

Definition at line 106 of file export_to_svg.py.

export_to_svg.FeedbackMsg

Definition at line 106 of file export_to_svg.py.

list export_to_svg.feedbackMsg = []

Definition at line 111 of file export_to_svg.py.

string export_to_svg.filename_string = "teb_svg_"

Definition at line 114 of file export_to_svg.py.

export_to_svg.goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)

Definition at line 213 of file export_to_svg.py.

export_to_svg.goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose

Definition at line 197 of file export_to_svg.py.

export_to_svg.goal_position = goal_pose.position

Definition at line 199 of file export_to_svg.py.

int export_to_svg.GRID_X_MAX = 2

Definition at line 34 of file export_to_svg.py.

int export_to_svg.GRID_X_MIN = -2

Definition at line 33 of file export_to_svg.py.

int export_to_svg.GRID_Y_MAX = 1

Definition at line 36 of file export_to_svg.py.

int export_to_svg.GRID_Y_MIN = -2

Definition at line 35 of file export_to_svg.py.

export_to_svg.hLines = svg.add(svg.g(id='hLines', stroke='black'))

Definition at line 126 of file export_to_svg.py.

export_to_svg.legend = svg.g(id='legend', font_size=25)

Definition at line 141 of file export_to_svg.py.

export_to_svg.legendGeometry = svg.text(stringGeometry)

Definition at line 143 of file export_to_svg.py.

export_to_svg.line
Initial value:
1 = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
2  stroke_linejoin='round', opacity=1.0 ) )

Definition at line 182 of file export_to_svg.py.

export_to_svg.line_end = obstacle.polygon.points[1]

Definition at line 228 of file export_to_svg.py.

export_to_svg.line_start = obstacle.polygon.points[0]

Definition at line 227 of file export_to_svg.py.

float export_to_svg.MIN_POSE_DISTANCE = 0.3

Definition at line 31 of file export_to_svg.py.

int export_to_svg.OBSTACLE_DIST = 50

Definition at line 39 of file export_to_svg.py.

export_to_svg.point = obstacle.polygon.points[0]

Definition at line 222 of file export_to_svg.py.

list export_to_svg.points = []

Definition at line 175 of file export_to_svg.py.

export_to_svg.queue_size

Definition at line 106 of file export_to_svg.py.

export_to_svg.rate = rospy.Rate(10.0)

Definition at line 110 of file export_to_svg.py.

export_to_svg.SCALE = 200

Definition at line 30 of file export_to_svg.py.

float export_to_svg.SCALE_VELOCITY_VEC = 0.4

Definition at line 32 of file export_to_svg.py.

export_to_svg.start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)

Definition at line 206 of file export_to_svg.py.

export_to_svg.start_pose = feedbackMsg.trajectories[0].trajectory[0].pose

Definition at line 196 of file export_to_svg.py.

export_to_svg.start_position = start_pose.position

Definition at line 198 of file export_to_svg.py.

string export_to_svg.stringGeometry = "Geometry: 1 Unit = 1.0m"

Definition at line 142 of file export_to_svg.py.

export_to_svg.svg = svgwrite.Drawing(filename=filename_string, debug=True)

Definition at line 120 of file export_to_svg.py.

export_to_svg.timestr = time.strftime("%Y%m%d_%H%M%S")

Definition at line 113 of file export_to_svg.py.

export_to_svg.topic_name = "/test_optim_node/teb_feedback"

Definition at line 104 of file export_to_svg.py.

export_to_svg.traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')

Definition at line 172 of file export_to_svg.py.

export_to_svg.tx

Definition at line 145 of file export_to_svg.py.

export_to_svg.ty

Definition at line 145 of file export_to_svg.py.

list export_to_svg.vertices = []

Definition at line 232 of file export_to_svg.py.

export_to_svg.vLines = svg.add(svg.g(id='vline', stroke='black'))

Definition at line 132 of file export_to_svg.py.



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