git commit -m "first commit"

navigations/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from geometry_msgs.msg import Twist
+from ackermann_msgs.msg import AckermannDriveStamped
+
+
+def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase):
+  if omega == 0 or v == 0:
+    return 0
+
+  radius = v / omega
+  return math.atan(wheelbase / radius)
+
+
+def cmd_callback(data):
+  global wheelbase
+  global ackermann_cmd_topic
+  global frame_id
+  global pub
+  global cmd_angle_instead_rotvel
+  
+  v = data.linear.x
+  # if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node
+  # in this case this script only needs to do the msg conversion from twist to Ackermann drive
+  if cmd_angle_instead_rotvel:
+    steering = data.angular.z
+  else:
+    steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
+  
+  msg = AckermannDriveStamped()
+  msg.header.stamp = rospy.Time.now()
+  msg.header.frame_id = frame_id
+  msg.drive.steering_angle = steering
+  msg.drive.speed = v
+  
+  pub.publish(msg)
+  
+
+
+
+
+if __name__ == '__main__': 
+  try:
+    
+    rospy.init_node('cmd_vel_to_ackermann_drive')
+        
+    twist_cmd_topic = rospy.get_param('~twist_cmd_topic', '/cmd_vel') 
+    ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic', '/ackermann_cmd')
+    wheelbase = rospy.get_param('~wheelbase', 1.0)
+    frame_id = rospy.get_param('~frame_id', 'odom')
+    cmd_angle_instead_rotvel = rospy.get_param('/move_base/TebLocalPlannerROS/cmd_angle_instead_rotvel', False)
+    
+    rospy.Subscriber(twist_cmd_topic, Twist, cmd_callback, queue_size=1)
+    pub = rospy.Publisher(ackermann_cmd_topic, AckermannDriveStamped, queue_size=1)
+    
+    rospy.loginfo("Node 'cmd_vel_to_ackermann_drive' started.\nListening to %s, publishing to %s. Frame id: %s, wheelbase: %f", "/cmd_vel", ackermann_cmd_topic, frame_id, wheelbase)
+    
+    rospy.spin()
+    
+  except rospy.ROSInterruptException:
+    pass
+

navigations/teb_local_planner/scripts/export_to_mat.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and exports data to a mat file.
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
+from tf.transformations import euler_from_quaternion
+import numpy as np
+import scipy.io as sio
+import time
+
+def feedback_callback(data):
+  global got_data
+
+  if not data.trajectories: # empty
+    trajectory = []
+    return
+  
+  if got_data:		
+    return
+
+  got_data = True
+  
+  # copy trajectory
+  trajectories = []
+  for traj in data.trajectories:
+    trajectory = []
+#    # store as struct and cell array
+#    for point in traj.trajectory:
+#      (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
+#      pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw}
+#      velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z}
+#      time_from_start = point.time_from_start.to_sec()
+#      trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start})
+      
+    # store as all-in-one mat
+    arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t
+    for index, point in enumerate(traj.trajectory):
+      arr[0,index] = point.pose.position.x
+      arr[1,index] = point.pose.position.y
+      (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
+      arr[2,index] = yaw
+      arr[3,index] = point.velocity.linear.x
+      arr[4,index] = point.velocity.angular.z
+      arr[5,index] = point.time_from_start.to_sec()
+      
+#   trajectories.append({'raw': trajectory, 'mat': arr})
+    trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']})
+  
+  # copy obstacles
+  obstacles = []
+  for obst_id, obst in enumerate(data.obstacle_msg.obstacles):
+    #polygon = []
+    #for point in obst.polygon.points:
+    #  polygon.append({'x': point.x, 'y': point.y, 'z': point.z})
+    obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y
+    for index, point in enumerate(obst.polygon.points):
+      obst_arr[0, index] = point.x
+      obst_arr[1, index] = point.y
+      obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x
+      obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y
+
+    #obstacles.append(polygon)
+    obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']})
+  
+  
+  # create main struct:
+  mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles}
+
+  timestr = time.strftime("%Y%m%d_%H%M%S")
+  filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat'
+  
+  rospy.loginfo("Saving mat-file '%s'.", filename)
+  sio.savemat(filename, mat)
+  
+
+  
+  
+  
+def feedback_exporter():
+  global got_data
+
+  rospy.init_node("export_to_mat", anonymous=True)
+  
+  
+  topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
+
+  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 
+
+  rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
+ 
+  r = rospy.Rate(2) # define rate here
+  while not rospy.is_shutdown():
+    
+    if got_data:
+      rospy.loginfo("Data export completed.")
+      return
+
+    r.sleep()
+
+if __name__ == '__main__': 
+  try:
+    global got_data
+    got_data = False
+    feedback_exporter()
+  except rospy.ROSInterruptException:
+    pass
+

