16 lepszy generator trajektorii

.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",

.gitignore

@@ -51,3 +51,4 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+.idea

src/control_system/rov_mfsmc/config/params.yaml

@@ -318,5 +318,8 @@ rov_passthrough_control_node:
     inv_torque_y: False
     inv_torque_z: True
 
-
+trajectory_generator:
+  ros__parameters:
+    use_traj_from_file: True
+    traj_file: 'output_trajectory_11-02_12:10.json'
 

src/control_system/rov_mfsmc/launch/mfsmc.launch.py

@@ -55,7 +55,7 @@ def generate_launch_description():
     traj_gen_node = Node(
         package='traj_gen',
         executable='min_snap_traj_generator',
-        arguments=['-d', LaunchConfiguration('config')],
+        parameters=[LaunchConfiguration('config')],
         output='screen'
     )
 
@@ -103,6 +103,7 @@ def generate_launch_description():
     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=[traj_gen_node, tf_traj_gen])
 
     return LaunchDescription([
         config_arg,
@@ -111,8 +112,7 @@ def generate_launch_description():
         description_timer,
         rviz_timer,
         stonefish_timer,
-        tf_traj_gen,
-        traj_gen_node,
+        traj_gen_timer,
         thruster_manager_node,
         passthrough_launch,
         mfsm_node

src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl.py

@@ -22,9 +22,12 @@ def __init__(self):
         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, 2.3, 1.0, 1.0, 0.35])
-        self.declare_parameter('kd', [4.5, 4.5, 4.5, 0.5, 1.0, 0.08])
-        self.declare_parameter('ki', [0.1, 0.1, 0.1, 0.1, 0.1, 0.4])
+        self.declare_parameter('A', [2.3, 2.3, 5.0, 1.0, 1.0, 0.2])
+        self.declare_parameter('kd', [4.5, 4.5, 2.5, 1.0, 1.0, 0.01])
+        self.declare_parameter('ki', [0.1, 0.2, 0.3, 0.1, 0.1, 0.05])
+        # self.declare_parameter('A', [1.15, 1.15, 2.5, 0.5, 0.5, 0.1])
+        # self.declare_parameter('kd', [2.25, 2.25, 2.25, 0.5, 0.5, 0.005])
+        # self.declare_parameter('ki', [0.05, 0.15, 0.15, 0.05, 0.05, 0.025])
 
         self.declare_parameter('odom_frame', 'base_link')
         self.declare_parameter('target_frame', 'traj_gen_node')
@@ -82,6 +85,7 @@ def cb_odom_frame(self) -> None:
             return
 
         transform = rnp.numpify(t.transform)
+        pos_from_tf = transform[:3, 3]
         self.pos = transform
 
     def cb_target_frame(self) -> None:

src/control_system/rov_mfsmc/rviz/tf_basic.rviz

@@ -12,7 +12,7 @@ Panels:
         - /Path1
         - /Path1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 1033
+    Tree Height: 641
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -129,8 +129,8 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /target_twist
       Value: true
-    - Alpha: 1
-      Buffer Length: 1
+    - Alpha: 200
+      Buffer Length: 200
       Class: rviz_default_plugins/Path
       Color: 25; 255; 0
       Enabled: true
@@ -150,12 +150,12 @@ Visualization Manager:
       Shaft Diameter: 0.10000000149011612
       Shaft Length: 0.10000000149011612
       Topic:
-        Depth: 15
+        Depth: 200
         Durability Policy: Volatile
-        Filter size: 10
+        Filter size: 198
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /traj_gen/path
+        Value: /traj_gen/waypoints
       Value: true
   Enabled: true
   Global Options:
@@ -203,7 +203,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.231819987297058
+      Distance: 4.799141883850098
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -226,10 +226,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1403
+  Height: 1011
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000026b000004cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000280000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004ce000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014d000004cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004ce000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000051fc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000063c000004ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000026b00000346fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000280000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000346000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014d00000346fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000346000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e00000051fc0100000002fb0000000800540069006d006501000000000000073e0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000037a0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -238,6 +238,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2560
-  X: 0
-  Y: 1080
+  Width: 1854
+  X: 66
+  Y: 32

src/control_system/rov_passthrough_control/rov_passthrough_control/PassthroughControl.py

