state_machine + action_planner: port 9 scripts/ Python files from rospy to rclpy

The per-package subagents focused on the C++ side and missed the 9
Python scripts living under action_planner/scripts/ and
state_machine_core/scripts/ that still imported rospy and actionlib.
These are dev tools (trajectory evaluators, a goal publisher) and the
main state-machine orchestrator, none of which are installed by
CMakeLists. Port them to rclpy under a separate subagent pass.

Scripts ported (9 files, ~1886 source lines):

action_planner/scripts/:
- publish_plantwopointaction.py: rclpy.create_node + rclpy.action.
  ActionClient(PlanTwoPoint) replacing actionlib.SimpleActionClient
  and the PlanTwoPointActionGoal publisher. send_goal_async +
  spin_until_future_complete flow.
- evaluate_traj.py: Evaluater(node) class with 3 ActionClient
  migrations. Local variable `time` renamed to `time_total` to avoid
  shadowing the time module (pre-existing latent bug).
- evaluate_traj_exp.py: 10 planner ActionClient migrations. Every
  ServiceProxy -> create_client + call_async + spin_until_future_complete.
  rospy.get_param_names() has no rclpy equivalent; a best-effort fallback
  using the node's internal parameter map is flagged with inline TODOs.

state_machine_core/scripts/:
- main_state_machine.py: rclpy.init + create_node + spin loop wrapper.
  rospy.names.get_namespace() -> node.get_namespace() with trailing-
  slash normalization. QuadTracker now constructed with (node, ...)
  first arg. smach ecosystem is ROS1-only; top-of-file TODO note
  preserved so the smach state graph itself still works when a ROS2
  smach port (smach_ros2 / yasmin) becomes available.
- QuadTracker.py: constructor signature changed to (self, node,
  abort_topic). 5 rospy.Subscriber calls -> node.create_subscription.
  tf.TransformListener -> tf2_ros.Buffer + TransformListener.
  tf.transformations.euler_from_quaternion -> tf_transformations.
  euler_from_quaternion.
- MainStates.py: _node(quad_monitor) helper pulls the rclpy node off
  the QuadTracker. Deferred publisher/client creation in
  PublishBoolMsgState and ArmDisarmMavros because they inherit from
  smach.State (no __init__ Node access) — main_state_machine.py wires
  `state.quad_monitor = quad_tracker` after constructing them.
  rospy.ServiceProxy -> create_client + async call pattern.
  WaitState.__init__ param renamed `time` -> `time_sec` to avoid
  shadowing the imported time module.
- Replanner.py: rospy.sleep -> time.sleep. smach_ros.SimpleActionState
  wrapper preserved as-is with a TODO note; the wrapper has no ROS2
  equivalent and the class will need to be reimplemented when
  smach_ros2 lands.
- SwitchState.py: rospy.Subscriber -> quad_monitor.node.create_subscription.
  roslib.load_manifest dropped (ROS1-only).
- Utils.py: tf.Transformer (ROS1-only) -> direct 4x4 matrix math via
  tf_transformations.concatenate_matrices + inverse_matrix. Pure
  helper module, no node reference.

Known limitations (documented inline in each file):

- `smach` itself is ROS1-only. Every SMACH state file has a top-of-file
  TODO note saying the state machine graph must be re-implemented once
  smach_ros2 or yasmin is available. The rospy -> rclpy translation
  below the TODO is best-effort and correct in isolation.
- Absolute parameter paths like /mapper/global/origin_x and
  /local_plan_server0/mav_name cannot be read directly in ROS2
  (parameters are per-node). The slash-named declare_parameter calls
  are preserved as best-effort mechanical translations; at runtime
  these should use rclpy.parameter_client to read from the target
  node, or the value should be passed in at launch.
- smach_ros.SimpleActionState and smach_ros.ServiceState wrappers are
  used in Replanner.py and MainStates.py. These don't exist in ROS2.
  Deferred to the smach_ros2 / yasmin follow-up.