navigations/teb_local_planner/scripts/export_to_svg.py

@@ -0,0 +1,244 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+========================================================================================
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and converts the current scene to a svg-image
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+It is recommendable to start this node after initialization of TEB is completed.
+
+Requirements:
+svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite
+=======================================================================================
+"""
+import roslib;
+import rospy
+import svgwrite
+import math
+import sys
+import time
+import random
+from svgwrite import cm, mm
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32, Quaternion
+
+
+# ================= PARAMETERS ==================
+# TODO: In case of a more general node, change parameter to ros-parameter
+# Drawing parameters: 
+SCALE = 200	 # Overall scaling: 100 pixel = 1 m
+MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image
+SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s
+GRID_X_MIN = -2  # Define, how many cells your grid should contain in each direction.
+GRID_X_MAX = 2
+GRID_Y_MIN = -2
+GRID_Y_MAX = 1
+
+# TEB parameters:
+OBSTACLE_DIST = 50 *SCALE/100 # cm
+
+
+# ================= FUNCTIONS ===================
+
+def 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}  
+    """
+    return cmp(number,0)
+
+def arrowMarker(color='green', orientation='auto'):
+    """
+    Create an arrow marker with svgwrite
+
+    @return:  arrow marker
+    @rtype:   svg_write marker object 
+    """
+    arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation)
+    arrow.viewbox(width=10, height=10)
+    arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0))
+    svg.defs.add(arrow)
+    return arrow
+
+def 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  
+    """
+    yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2)))
+    return yawRad*180/math.pi
+
+
+def 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
+    """
+    # TODO: Remove global variables
+    global feedbackMsg
+    
+    if not feedbackMsg:
+      feedbackMsg = data
+      rospy.loginfo("TEB feedback message received...") 
+               
+   
+# ================ MAIN FUNCTION ================
+
+if __name__ == '__main__':
+    rospy.init_node('export_to_svg', anonymous=True)
+    
+    topic_name = "/test_optim_node/teb_feedback" # define feedback topic here!
+
+    rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) 
+
+    rospy.loginfo("Waiting for feedback message on topic %s.", topic_name)
+
+    rate = rospy.Rate(10.0)
+    feedbackMsg = []
+
+    timestr = time.strftime("%Y%m%d_%H%M%S")
+    filename_string = "teb_svg_" + timestr + '.svg'
+  
+    rospy.loginfo("SVG will be written to '%s'.", filename_string)
+    
+    random.seed(0)
+
+    svg=svgwrite.Drawing(filename=filename_string, debug=True)
+    
+    # Create viewbox -> this box defines the size of the visible drawing
+    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)
+
+    # Draw grid:
+    hLines = svg.add(svg.g(id='hLines', stroke='black'))
+    hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0)))
+    for y in range(GRID_Y_MAX):
+        hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE)))
+    for y in range(-GRID_Y_MIN):
+        hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE)))
+    vLines = svg.add(svg.g(id='vline', stroke='black'))
+    vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE)))
+    for x in range(GRID_X_MAX):
+        vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE)))
+    for x in range(-GRID_X_MIN):
+        vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE)))
+
+
+    # Draw legend:
+    legend = svg.g(id='legend', font_size=25)
+    stringGeometry = "Geometry: 1 Unit = 1.0m"
+    legendGeometry = svg.text(stringGeometry)
+    legend.add(legendGeometry)
+    legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner
+    svg.add(legend)
+
+
+    #arrow = arrowMarker() # Init arrow marker
+
+    rospy.loginfo("Initialization completed.\nWaiting for feedback message...") 
+
+    # -------------------- WAIT FOR CALLBACKS --------------------------  
+    while not rospy.is_shutdown():
+        if feedbackMsg:
+            break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing
+        rate.sleep()
+    # ------------------------------------------------------------------
+    
+    if not feedbackMsg.trajectories:
+      rospy.loginfo("Received message does not contain trajectories. Shutting down...")
+      sys.exit()
+    
+    if len(feedbackMsg.trajectories[0].trajectory) < 2:
+      rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...")
+      sys.exit()
+        
+    # iterate trajectories
+    for index, traj in enumerate(feedbackMsg.trajectories):
+      
+        #color
+        traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB')
+   
+        # Iterate through TEB positions -> Draw Paths
+        points = []
+        for point in traj.trajectory:
+            points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates
+            # svgwrite rotates clockwise!
+            
+
+        if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb
+          line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \
+                          stroke_linejoin='round', opacity=1.0 ) )
+        else:
+          line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \
+                          stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) )
+        #marker_points = points[::7]
+        #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) )
+        #arrow = arrowMarker(traj_color)
+        #markerline.set_markers( (arrow, arrow, arrow) )
+        #line.set_markers( (arrow, arrow, arrow) )
+        #line['marker-start'] = arrow.get_funciri()
+    
+  
+    # Add Start and Goal Point
+    start_pose = feedbackMsg.trajectories[0].trajectory[0].pose
+    goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose
+    start_position = start_pose.position
+    goal_position = goal_pose.position
+    svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue'))
+    svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label
+    svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red'))
+    svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label
+    
+    # draw start arrow
+    start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0)
+    start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE)
+    start_arrow.rotate( quaternion2YawDegree(start_pose.orientation)  )
+    start_arrow.scale(3)
+    svg.add(start_arrow)
+    
+    # draw goal arrow
+    goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0)
+    goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE)
+    goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation)  )
+    goal_arrow.scale(3)
+    svg.add(goal_arrow)
+    
+    # Draw obstacles
+    for obstacle in feedbackMsg.obstacles:
+      if len(obstacle.polygon.points) == 1: # point obstacle
+          point = obstacle.polygon.points[0]
+          svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3))
+          svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black'))
+          svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label
+      if len(obstacle.polygon.points) == 2: # line obstacle
+          line_start = obstacle.polygon.points[0]
+          line_end = obstacle.polygon.points[1]
+          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))
+          svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label
+      if len(obstacle.polygon.points) > 2: # polygon obstacle
+          vertices = []
+          for point in obstacle.polygon.points:
+            vertices.append((point.x*SCALE, -point.y*SCALE))
+          svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0))
+          svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label
+          
+          
+    
+    # Save svg to file (svg_output.svg) and exit node            
+    svg.save() 
+    
+    rospy.loginfo("Drawing completed.") 

navigations/teb_local_planner/scripts/publish_dynamic_obstacle.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+# Author: franz.albers@tu-dortmund.de
+
+import rospy, math, tf
+from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
+from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance
+from tf.transformations import quaternion_from_euler
+
+
+def publish_obstacle_msg():
+  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
+  #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
+  rospy.init_node("test_obstacle_msg")
+
+  y_0 = -3.0
+  vel_x = 0.0
+  vel_y = 0.3
+  range_y = 6.0
+
+  obstacle_msg = ObstacleArrayMsg() 
+  obstacle_msg.header.stamp = rospy.Time.now()
+  obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map
+  
+  # Add point obstacle
+  obstacle_msg.obstacles.append(ObstacleMsg())
+  obstacle_msg.obstacles[0].id = 99
+  obstacle_msg.obstacles[0].polygon.points = [Point32()]
+  obstacle_msg.obstacles[0].polygon.points[0].x = -1.5
+  obstacle_msg.obstacles[0].polygon.points[0].y = 0
+  obstacle_msg.obstacles[0].polygon.points[0].z = 0
+
+  yaw = math.atan2(vel_y, vel_x)
+  q = tf.transformations.quaternion_from_euler(0,0,yaw)
+  obstacle_msg.obstacles[0].orientation = Quaternion(*q)
+
+  obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
+  obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
+  obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
+  obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
+  obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
+  obstacle_msg.obstacles[0].velocities.twist.angular.z = 0
+
+  r = rospy.Rate(10) # 10hz
+  t = 0.0
+  while not rospy.is_shutdown():
+    
+    # Vary y component of the point obstacle
+    if (vel_y >= 0):
+      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
+    else:
+      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y
+
+    t = t + 0.1
+    
+    pub.publish(obstacle_msg)
+    
+    r.sleep()
+
+
+
+if __name__ == '__main__': 
+  try:
+    publish_obstacle_msg()
+  except rospy.ROSInterruptException:
+    pass
+

