diff --git a/.devcontainer/Dockerfile.devcontainer b/.devcontainer/Dockerfile.devcontainer index cd3c517..40d10ea 100644 --- a/.devcontainer/Dockerfile.devcontainer +++ b/.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"] \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index e32eaa5..2af3d13 100644 --- a/.devcontainer/devcontainer.json +++ b/.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", diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/autonomy_system/autonomy_system/behavior_tree_node.py b/src/autonomy_system/autonomy_system/behavior_tree_node.py index 811cf58..08daeeb 100644 --- a/src/autonomy_system/autonomy_system/behavior_tree_node.py +++ b/src/autonomy_system/autonomy_system/behavior_tree_node.py @@ -1,4 +1,5 @@ import time +from typing import Optional import py_trees import py_trees_ros @@ -6,7 +7,8 @@ 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 diff --git a/src/autonomy_system/autonomy_system/behaviours/StabilizeOnPosition.py b/src/autonomy_system/autonomy_system/behaviours/StabilizeOnPosition.py new file mode 100644 index 0000000..2ff2cd3 --- /dev/null +++ b/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() diff --git a/src/autonomy_system/autonomy_system/helpers/ControlOperations.py b/src/autonomy_system/autonomy_system/helpers/ControlOperations.py new file mode 100644 index 0000000..61d8807 --- /dev/null +++ b/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) diff --git a/src/autonomy_system/config/params.yaml b/src/autonomy_system/config/params.yaml new file mode 100644 index 0000000..cbb3d22 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/autonomy_system/launch/task1.launch.py b/src/autonomy_system/launch/task1.launch.py new file mode 100644 index 0000000..ff59b8b --- /dev/null +++ b/src/autonomy_system/launch/task1.launch.py @@ -0,0 +1,164 @@ +from launch_ros.actions import Node +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from launch.actions import RegisterEventHandler, Shutdown +from launch.event_handlers import OnProcessExit +from ament_index_python import get_package_share_directory + + +def shutdown_on_exit(node, reason): + return RegisterEventHandler( + OnProcessExit( + target_action=node, + on_exit=[Shutdown(reason=reason)] + ) + ) + +def generate_launch_description(): + mfsmc_share = FindPackageShare('rov_mfsmc') + autonomy_system_share = FindPackageShare('autonomy_system') + passthrough_share = FindPackageShare('rov_passthrough_control') + + default_config_path = PathJoinSubstitution([mfsmc_share, 'config', 'params.yaml']) + default_autonomy_config_path = PathJoinSubstitution([autonomy_system_share, 'config', 'params.yaml']) + default_rviz_config_path = PathJoinSubstitution([mfsmc_share, 'rviz', 'tf_basic.rviz']) + + # Define default configuration paths + default_config_path = PathJoinSubstitution([mfsmc_share, 'config', 'params.yaml']) + + # Launch arguments + config_arg = DeclareLaunchArgument( + name="config", + default_value=default_config_path, + description="Path to the configuration file for the stonefish simulator" + ) + autonomy_config_arg = DeclareLaunchArgument( + name="autonomy_config", + default_value=default_autonomy_config_path, + description="Path to the autonomy configuration file" + ) + rviz_config_arg = DeclareLaunchArgument( + name="rviz_config", + default_value=default_rviz_config_path, + description="Path to the RViz2 configuration file" + ) + + thruster_manager_node = Node( + package='rov_thruster_manager', + executable="thruster_manager", + parameters=[LaunchConfiguration('config')], + output="screen" + ) + + rov_state_publisher_node = Node( + package='rov_description', + executable='rov_state_publisher', + parameters=[LaunchConfiguration('config')] + ) + + passthrough_launch = IncludeLaunchDescription( + launch_description_source=PathJoinSubstitution([passthrough_share, 'launch', 'base.launch.py']), + launch_arguments={ + "config": LaunchConfiguration('config') + }.items() + ) + + # traj_gen_node = Node( + # package='traj_gen', + # executable='min_snap_traj_generator', + # parameters=[LaunchConfiguration('config')], + # output='screen' + # ) + + tf_traj_gen = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=["0", "0", "0", "0", "0", "0", "world_ned", "traj_gen"] + ) + + robot_localization_node = Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + parameters=[LaunchConfiguration('config')], + output='screen' + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', LaunchConfiguration('rviz_config')], + output='screen' + ) + + mfsm_node = Node( + package='rov_mfsmc', + executable='rov_mfsmc_better_node', + parameters=[LaunchConfiguration('config')], + output='screen' + ) + + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(get_package_share_directory('stonefish_ros2') + \ + '/launch/stonefish_simulator.launch.py'), + launch_arguments={ + 'simulation_data': get_package_share_directory('rov_stonefish') + '/data/', + 'scenario_desc': get_package_share_directory('rov_stonefish') + '/scenarios/windturbine_bluerov2.scn', + 'simulation_rate': '30.0', + 'window_res_x': '1820', + 'window_res_y': '980', + 'rendering_quality': 'low', + }.items() + ) + + stabilize_on_position_action = Node( + package='autonomy_system', + executable='stabilize_on_position_action', + name='stabilize_on_position_action_node', + parameters=[LaunchConfiguration('autonomy_config')], + output='screen', + emulate_tty=True + ) + + stabilize_on_position_2_action = Node( + package='autonomy_system', + executable='stabilize_on_position_action', + name='stabilize_on_position_action_2_node', + parameters=[LaunchConfiguration('autonomy_config')], + output='screen', + emulate_tty=True + ) + + behavior_tree = Node( + package='autonomy_system', + executable='behavior_tree', + name='behavior_tree', + parameters=[LaunchConfiguration('autonomy_config')], + output='screen', + emulate_tty=True + ) + + description_timer = TimerAction(period=1.0, actions=[rov_state_publisher_node]) + rviz_timer = TimerAction(period=1.0, actions=[rviz_node]) + stonefish_timer = TimerAction(period=2.0, actions=[launch_include]) + traj_gen_timer = TimerAction(period=3.0, actions=[tf_traj_gen]) + tasks_timer = TimerAction(period=3.0, actions=[stabilize_on_position_action,stabilize_on_position_2_action, behavior_tree]) + + return LaunchDescription([ + config_arg, + rviz_config_arg, + autonomy_config_arg, + robot_localization_node, + thruster_manager_node, + passthrough_launch, + mfsm_node, + description_timer, + rviz_timer, + stonefish_timer, + traj_gen_timer, + tasks_timer + ]) diff --git a/src/autonomy_system/setup.py b/src/autonomy_system/setup.py index 5d68156..00cb4f9 100644 --- a/src/autonomy_system/setup.py +++ b/src/autonomy_system/setup.py @@ -14,6 +14,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.py*'))), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml*'))), ], install_requires=['setuptools'], zip_safe=True, @@ -31,6 +32,8 @@ 'console_scripts': [ 'behavior_tree = autonomy_system.behavior_tree_node:main', 'hello_world_action = autonomy_system.behaviours.HelloWorld:main', + 'stabilize_on_position_action = autonomy_system.behaviours.StabilizeOnPosition:main', + 'dive_task_action = autonomy_system.behaviours.DiveTask:main', 'move_to_gate_action = autonomy_system.behaviours.MoveToGate:main', 'search_gate_action = autonomy_system.behaviours.SearchGate:main', ], diff --git a/src/autonomy_system_interfaces/CMakeLists.txt b/src/autonomy_system_interfaces/CMakeLists.txt index fe1f039..79c6456 100644 --- a/src/autonomy_system_interfaces/CMakeLists.txt +++ b/src/autonomy_system_interfaces/CMakeLists.txt @@ -25,5 +25,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/SearchGate.action" "action/MoveToGate.action" "action/HelloWorld.action" + "action/NormalAction.action" ) ament_package() diff --git a/src/autonomy_system_interfaces/action/NormalAction.action b/src/autonomy_system_interfaces/action/NormalAction.action new file mode 100644 index 0000000..1c8f71a --- /dev/null +++ b/src/autonomy_system_interfaces/action/NormalAction.action @@ -0,0 +1,7 @@ +# Goal +--- +# Result +bool success +--- +# Feedback +string status diff --git a/src/control_system/rov_mfsmc/nodes/n_mfsmc_better_node.py b/src/control_system/rov_mfsmc/nodes/n_mfsmc_better_node.py new file mode 100644 index 0000000..8e33792 --- /dev/null +++ b/src/control_system/rov_mfsmc/nodes/n_mfsmc_better_node.py @@ -0,0 +1,16 @@ +import rclpy +from rov_mfsmc.ModelFreeSlidingControl_Better import ModelFreeSlidingControl + + +def main(): + rclpy.init() + + node = ModelFreeSlidingControl() + + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl_Better.py b/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl_Better.py new file mode 100644 index 0000000..8bdd422 --- /dev/null +++ b/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl_Better.py @@ -0,0 +1,359 @@ +from rclpy.node import Node +from rclpy.time import Time +from nav_msgs.msg import Odometry +from std_srvs.srv import Trigger +from geometry_msgs.msg import Pose, Twist, TwistStamped, WrenchStamped, TransformStamped, Transform +import numpy as np +import ros2_numpy as rnp +from tf2_ros import Buffer, TransformListener, StaticTransformBroadcaster +import tf_transformations + +np.printoptions(precision=4, suppress=True) + + +class ModelFreeSlidingControl(Node): + _is_running: bool = False + + def __init__(self): + Node.__init__(self=self, node_name='rov_mfsmc_node') + + self.declare_parameter('desired_position_topic_name', '/target_pose') + self.declare_parameter('desired_twist_topic_name', '/target_twist') + self.declare_parameter('odom_topic_name', '/odometry/filtered') + + self.declare_parameter('A', [2.3, 2.3, 5.0, 0.1, 0.1, 0.1]) + self.declare_parameter('kd', [4.5, 4.5, 2.5, 0.1, 0.1, 0.1]) + self.declare_parameter('ki', [0.1, 0.2, 0.3, 0.0, 0.0, 0.0]) + self.declare_parameter('alpha', 0.15) + self.declare_parameter('phi', [0.3, 0.3, 0.15, 0.4, 0.4, 0.4]) + self.declare_parameter('k_min', [0.6, 0.6, 0.6, 0.6, 0.6, 0.6]) + self.declare_parameter('k_max', [1.0, 1.0, 1.0, 1.0, 0.4, 1.0]) + self.declare_parameter('e_big', [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]) + + self.declare_parameter('odom_frame', 'base_link') + self.declare_parameter('target_frame', 'traj_gen_node') + self.declare_parameter('reference_frame', 'world_ned') + self.declare_parameter('hz', 120) + + self.wrench_pub = self.create_publisher(WrenchStamped, 'calculated_wrench', 0) + self.sync_trajectory_srv = self.create_service(Trigger, 'sync_trajectory', self.cb_sync_trajectory) + + self.tf_buffer = Buffer() + self.tf_broadcaster = StaticTransformBroadcaster(self) + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.odom_clock = None + self.target_clock = None + self.main_clock = None + + self.desired_twist_sub = None + self.odom_sub = None + + self.__des_pos = np.zeros(6) + self.__pos = np.zeros(6) + self.__des_twist = np.zeros(6) + self.__twist = np.zeros(6) + + self.u_sign = np.zeros(6) + + self._frames_synced = False + + self.desired_twist_sub = self.create_subscription(TwistStamped, + self.desired_twist_topic_name, + self.cb_desired_twist, 0) + self.odom_sub = self.create_subscription(Odometry, + self.odom_topic_name, + self.cb_odom, qos_profile=0) + + self.odom_clock = self.create_timer(1 / self.hz, self.cb_odom_frame) + self.target_clock = self.create_timer(1 / self.hz, self.cb_target_frame) + self.main_clock = self.create_timer(1 / self.hz, self.cb_main_clock) + + self.sync_frames() + + self.sync_clock = self.create_timer(1 / self.hz, self.cb_sync_clock) + + self.des_pos_f = np.zeros(6) + self.u_sign = np.zeros(6) + + def sat(self, x): + return np.clip(x, -1.0, 1.0) + + @property + def is_running(self) -> bool: + return self._is_running + + 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.get_logger().error(f"{e}") + return + + transform = rnp.numpify(t.transform) + self.pos = transform + + def cb_target_frame(self) -> None: + now = Time() + try: + t = self.tf_buffer.lookup_transform(self.reference_frame, self.target_frame, now) + except Exception as e: + self.get_logger().error(f"{e}") + return + + transform = rnp.numpify(t.transform) + self.des_pos = transform + tf_transformations + + def cb_desired_twist(self, msg: TwistStamped): + self.des_twist = rnp.numpify(msg.twist) + + def cb_odom(self, msg: Odometry): + self.twist = rnp.numpify(msg.twist.twist) + + def pose_to_vec6(self, pose): + pos = rnp.numpify(pose.position) + quat = rnp.numpify(pose.orientation) + quat = quat[..., (1, 2, 3, 0)] # xyzw → wxyz + rpy = tf_transformations.euler_from_quaternion(quat) + return np.concatenate((pos, rpy), axis=None) + + def T_to_vec6(self, T): + pos = T[:3, 3] + rpy = tf_transformations.euler_from_matrix(T[:3, :3]) + return np.concatenate((pos, rpy), axis=None) + + def cb_main_clock(self) -> None: + if not self._frames_synced: + return + + dt = 1.0 / self.hz + + # ====================================================== + # 1️⃣ REFERENCE GOVERNOR (softens step input) + # ====================================================== + des_vec = self.pose_to_vec6(rnp.msgify(Pose, self.des_pos)) + self.des_pos_f += self.alpha * (des_vec - self.des_pos_f) * dt + + # ====================================================== + # 2️⃣ Pose → [x y z roll pitch yaw] + # ====================================================== + def pose_to_vec(pose): + pos = rnp.numpify(pose.position) + quat = rnp.numpify(pose.orientation) + quat = quat[..., (1, 2, 3, 0)] + rot = tf_transformations.euler_from_quaternion(quat) + return np.concatenate((pos, rot), axis=None) + + u_ref = self.des_pos_f + u_act = self.pose_to_vec6(rnp.msgify(Pose, self.pos)) + + # ====================================================== + # 3️⃣ Error + velocity error + # ====================================================== + e = u_act - u_ref + e_dot = self.twist - self.des_twist + + # ====================================================== + # 4️⃣ Sliding surface (MFSMC) + # ====================================================== + # sliding surface + s = self.A * e + e_dot + + # adaptive gain (6D!) + k = self.k_min + (self.k_max - self.k_min) * np.minimum( + 1.0, np.abs(e) / self.e_big + ) + + # equivalent control + u_eq = -k * self.sat(s / self.phi) + + # integral sliding + self.u_sign += self.sat(s) * dt + self.u_sign = np.clip(self.u_sign, -1.0, 1.0) + + u_i = self.ki * self.u_sign + + # FINAL CONTROL (6D VECTOR) + u = self.kd * (u_eq + u_i) + + # ====================================================== + # 9️⃣ Publish wrench (ROV convention) + # ====================================================== + wrench_msg = WrenchStamped() + wrench_msg.header.stamp = self.get_clock().now().to_msg() + wrench_msg.header.frame_id = self.odom_frame + + wrench_msg.wrench.force.x = u[0] + wrench_msg.wrench.force.y = -u[1] + wrench_msg.wrench.force.z = u[2] + + wrench_msg.wrench.torque.x = -u[5] + wrench_msg.wrench.torque.y = -u[4] + wrench_msg.wrench.torque.z = -u[3] + + self.wrench_pub.publish(wrench_msg) + + def cb_sync_trajectory(self, request: Trigger.Request, response: Trigger.Response): + ok, msg = self.sync_frames() + response.success = bool(ok) + response.message = msg + return response + + def cb_sync_clock(self) -> None: + if self.sync_frames()[0]: + self._frames_synced = True + self.sync_clock.cancel() + + def sync_frames(self) -> tuple[bool, str]: + now = self.get_clock().now() + target_parent_frame = 'traj_gen' + try: + t = self.tf_buffer.lookup_transform(target_parent_frame, self.target_frame, Time()) + t_base = self.tf_buffer.lookup_transform(self.reference_frame, self.odom_frame, Time()) + + T = rnp.numpify(t.transform) + T_base = rnp.numpify(t_base.transform) + T = T_base @ np.linalg.inv(T) + t.transform = rnp.msgify(Transform, T) + + except Exception as e: + msg = f'Could not sync frames {self.odom_frame}, {self.target_frame}\n{e}' + return False, msg + + tn = TransformStamped() + tn.header.frame_id = self.reference_frame + tn.header.stamp = now.to_msg() + tn.child_frame_id = 'traj_gen' + tn.transform = t.transform + + self.tf_broadcaster.sendTransform(tn) + return True, 'Frames synced' + + @property + def desired_position_topic_name(self): + return self.get_parameter('desired_position_topic_name').value + + @property + def desired_twist_topic_name(self): + return self.get_parameter('desired_twist_topic_name').value + + @property + def odom_topic_name(self): + return self.get_parameter('odom_topic_name').value + + @property + def odom_frame(self) -> str: + return self.get_parameter('odom_frame').value + + @property + def target_frame(self) -> str: + return self.get_parameter('target_frame').value + + @property + def reference_frame(self) -> str: + return self.get_parameter('reference_frame').value + + @property + def hz(self) -> int: + return self.get_parameter('hz').value + + @property + def des_pos(self) -> np.ndarray: + return self.__des_pos + + @des_pos.setter + def des_pos(self, value: np.ndarray) -> None: + self.__des_pos = value + + @property + def des_twist(self) -> np.ndarray: + return self.__des_twist + + @des_twist.setter + def des_twist(self, value: np.ndarray) -> None: + self.__des_twist = value + + @property + def pos(self) -> np.ndarray: + return self.__pos + + @pos.setter + def pos(self, value: np.ndarray) -> None: + self.__pos = value + + @property + def twist(self) -> np.ndarray: + return self.__twist + + @twist.setter + def twist(self, value: np.ndarray) -> None: + self.__twist = value + + @property + def A(self) -> np.ndarray: + A_ = self.get_parameter('A').value + return A_ + + # @A.setter + # def A(self, value: float|np.ndarray) -> None: + # self._A = value * np.eye(6) + + @property + def kd(self) -> np.matrix: + kd_ = self.get_parameter('kd').value + return kd_ + + # @kd.setter + # def kd(self, value: float|np.ndarray) -> None: + # self._kd = value * np.eye(6) + + @property + def ki(self) -> np.ndarray: + ki_ = self.get_parameter('ki').value + return ki_ + + @property + def alpha(self) -> np.float64: + return self.get_parameter('alpha').value + + @property + def phi(self) -> np.ndarray: + return np.array(self.get_parameter('phi').value) + + @property + def k_min(self) -> np.ndarray: + return np.array(self.get_parameter('k_min').value) + + @property + def k_max(self) -> np.ndarray: + return np.array(self.get_parameter('k_max').value) + + @property + def e_big(self) -> np.ndarray: + return np.array(self.get_parameter('e_big').value) + + # @ki.setter + # def ki(self, value: float|np.ndarray) -> None: + # self._ki = value * np.eye(6) + + @rnp.registry.converts_to_numpy(Twist) + def convert(my_msg: Twist) -> np.ndarray: + v = np.array([my_msg.linear.x, my_msg.linear.y, my_msg.linear.z], dtype=np.float64) + w = np.array([my_msg.angular.z, my_msg.angular.y, my_msg.angular.x], dtype=np.float64) + + t = np.concatenate((v, w), axis=None) + return t + + @rnp.registry.converts_from_numpy(Twist) + def convert_back(arr: np.ndarray) -> Twist: + msg = Twist() + msg.linear.x = arr[0] + msg.linear.y = arr[1] + msg.linear.z = arr[2] + msg.angular.x = arr[3] + msg.angular.y = arr[4] + msg.angular.z = arr[5] + + return msg diff --git a/src/control_system/rov_mfsmc/setup.py b/src/control_system/rov_mfsmc/setup.py index 0c76eff..99de9ed 100644 --- a/src/control_system/rov_mfsmc/setup.py +++ b/src/control_system/rov_mfsmc/setup.py @@ -28,7 +28,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'rov_mfsmc_node = nodes.n_mfsmc_node:main' + 'rov_mfsmc_node = nodes.n_mfsmc_node:main', + 'rov_mfsmc_better_node = nodes.n_mfsmc_better_node:main' ], }, ) diff --git a/src/simulation/rov_stonefish/scenarios/robosub.scn b/src/simulation/rov_stonefish/scenarios/robosub.scn new file mode 100644 index 0000000..24ada01 --- /dev/null +++ b/src/simulation/rov_stonefish/scenarios/robosub.scn @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +