Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#! /usr/bin/python
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler
from std_msgs.msg import Float64
got_path = False
def path_callback(msg):
global got_path
if msg.data == -1.0:
got_path = True
def path_publisher():
rospy.init_node('path_publisher')
rospy.Subscriber('got_path', Float64, path_callback)
path_pub = rospy.Publisher('/drive_path', Path, queue_size=10)
path = Path()
path.header.frame_id = "map"
path.header.seq = 0
path.header.stamp = rospy.Time.now()
seq = 0
for line in content:
#print(line)
pose = PoseStamped()
points = line.split()
#print(points)
x = float(points[0])
y = float(points[1])
yaw = float(points[2])
quat = quaternion_from_euler(0, 0, yaw)
pose.header.frame_id = "map"
pose.header.seq = seq
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = 0
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
pose.header.stamp = path.header.stamp
path.poses.append(pose)
seq += 1
while not rospy.is_shutdown():
if (got_path is False):
print("Sending Path")
path_pub.publish(path)
else:
break
rospy.sleep(5)
content = {}
def load_file():
# TODO: Get filename from rosparam
# File format each waypoint on its own line:
# x y yaw
# x y yaw
global content
try:
with open('generated_points.txt', 'r') as file:
content = file.readlines()
content = [x.strip() for x in content]
except:
rospy.loginfo("File failed to load")
if __name__ == '__main__':
try:
load_file()
path_publisher()
except rospy.ROSInterruptException:
pass
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment