47 lines
943 B
Python
Executable File
47 lines
943 B
Python
Executable File
#!/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
|
|
|