Skip to content

Instantly share code, notes, and snippets.

@jones2126
Created December 4, 2020 03:15
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save jones2126/4d9a792babfc34533987f625722b3875 to your computer and use it in GitHub Desktop.
Save jones2126/4d9a792babfc34533987f625722b3875 to your computer and use it in GitHub Desktop.
This node will read the generated points file and publish a path to the topic /drive_path
0.0 0.0 0.0
1.0 0.0 0.0
2.0 0.0 0.0
3.0 0.0 0.0
4.0 0.0 0.0
5.0 0.0 0.0
6.0 0.0 0.0
7.0 0.0 0.0
8.0 0.0 0.0
9.0 0.0 0.0
10.0 0.0 0.0
10.9588510772 0.244834876219 0.5
11.6829419696 0.919395388264 1.0
12.345132615 1.65599879199 0.592802814922
13.2771689026 1.98863652817 0.0928028149222
14.2545827214 1.8337116057 5.8759881221
15.0380690802 1.2291550488 5.3759881221
15.4358031933 0.322983387245 4.8759881221
15.3504058782 -0.662940952378 4.3759881221
14.8027853761 -1.48722930641 3.8759881221
13.9270182849 -1.94806713773 3.3759881221
12.9375229319 -1.93262527295 2.8759881221
12.0765622893 -1.44468441911 2.3759881221
11.4731950798 -0.650812169696 2.31120906298
10.6456264892 -0.108161515323 2.81120906298
10.0 0.0 3.14
9.00000000135 2.03052971583e-06 3.14159316122
8.00000000135 1.52289728786e-06 3.14159316122
7.00000000135 1.01526485989e-06 3.14159316122
6.00000000135 5.07632431918e-07 3.14159316122
5.00000000135 3.9479625454e-15 3.14159316122
4.00000000135 -5.07632424022e-07 3.14159316122
3.00000000135 -1.01526485199e-06 3.14159316122
2.00000000135 -1.52289727996e-06 3.14159316122
1.00000000135 -2.03052970793e-06 3.14159316122
2.69450950441e-09 -4.28492797529e-12 3.14000000135
0.0 0.0 3.14
-0.958459924144 0.246361682667 2.64
-1.68196375605 0.921750282441 2.17423741664
-2.42768581502 1.57231781594 2.67423741664
-3.39401718013 1.78572633891 3.17423741664
-4.34436623123 1.50972600203 3.67423741664
-5.04605437602 0.811891313629 4.17423741664
-5.32728388443 -0.136923456743 4.67423741664
-5.11919996458 -1.10441536223 5.17423741664
-4.47274881722 -1.8537086419 5.67423741664
-3.54620422901 -2.20135016838 6.17423741664
-2.56641662951 -2.06222517156 0.391052109465
-1.77327219436 -1.47039630282 0.891052109465
-1.21827846334 -0.642002078765 0.824398108683
-0.387476690619 -0.104314512131 0.324398108683
#! /usr/bin/python
# credit Matt Droter
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