stabilize on position task

.devcontainer/Dockerfile.devcontainer

@@ -131,7 +131,7 @@ RUN echo 'source /opt/ros/'$ROS_DISTRO'/setup.bash' >> /home/$USERNAME/.bashrc \
 
 
 COPY ./entrypoint.sh /entrypoint.sh
-
+ENV PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/home/dev/ros2_ws/install
 ENTRYPOINT ["/entrypoint.sh"]
 
 CMD ["bash"]

.devcontainer/devcontainer.json

@@ -38,7 +38,7 @@
     ],
     "runArgs": [
         "--network", "host",
-        "--device-cgroup-rule", "c *:* rmw",
+        "--device-cgroup-rule: 'c *:* rmw'",
         "--cap-add=SYS_PTRACE",
         "--security-opt=seccomp:unconfined",
 		"--security-opt=apparmor:unconfined",

src/autonomy_system/autonomy_system/behavior_tree_node.py

@@ -1,12 +1,14 @@
 import time
+from typing import Optional
 
 import py_trees
 import py_trees_ros
 import rclpy
 from autonomy_system_interfaces.action import (
     SearchGate,
     MoveToGate,
-    HelloWorld
+    HelloWorld,
+    NormalAction
 )
 from rclpy.action import ActionClient
 from rclpy.node import Node
@@ -20,7 +22,7 @@ def __init__(
             node: Node,
             action_type,
             action_name: str,
-            timeout_sec: float = 30.0,
+            timeout_sec: Optional[float] = 30.0,
     ):
         super().__init__(name)
         self.node = node
@@ -60,7 +62,7 @@ def update(self):
                           self.node.get_clock().now() - self._start_time
                   ).nanoseconds / 1e9
 
-        if elapsed > self.timeout_sec:
+        if self.timeout_sec is not None and elapsed > self.timeout_sec:
             self.node.get_logger().error(
                 f"{self.name}: TIMEOUT"
             )
@@ -117,23 +119,17 @@ def create_tree(self):
 
         root.add_children([
             ActionBehaviour(
-                name="Search Gate",
+                name="Stabilize on position",
                 node=self,
-                action_type=SearchGate,
-                action_name="search_gate"
+                action_type=NormalAction,
+                action_name="stabilize_on_position"
             ),
             ActionBehaviour(
-                name="Move To Gate",
+                name="Stabilize on position 2",
                 node=self,
-                action_type=MoveToGate,
-                action_name="move_to_gate"
-            ),
-            ActionBehaviour(
-                name="Hello World",
-                node=self,
-                action_type=HelloWorld,
-                action_name="hello_world"
-            ),
+                action_type=NormalAction,
+                action_name="stabilize_2"
+            )
         ])
 
         return root

src/autonomy_system/autonomy_system/behaviours/StabilizeOnPosition.py

@@ -0,0 +1,119 @@
+import numpy as np
+import rclpy
+from joblib.testing import param
+from scipy.spatial.transform import Rotation
+from geometry_msgs.msg import (Vector3Stamped,
+                               PoseStamped,
+                               TwistStamped,
+                               AccelStamped,
+                               Point,
+                               TransformStamped)
+import py_trees
+import py_trees_ros
+from rclpy.node import Node
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from autonomy_system.helpers.ControlOperations import ControlOperations
+from autonomy_system_interfaces.action import NormalAction
+from tf2_ros import TransformBroadcaster
+import time
+from rclpy.executors import MultiThreadedExecutor
+
+class StabilizeOnPositionServer(Node):
+
+    def __init__(self):
+        super().__init__('stabilize_on_position_server')
+        self.declare_parameter('action_name', "stabilize_on_position")
+        self.declare_parameter('position_stabilization', [0.0, 0.0, 0.0])
+        self.declare_parameter('rotation_stabilization', [0.0, 0.0, 0.0])
+        self.declare_parameter('near_time', 5.0)
+        self.declare_parameter('near_dist', 5.0)
+        self._server = ActionServer(
+            self,
+            NormalAction,
+            self.action_name,
+            execute_callback=self.execute_callback,
+            goal_callback=self.goal_callback,
+            cancel_callback=self.cancel_callback,
+        )
+
+        self.control_operations = ControlOperations(self)
+
+    @property
+    def action_name(self):
+        return self.get_parameter("action_name").value
+
+    @property
+    def position_stabilization(self):
+        return np.array(self.get_parameter("position_stabilization").value)
+
+    @property
+    def rotation_stabilization(self):
+        return np.array(self.get_parameter("rotation_stabilization").value)
+
+    @property
+    def near_time(self):
+        return self.get_parameter("near_time").value
+
+    @property
+    def near_dist(self):
+        return self.get_parameter("near_dist").value
+
+    def execute_callback(self, goal_handle):
+        self.get_logger().info("[Stabilize on Position] Started")
+
+        feedback = NormalAction.Feedback()
+        is_near = False
+        near_start_time = time.time()
+        while not (is_near and time.time() - near_start_time > self.near_time):
+            position = self.position_stabilization
+            att_rpy = self.rotation_stabilization
+            print(self.control_operations.position(), position)
+            print(np.linalg.norm(self.control_operations.position() - position), self.near_dist,is_near,time.time() - near_start_time)
+            if np.linalg.norm(self.control_operations.position() - position) < self.near_dist:
+                if not is_near:
+                    is_near = True
+                    near_start_time = time.time()
+            else:
+                is_near = False
+            self.control_operations.pub_pose_cmd(position, att_rpy)
+            self.control_operations.pub_transform_broadcaster(position, att_rpy)
+            if goal_handle.is_cancel_requested:
+                self.get_logger().warn("[Stabilize on Position] Cancelled")
+                goal_handle.canceled()
+                result = NormalAction.Result()
+                result.success = False
+                return result
+
+            feedback.status = f"Stabilize..."
+            goal_handle.publish_feedback(feedback)
+            rclpy.spin_once(self, timeout_sec=0.0)
+            time.sleep(0.1)
+
+        goal_handle.succeed()
+        result = NormalAction.Result()
+        result.success = True
+
+        self.get_logger().info("[Stabilize on Position] Finished SUCCESS")
+        return result
+
+    def goal_callback(self, goal_request):
+        self.get_logger().info("Received goal request")
+        return GoalResponse.ACCEPT
+
+    def cancel_callback(self, goal_handle):
+        self.get_logger().warn("Received cancel request")
+        return CancelResponse.ACCEPT
+
+
+def main():
+    rclpy.init()
+
+    node = StabilizeOnPositionServer()
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/autonomy_system/autonomy_system/helpers/ControlOperations.py

