Created
December 4, 2020 03:15
-
-
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
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#! /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