Joe view goals

src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/poop.yaml

@@ -0,0 +1,8 @@
+mission:
+  - task: "track_object"
+    parameters:
+      object_type: "green_square"
+      type_of_movement: "rotate"
+      max_duration: 120.0
+      desired_distance: 2.0
+      keep_centered: true

src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py

@@ -17,7 +17,6 @@
         ),
         (f"share/{package_name}", ["package.xml"]),
         (f"share/{package_name}/missions", glob("missions/*.yaml")),
-        (f"share/{package_name}/missions", ["missions/prequal.yaml"]),
         (
             f"share/{package_name}/launch",
             ["launch/mission_planner_launch.py", "launch/task_server_launch.py"],
@@ -39,6 +38,7 @@
             "search_server = subjugator_mission_planner.search_server:main",
             "movement_server = subjugator_mission_planner.movement_server:main",
             "wait_server = subjugator_mission_planner.wait_server:main",
+            "track_object = subjugator_mission_planner.track_object_server:main",
         ],
     },
 )

src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/mission_planner.py

@@ -4,11 +4,12 @@
 import yaml
 from ament_index_python.packages import get_package_share_directory
 from geometry_msgs.msg import Pose
-from rclpy.action import ActionClient
+from rclpy.action.client import ActionClient
 from rclpy.node import Node
 from subjugator_msgs.action import (
     Movement,
     NavigateAround,
+    TrackObject,
     Wait,
 )
 
@@ -19,8 +20,7 @@ def __init__(self):
 
         package_share = get_package_share_directory("subjugator_mission_planner")
         print(f"PATH: {package_share}")
-
-        default_mission_file = os.path.join("prequal.yaml")
+        default_mission_file = os.path.join(package_share, "missions", "poop.yaml")
         self.declare_parameter("mission_file", default_mission_file)
         mission_file = os.path.join(
             package_share,
@@ -48,6 +48,11 @@ def __init__(self):
             Wait,
             "wait",
         )
+        self.track_object_client = ActionClient(
+            self,
+            TrackObject,
+            "track_object",
+        )
 
         # Timer to periodically check mission progress and start tasks
         self.timer = self.create_timer(0.5, self.execute_mission)
@@ -88,6 +93,8 @@ def execute_mission(self):
             self.send_navigate_around_goal(params)
         elif task_name == "wait":
             self.send_wait_goal(params)
+        elif task_name == "track_object":
+            self.send_track_object_goal(params)
         else:
             self.get_logger().error(f"Unknown task: {task_name}, skipping")
             self.current_task_index += 1
@@ -113,6 +120,21 @@ def send_move_to_goal(self, params):
         self.executing_task = True
         self._send_goal(self.movement_client, goal_msg)
 
+    def send_track_object_goal(self, params):
+        if not self.track_object_client.wait_for_server(timeout_sec=2.0):
+            self.get_logger().error("Track object goal no working")
+            return
+
+        goal_msg = TrackObject.Goal()
+        goal_msg.object_type = "green"
+        goal_msg.type_of_movement = params.get(
+            "type_of_movement",
+            "rotate",
+        )  # either translate or rotate
+
+        self.executing_task = True
+        self._send_goal(self.track_object_client, goal_msg)
+
     def send_navigate_around_goal(self, params):
         if not self.navigate_around_client.wait_for_server(timeout_sec=2.0):
             self.get_logger().error("Navigate around action server not available")