diff --git a/src/example_robot_brain/launch/task_mapping.launch b/src/example_robot_brain/launch/task_mapping.launch index 5024d4b..c3108aa 100644 --- a/src/example_robot_brain/launch/task_mapping.launch +++ b/src/example_robot_brain/launch/task_mapping.launch @@ -5,7 +5,7 @@ - + diff --git a/src/example_robot_brain/scripts/detection.py b/src/example_robot_brain/scripts/detection.py index e50762e..d64dbfa 100755 --- a/src/example_robot_brain/scripts/detection.py +++ b/src/example_robot_brain/scripts/detection.py @@ -10,6 +10,7 @@ ] i = 0 + # Does not actually detect anything, just returns hardcoded locations def detect(): global i diff --git a/src/example_robot_brain/scripts/navigation.py b/src/example_robot_brain/scripts/navigation.py index ea28080..83f3d36 100755 --- a/src/example_robot_brain/scripts/navigation.py +++ b/src/example_robot_brain/scripts/navigation.py @@ -15,4 +15,3 @@ def drive(duration=10): msg.linear.x = 1 pub.publish(msg) rate.sleep() - diff --git a/src/example_robot_brain/scripts/task_mapping.py b/src/example_robot_brain/scripts/task_mapping.py index 865368a..9b889e6 100755 --- a/src/example_robot_brain/scripts/task_mapping.py +++ b/src/example_robot_brain/scripts/task_mapping.py @@ -3,22 +3,20 @@ import csv import rospy -import rospkg from navigation import drive from detection import detect from std_msgs.msg import String +from virtual_maize_field import get_markers_file + if __name__ == "__main__": rospy.init_node("task_mapping_node") - pkg_path = rospkg.RosPack().get_path("virtual_maize_field") - # Read the relative position of the pillars - markers_path = os.path.join(pkg_path, "map/markers.csv") - with open(markers_path, "r") as f: + with open(get_markers_file(), "r") as f: reader = csv.reader(f) # Skip header @@ -34,7 +32,7 @@ # Create path to file where we have to save the predicted coordinates of the # litter and weeds - out_map_path = os.path.join(pkg_path, "map/pred_map.csv") + out_map_path = os.path.join(os.path.expanduser("~"), "pred_map.csv") try: # Open output file @@ -63,7 +61,7 @@ # Write found position to file writer.writerow([obj_position[0], obj_position[1], obj_type]) - + rospy.loginfo("Done driving") except rospy.ROSInterruptException: diff --git a/src/example_robot_brain/scripts/task_navigation.py b/src/example_robot_brain/scripts/task_navigation.py index 1288156..14f557f 100755 --- a/src/example_robot_brain/scripts/task_navigation.py +++ b/src/example_robot_brain/scripts/task_navigation.py @@ -1,18 +1,16 @@ #!/usr/bin/env python3 -import os -import rospkg + import rospy from navigation import drive +from virtual_maize_field import get_driving_pattern_file + + if __name__ == "__main__": rospy.init_node("task_navigation_node") - # Read the driving directions from the file - pkg_path = rospkg.RosPack().get_path("virtual_maize_field") - dp_path = os.path.join(pkg_path, "map/driving_pattern.txt") - - with open(dp_path) as direction_f: + with open(get_driving_pattern_file()) as direction_f: directions = direction_f.readline() rospy.loginfo(f"Driving directions are: {directions}") @@ -20,9 +18,9 @@ try: # Drive rospy.loginfo("Starting to drive") - + drive() - + rospy.loginfo("Done driving") except rospy.ROSInterruptException: pass