@@ -4,12 +4,10 @@
 from ds4_driver_msgs.msg import Status
 from geometry_msgs.msg import Wrench, WrenchStamped
 
-from rov_controll_bus.Controller import Controller
-
 import numpy as np
 
 
-class PassthroughControl(Node, Controller):
+class PassthroughControl(Node):
     """
 
     Class managing different controllers for creating wrenches.
@@ -129,7 +127,6 @@ def __init__(self):
             None
         """
         Node.__init__(self=self, node_name='rov_passthrough_control_node')
-        Controller.__init__(self=self, name='PassthroughControl', version='0.1.0')
 
         self._logger = self.get_logger()
 
@@ -191,12 +188,74 @@ def __init__(self):
         self.declare_parameter('inv_torque_y', False)
         self.declare_parameter('inv_torque_z', False)
 
-        self.create_service(Trigger, f'controllers/{self.controller_name}/run', self.run)
-        self.create_service(Trigger, f'controllers/{self.controller_name}/stop', self.stop)
-
         self.sub_ds4_driver = None
         self.pub_joy_wrench = None
 
+        # Get parameters
+        self.controller = self.get_parameter("controller").value
+        self.controller = str(self.controller).upper()
+
+        self.wrench_topic_name = self.get_parameter("wrench_topic_name").value
+        self.wrench_stamped_topic_name = self.get_parameter("wrench_stamped_topic_name").value
+
+        self.send_stamped = self.get_parameter("send_stamped").value
+        self.frame_id = self.get_parameter("frame_id").value
+
+        self.max_norm = self.get_parameter('max_norm').value
+
+        self.max_force = self.get_parameter('max_force').value
+        self.max_torque = self.get_parameter('max_torque').value
+
+        self.equalization_type = self.get_parameter("equalization_type").value
+        self.equalization_type = str(self.equalization_type).upper()
+
+        self.inv_force_x = self.get_parameter('inv_force_x').value
+        self.inv_force_y = self.get_parameter('inv_force_y').value
+        self.inv_force_z = self.get_parameter('inv_force_z').value
+
+        self.inv_torque_x = self.get_parameter('inv_torque_x').value
+        self.inv_torque_y = self.get_parameter('inv_torque_y').value
+        self.inv_torque_z = self.get_parameter('inv_torque_z').value
+
+        # Create publishers
+        if self.send_stamped:
+            # If published message is WrenchStamped
+
+            self.pub_joy_wrench = self.create_publisher(WrenchStamped, self.wrench_stamped_topic_name, 0)
+        else:
+            # if published message is Wrench
+
+            self.pub_joy_wrench = self.create_publisher(Wrench, self.wrench_topic_name, 0)
+
+        if self.controller == 'DS4':
+            # Subscribe to ds4 topics
+
+            self.sub_ds4_driver = self.create_subscription(Status, "status", self.cb_ds4_driver, 0)
+
+            # Get name of axis on which wrench will be calculated
+            self.joy_force_x = self.get_parameter("ds4_force_x").value  # Force along x-axis.
+            self.joy_force_y = self.get_parameter("ds4_force_y").value  # Force along y-axis.
+            self.joy_force_z = self.get_parameter("ds4_force_z").value  # Force along z-axis.
+
+            self.joy_torque_x = self.get_parameter("ds4_torque_x").value  # Torque along x-axis.
+            self.joy_torque_y = self.get_parameter("ds4_torque_y").value  # Torque along x-axis. (not mapped by default)
+            self.joy_torque_z = self.get_parameter("ds4_torque_z").value  # Torque along x-axis. (not mapped by default)
+
+        elif self.controller == 'JOY':
+            # TODO: implement 'joy' controller
+
+            self._logger.fatal(f"Controller: {self.controller} not implemented yet")
+
+        elif self.controller == 'STATION':
+            # TODO: implement 'station' controller
+
+            self._logger.fatal(f"Controller: {self.controller} not implemented yet")
+
+        else:
+            # Wrong controller parameter
+            self._logger.error(f"Controller type: {self.controller} is not supported")
+            self._logger.error("Use: DS4, JOY or STATION instead")
+
     def run(self, request: Trigger.Request, response: Trigger.Response):
         # Get parameters
         self.controller = self.get_parameter("controller").value