None of these scripts are installed by any CMakeLists.txt, so this
commit does not affect colcon build. It does bring the tree into a
state where grep -rn 'import rospy' returns zero hits.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

Author: XuRobotics

autonomy_core/map_plan/action_planner/scripts/evaluate_traj.py

@@ -1,13 +1,16 @@
 #!/usr/bin/env python3
-import rospy
+import time
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
 # from kr_planning_msgs.msg import SplineTrajectory
-from kr_planning_msgs.msg import PlanTwoPointAction, PlanTwoPointGoal
+from kr_planning_msgs.action import PlanTwoPoint
+from rclpy.duration import Duration
 import numpy as np
 # import matplotlib.pyplot as plt
 import pandas as pd
 from copy import deepcopy
 from visualization_msgs.msg import MarkerArray, Marker
-from actionlib import SimpleActionClient
 from std_srvs.srv import Empty
 
 # filename = '/home/laura/autonomy_ws/src/kr_autonomous_flight/autonomy_core/map_plan/action_planner/scripts/map_balls_start_goal.csv'
@@ -42,16 +45,17 @@ def evaluate(msg, t, deriv_num):
 
 
 class Evaluater:
-    def __init__(self):
+    def __init__(self, node):
+        self.node = node
         # print("reading "+filename)
         # self.start_goals = pd.read_csv(filename)
-        # self.path_pub = rospy.Publisher('/local_plan_server/plan_local_trajectory/goal', PlanTwoPointActionGoal, queue_size=10, latch=True)
-        self.client = SimpleActionClient('/local_plan_server/plan_local_trajectory', PlanTwoPointAction)
-        self.client2 = SimpleActionClient('/local_plan_server2/plan_local_trajectory', PlanTwoPointAction)
-        self.client3 = SimpleActionClient('/local_plan_server3/plan_local_trajectory', PlanTwoPointAction)
+        # self.path_pub = node.create_publisher(PlanTwoPointActionGoal, '/local_plan_server/plan_local_trajectory/goal', 10)
+        self.client = ActionClient(node, PlanTwoPoint, '/local_plan_server/plan_local_trajectory')
+        self.client2 = ActionClient(node, PlanTwoPoint, '/local_plan_server2/plan_local_trajectory')
+        self.client3 = ActionClient(node, PlanTwoPoint, '/local_plan_server3/plan_local_trajectory')
 
-        self.start_and_goal_pub = rospy.Publisher('/start_and_goal', MarkerArray, queue_size=10, latch=True)
-        # rospy.Subscriber("/local_plan_server/trajectory", SplineTrajectory, self.callback)
+        self.start_and_goal_pub = node.create_publisher(MarkerArray, '/start_and_goal', 10)
+        # node.create_subscription(SplineTrajectory, "/local_plan_server/trajectory", self.callback, 10)
         self.num_trials = 20
         self.success = np.zeros(self.num_trials, dtype=bool)
         self.traj_time = np.zeros(self.num_trials)
@@ -75,28 +79,29 @@ def computeJerk(self, traj):
             t_vec = np.append(t_vec, t)
             jerk_sq = np.append(jerk_sq, (np.linalg.norm(evaluate(traj, t, 3)))**2 )
         return np.sqrt(np.trapz(jerk_sq, t_vec))/traj.data[0].t_total
-        
+
 
     def computeCost(self, traj, rho):
-        time = traj.data[0].t_total
-        cost = rho*time + self.computeJerk(traj)
+        time_total = traj.data[0].t_total
+        cost = rho*time_total + self.computeJerk(traj)
         return cost
 
     def publisher(self):
         print("waiting for map server")
-        rospy.wait_for_service('/image_to_map')
-        map_client = rospy.ServiceProxy('/image_to_map', Empty)
-        map_client()
+        map_client = self.node.create_client(Empty, '/image_to_map')
+        while not map_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info("Waiting for /image_to_map service...")
+        map_client.call_async(Empty.Request())
 
         print("waiting for action server")
         self.client.wait_for_server()
 
         # for i in range(self.start_goals.shape[0]):