navigations/teb_local_planner/scripts/publish_test_obstacles.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
+from geometry_msgs.msg import PolygonStamped, Point32
+
+
+def publish_obstacle_msg():
+  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
+  #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)
+  rospy.init_node("test_obstacle_msg")
+
+
+  obstacle_msg = ObstacleArrayMsg() 
+  obstacle_msg.header.stamp = rospy.Time.now()
+  obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
+  
+  # Add point obstacle
+  obstacle_msg.obstacles.append(ObstacleMsg())
+  obstacle_msg.obstacles[0].id = 0
+  obstacle_msg.obstacles[0].polygon.points = [Point32()]
+  obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
+  obstacle_msg.obstacles[0].polygon.points[0].y = 0
+  obstacle_msg.obstacles[0].polygon.points[0].z = 0
+
+
+  # Add line obstacle
+  obstacle_msg.obstacles.append(ObstacleMsg())
+  obstacle_msg.obstacles[1].id = 1
+  line_start = Point32()
+  line_start.x = -2.5
+  line_start.y = 0.5
+  #line_start.y = -3
+  line_end = Point32()
+  line_end.x = -2.5
+  line_end.y = 2
+  #line_end.y = -4
+  obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
+  
+  # Add polygon obstacle
+  obstacle_msg.obstacles.append(ObstacleMsg())
+  obstacle_msg.obstacles[1].id = 2
+  v1 = Point32()
+  v1.x = -1
+  v1.y = -1
+  v2 = Point32()
+  v2.x = -0.5
+  v2.y = -1.5
+  v3 = Point32()
+  v3.x = 0
+  v3.y = -1
+  obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]
+  
+
+  r = rospy.Rate(10) # 10hz
+  t = 0.0
+  while not rospy.is_shutdown():
+    
+    # Vary y component of the point obstacle
+    obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
+    t = t + 0.1
+    
+    pub.publish(obstacle_msg)
+    
+    r.sleep()
+
+
+
+if __name__ == '__main__': 
+  try:
+    publish_obstacle_msg()
+  except rospy.ROSInterruptException:
+    pass
+

navigations/teb_local_planner/scripts/publish_viapoints.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+
+def publish_via_points_msg():
+  pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1)
+  rospy.init_node("test_via_points_msg")
+
+
+  via_points_msg = Path() 
+  via_points_msg.header.stamp = rospy.Time.now()
+  via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
+  
+  # Add via-points
+  point1 = PoseStamped()
+  point1.pose.position.x = 0.0;
+  point1.pose.position.y = 1.5;
+
+  point2 = PoseStamped()
+  point2.pose.position.x = 2.0;
+  point2.pose.position.y = -0.5;
+
+
+  via_points_msg.poses = [point1, point2]
+
+  r = rospy.Rate(5) # 10hz
+  t = 0.0
+  while not rospy.is_shutdown():
+        
+    pub.publish(via_points_msg)
+    
+    r.sleep()
+
+
+
+if __name__ == '__main__': 
+  try:
+    publish_via_points_msg()
+  except rospy.ROSInterruptException:
+    pass
+

navigations/teb_local_planner/scripts/visualize_velocity_profile.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+# This small script subscribes to the FeedbackMsg message of teb_local_planner
+# and plots the current velocity.
+# publish_feedback must be turned on such that the planner publishes this information.
+# Author: christoph.roesmann@tu-dortmund.de
+
+import rospy, math
+from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg
+from geometry_msgs.msg import PolygonStamped, Point32
+import numpy as np
+import matplotlib.pyplot as plotter
+
+def feedback_callback(data):
+  global trajectory
+
+  if not data.trajectories: # empty
+    trajectory = []
+    return
+  trajectory = data.trajectories[data.selected_trajectory_idx].trajectory
+  
+  
+def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega):
+  ax_v.cla()
+  ax_v.grid()
+  ax_v.set_ylabel('Trans. velocity [m/s]')
+  ax_v.plot(t, v, '-bx')
+  ax_omega.cla()
+  ax_omega.grid()
+  ax_omega.set_ylabel('Rot. velocity [rad/s]')
+  ax_omega.set_xlabel('Time [s]')
+  ax_omega.plot(t, omega, '-bx')
+  fig.canvas.draw()
+
+  
+  
+def velocity_plotter():
+  global trajectory
+  rospy.init_node("visualize_velocity_profile", anonymous=True)
+  
+  topic_name = "/test_optim_node/teb_feedback"
+  topic_name = rospy.get_param('~feedback_topic', topic_name)
+  rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here!
+
+  rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) 
+  rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.")
+
+  # two subplots sharing the same t axis
+  fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True)
+  plotter.ion()
+  plotter.show()
+  
+
+  r = rospy.Rate(2) # define rate here
+  while not rospy.is_shutdown():
+    
+    t = []
+    v = []
+    omega = []
+    
+    for point in trajectory:
+      t.append(point.time_from_start.to_sec())
+      v.append(point.velocity.linear.x)
+      omega.append(point.velocity.angular.z)
+          
+    plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega))
+        
+    r.sleep()
+
+if __name__ == '__main__': 
+  try:
+    trajectory = []
+    velocity_plotter()
+  except rospy.ROSInterruptException:
+    pass
+