@@ -266,7 +325,7 @@ def run(self, request: Trigger.Request, response: Trigger.Response):
         self._is_running = True
         response.success = True
         response.message = 'ok'
-        self.get_logger().info(f"{self.controller_name} started with status: {self.is_running}")
+        self.get_logger().info(f"started with status: {self.is_running}")
 
         return response
 

src/trajectory/rov_position_snapshot/config/params.yaml

@@ -283,7 +283,7 @@ rov_thruster_manager:
     #   20
     thruster_voltage: 20
 
-rov_wrench_system_base:
+rov_passthrough_control_node:
   ros__parameters:
     controller: "ds4"
 
@@ -298,7 +298,7 @@ rov_wrench_system_base:
     # Maximum force that can be applied by the controller
     # If given force is higher than calculated by thruster manager, it will be clipped
     max_force: 25.0 # kg f
-    max_torque: 2.0 # Nm
+    max_torque: 1.0 # Nm
 
     equalization_type: "square"
 

src/trajectory/rov_position_snapshot/launch/snapshot_trajectories.launch.py

@@ -12,7 +12,7 @@
 def generate_launch_description():
 
     position_snapshot = FindPackageShare('rov_position_snapshot')
-    wrench_system_share = FindPackageShare('rov_wrench_system')
+    wrench_system_share = FindPackageShare('rov_passthrough_control')
 
     default_config_path = PathJoinSubstitution([position_snapshot, 'config', 'params.yaml'])
     default_rviz_config_path = PathJoinSubstitution([position_snapshot, 'rviz', 'tf_basic.rviz'])

src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:38.json

@@ -0,0 +1,105 @@
+{
+    "data": [
+        {
+            "position": {
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0
+            },
+            "orientation": {
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0,
+                "w": 1.0
+            },
+            "time": 0.0
+        },
+        {
+            "position": {
+                "x": 0.36145227010741277,
+                "y": -0.024590492407484454,
+                "z": -0.28961456819399123
+            },
+            "orientation": {
+                "x": -0.014192433409649761,
+                "y": -0.021829122689475997,
+                "z": -0.15278419026635826,
+                "w": 0.9879165225064179
+            },
+            "time": 5.1158568859100342
+        },
+        {
+            "position": {
+                "x": 0.7387028583959161,
+                "y": -0.11539607914831951,
+                "z": -0.4083403421370684
+            },
+            "orientation": {
+                "x": -0.01713825289027343,
+                "y": -0.007766335770419132,
+                "z": -0.18308565167306526,
+                "w": 0.9829168878740558
+            },
+            "time": 10.034105062484741
+        },
+        {
+            "position": {
+                "x": 1.2769636285199617,
+                "y": -0.29240204046312046,
+                "z": -0.37956671962739286
+            },
+            "orientation": {
+                "x": -0.018909542882122016,
+                "y": -0.0037579815447511913,
+                "z": -0.1904661135746421,
+                "w": 0.9815044403070574
+            },
+            "time": 15.1355578899383545
+        },
+        {
+            "position": {
+                "x": 1.7748052038788975,
+                "y": -0.4748072263552605,
+                "z": -0.3572568008435937
+            },
+            "orientation": {
+                "x": 0.002144429175860448,
+                "y": -0.019448834451266463,
+                "z": -0.15960120400357436,
+                "w": 0.9869876392045679
+            },
+            "time": 20.059095859527588
+        },
+        {
+            "position": {
+                "x": 2.3860496395791673,
+                "y": -0.6709787805626901,
+                "z": -0.33043948294641656
+            },
+            "orientation": {
+                "x": 0.003785203178515909,
+                "y": -0.033585922391302245,
+                "z": -0.09079772031749785,
+                "w": 0.9952956505677939
+            },
+            "time": 25.17641806602478
+        },
+        {
+            "position": {
+                "x": 2.913734835973335,
+                "y": -0.7968379647990971,
+                "z": -0.30404205677115903
+            },
+            "orientation": {
+                "x": 0.0008182281319539102,
+                "y": -0.03282923057810793,
+                "z": -0.0658784433416117,
+                "w": 0.997287121557909
+            },
+            "time": 30.0997209548950195
+        }
+    ],
+    "timestamp": 1761857086.212088,
+    "creation_time": "25-10-30_20:44",
+    "time_ratio": 1.0
+}