-        #     if rospy.is_shutdown():
+        #     if not rclpy.ok():
         #         break
 
         #     print(i)
-        #     msg = PlanTwoPointGoal()
+        #     msg = PlanTwoPoint.Goal()
         #     msg.p_init.position.x = self.start_goals['xi'][i]
         #     msg.p_init.position.y = self.start_goals['yi'][i]
         #     msg.p_init.position.z = 5
@@ -108,21 +113,21 @@ def publisher(self):
 
         for i in range(self.num_trials):
             print(i)
-            if rospy.is_shutdown():
+            if not rclpy.ok():
                 break
             if (i > 0):
-                map_client()
+                map_client.call_async(Empty.Request())
                 #TODO(Laura): actually send map ?
-                rospy.sleep(2)
-            msg = PlanTwoPointGoal()
+                time.sleep(2)
+            msg = PlanTwoPoint.Goal()
             msg.p_init.position.x = 1.25
             msg.p_init.position.y = 1.25
-            msg.p_init.position.z = 5
+            msg.p_init.position.z = 5.0
             # msg.v_init.linear.x = 2
             # msg.v_init.linear.y = 2
             msg.p_final.position.x = 19.5
             msg.p_final.position.y = 10 - 1.25
-            msg.p_final.position.z = 5
+            msg.p_final.position.z = 5.0
 
 
 
@@ -131,31 +136,36 @@ def publisher(self):
             start_and_goal = MarkerArray()
             start = Marker()
             start.header.frame_id = "map"
-            start.header.stamp = rospy.Time.now()
+            start.header.stamp = self.node.get_clock().now().to_msg()
             start.pose.position = msg.p_init.position
-            start.pose.orientation.w = 1
-            start.color.g = 1
-            start.color.a = 1
+            start.pose.orientation.w = 1.0
+            start.color.g = 1.0
+            start.color.a = 1.0
             start.type = 2
-            start.scale.x = start.scale.y = start.scale.z = 1
+            start.scale.x = start.scale.y = start.scale.z = 1.0
             goal = deepcopy(start)
             goal.pose.position = msg.p_final.position
             goal.id = 1
-            goal.color.r = 1
-            goal.color.g = 0
+            goal.color.r = 1.0
+            goal.color.g = 0.0
             start_and_goal.markers.append(start)
             start_and_goal.markers.append(goal)
             # self.path_pub.publish(msg)
             self.start_and_goal_pub.publish(start_and_goal)
-            self.client.send_goal(msg)
+
+            # Action client goal send + wait (was self.client.send_goal(msg) + wait_for_result)
+            send_goal_future = self.client.send_goal_async(msg)
+            rclpy.spin_until_future_complete(self.node, send_goal_future)
+            goal_handle = send_goal_future.result()
+            result = None
+            if goal_handle is not None and goal_handle.accepted:
+                get_result_future = goal_handle.get_result_async()
+                rclpy.spin_until_future_complete(self.node, get_result_future, timeout_sec=3.0)
+                if get_result_future.done():
+                    result = get_result_future.result().result
             # self.client2.send_goal(msg)
             # self.client3.send_goal(msg)
-            # Waits for the server to finish performing the action.
-            self.client.wait_for_result(rospy.Duration.from_sec(3.0))
-            # self.client2.wait_for_result(rospy.Duration.from_sec(5.0))
-            # self.client3.wait_for_result(rospy.Duration.from_sec(5.0))
 
-            result = self.client.get_result()
             #TODO(Laura) check if the path is collision free and feasible
             if result:
                 self.success[i] = result.success
@@ -190,15 +200,18 @@ def publisher(self):
 
 
 def subscriber():
-    rospy.init_node('evaluate_traj')
-    Evaluater()
+    rclpy.init()
+    node = rclpy.create_node('evaluate_traj')
+    Evaluater(node)
 
     # spin() simply keeps python from exiting until this node is stopped
-    # rospy.spin()
+    # rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
 
 
 if __name__ == '__main__':
     try:
         subscriber()
-    except rospy.ROSInterruptException:
+    except KeyboardInterrupt:
         pass