git commit -m "first commit"

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
+