@@ -0,0 +1,146 @@
+import numpy as np
+import py_trees
+import py_trees_ros
+import rclpy
+from autonomy_system_interfaces.action import NormalAction
+from geometry_msgs.msg import (Vector3Stamped,
+                               PoseStamped,
+                               TwistStamped,
+                               AccelStamped,
+                               Point,
+                               TransformStamped)
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.node import Node
+from scipy.spatial.transform import Rotation
+from tf2_ros import TransformBroadcaster, TransformListener, StaticTransformBroadcaster
+import ros2_numpy as rnp
+from rclpy.time import Time
+from tf_transformations import euler_from_matrix
+from tf2_ros import Buffer
+
+
+class ControlOperations:
+    def __init__(self, node):
+        self.node = node
+        self.pose_sp_pub = node.create_publisher(PoseStamped, '/target_pose', 10)
+        self.transform_broadcaster = TransformBroadcaster(node)
+        self.frame_id_param = node.declare_parameter('frame_id', 'traj_gen')
+        self.child_frame_id_param = node.declare_parameter('child_frame_id', 'traj_gen_node')
+        node.declare_parameter('odom_frame', 'base_link')
+        node.declare_parameter('reference_frame', 'world_ned')
+        node.declare_parameter('hz', 120)
+
+        self.tf_buffer = Buffer()
+        self.tf_broadcaster = StaticTransformBroadcaster(node)
+        self.tf_listener = TransformListener(self.tf_buffer, node)
+        self.odom_clock = self.node.create_timer(1 / self.hz, self.cb_odom_frame)
+
+        self.pos = np.array([1000.0, 1000.0, 1000.0])
+        self.rot = np.array([0.0, 0.0, 0.0])
+
+    def position(self):
+        return self.pos
+
+    def rotation(self):
+        return self.rot
+
+    def cb_odom_frame(self) -> None:
+        now = Time()
+        try:
+            t = self.tf_buffer.lookup_transform(self.reference_frame,self.odom_frame,  now)
+        except Exception as e:
+            self.node.get_logger().error(f"{e}")
+            return
+        transform = rnp.numpify(t.transform)
+        pos_from_tf = transform[:3, 3]
+        rotation_matrix = transform[:3, :3]  # 3x3
+        roll, pitch, yaw = euler_from_matrix(rotation_matrix)
+        self.pos = pos_from_tf
+        self.rot = np.array([roll, pitch, yaw])
+
+    @property
+    def child_frame_id(self) -> str:
+        return self.node.get_parameter('child_frame_id').value
+
+    @property
+    def frame_id(self) -> str:
+        return self.node.get_parameter('frame_id').value
+
+    @property
+    def odom_frame(self) -> str:
+        return self.node.get_parameter('odom_frame').value
+
+    @property
+    def reference_frame(self) -> str:
+        return self.node.get_parameter('reference_frame').value
+
+    @property
+    def hz(self) -> int:
+        return self.node.get_parameter('hz').value
+
+    def pub_pose_cmd(self, position: np.array, rpy: np.array) -> PoseStamped:
+        q_tmp = Rotation.from_euler(
+            'XYZ', [rpy[0], rpy[1], rpy[2]]).as_quat()
+        q = np.zeros(4)
+        q[0] = q_tmp[3]
+        q[1] = q_tmp[0]
+        q[2] = q_tmp[1]
+        q[3] = q_tmp[2]
+        msg = PoseStamped()
+        msg.header.frame_id = self.child_frame_id
+        msg.pose.position.x = position[0]
+        msg.pose.position.y = position[1]
+        msg.pose.position.z = position[2]
+        msg.pose.orientation.w = q[0]
+        msg.pose.orientation.x = q[1]
+        msg.pose.orientation.y = q[2]
+        msg.pose.orientation.z = q[3]
+        self.pose_sp_pub.publish(msg)
+        return msg
+
+    def pub_transform_broadcaster(self, position: np.ndarray, rpy: np.ndarray) -> None:
+        q_tmp = Rotation.from_euler(
+            'XYZ', [rpy[0], rpy[1], rpy[2]]
+        ).as_quat()
+        q = np.zeros(4)
+        q[0] = q_tmp[3]
+        q[1] = q_tmp[0]
+        q[2] = q_tmp[1]
+        q[3] = q_tmp[2]
+        msg = TransformStamped()
+        msg.header.stamp = self.node.get_clock().now().to_msg()
+        msg.header.frame_id = self.frame_id
+        msg.child_frame_id = self.child_frame_id
+        msg.transform.translation.x = position[0]
+        msg.transform.translation.y = position[1]
+        msg.transform.translation.z = position[2]
+        msg.transform.rotation.w = q[0]
+        msg.transform.rotation.x = q[1]
+        msg.transform.rotation.y = q[2]
+        msg.transform.rotation.z = q[3]
+
+        self.transform_broadcaster.sendTransform(msg)
+
+
+def pub_transform_broadcaster(self, position: np.ndarray, rpy: np.ndarray) -> None:
+    q_tmp = Rotation.from_euler(
+        'XYZ', [rpy[0], rpy[1], rpy[2]]
+    ).as_quat()
+    q = np.zeros(4)
+    q[0] = q_tmp[3]
+    q[1] = q_tmp[0]
+    q[2] = q_tmp[1]
+    q[3] = q_tmp[2]
+    msg = TransformStamped()
+    msg.header.stamp = self.get_clock().now().to_msg()
+    msg.header.frame_id = self.frame_id
+    msg.child_frame_id = self.child_frame_id
+    msg.transform.translation.x = position[0]
+    msg.transform.translation.y = position[1]
+    msg.transform.translation.z = position[2]
+    msg.transform.rotation.w = q[0]
+    msg.transform.rotation.x = q[1]
+    msg.transform.rotation.y = q[2]
+    msg.transform.rotation.z = q[3]
+
+    self.transform_broadcaster.sendTransform(msg)

src/autonomy_system/config/params.yaml

@@ -0,0 +1,15 @@
+stabilize_on_position_action_node:
+  ros__parameters:
+    position_stabilization: [0.0, 0.0, 0.0]
+    rotation_stabilization: [0.0, 0.0, 0.0]
+    near_time: 5.0
+    near_dist: 0.07
+
+
+stabilize_on_position_action_2_node:
+  ros__parameters:
+    action_name: "stabilize_2"
+    position_stabilization: [3.0, 2.0, -2.0]
+    rotation_stabilization: [0.0, 0.0, 0.0]
+    near_time: 255.0
+    near_dist: 0.07