diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index e32eaa5..70a0c6b 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/.gitignore b/.gitignore index 90e6b86..50d899b 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,4 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE +.idea \ No newline at end of file diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/control_system/rov_mfsmc/config/params.yaml b/src/control_system/rov_mfsmc/config/params.yaml index 7576b5f..6834278 100644 --- a/src/control_system/rov_mfsmc/config/params.yaml +++ b/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' diff --git a/src/control_system/rov_mfsmc/launch/mfsmc.launch.py b/src/control_system/rov_mfsmc/launch/mfsmc.launch.py index 328b72b..76ae29e 100644 --- a/src/control_system/rov_mfsmc/launch/mfsmc.launch.py +++ b/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 diff --git a/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl.py b/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl.py index dc16962..d219528 100644 --- a/src/control_system/rov_mfsmc/rov_mfsmc/ModelFreeSlidingControl.py +++ b/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: diff --git a/src/control_system/rov_mfsmc/rviz/tf_basic.rviz b/src/control_system/rov_mfsmc/rviz/tf_basic.rviz index c342322..2c93f7e 100644 --- a/src/control_system/rov_mfsmc/rviz/tf_basic.rviz +++ b/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 diff --git a/src/control_system/rov_passthrough_control/rov_passthrough_control/PassthroughControl.py b/src/control_system/rov_passthrough_control/rov_passthrough_control/PassthroughControl.py index d3ce609..0e5b3bf 100644 --- a/src/control_system/rov_passthrough_control/rov_passthrough_control/PassthroughControl.py +++ b/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 diff --git a/src/trajectory/__init__.py b/src/trajectory/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/trajectory/rov_position_snapshot/config/params.yaml b/src/trajectory/rov_position_snapshot/config/params.yaml index 8509332..861d879 100644 --- a/src/trajectory/rov_position_snapshot/config/params.yaml +++ b/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" diff --git a/src/trajectory/rov_position_snapshot/launch/snapshot_trajectories.launch.py b/src/trajectory/rov_position_snapshot/launch/snapshot_trajectories.launch.py index bd05ad9..5afeda3 100644 --- a/src/trajectory/rov_position_snapshot/launch/snapshot_trajectories.launch.py +++ b/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']) diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:38.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:38.json new file mode 100644 index 0000000..6c8f20f --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:49.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:49.json new file mode 100644 index 0000000..e8c479f --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:49.json @@ -0,0 +1,287 @@ +{ + "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": 1.144782323927497, + "y": -0.028623928622120418, + "z": -0.018636215486906416 + }, + "orientation": { + "x": -0.011686130073814938, + "y": 0.04788028505495589, + "z": 0.014864434246978853, + "w": 0.9986741016274885 + }, + "time": 4.882278919219971 + }, + { + "position": { + "x": 1.5300105953227283, + "y": -0.029925881235074545, + "z": -0.03561443907790662 + }, + "orientation": { + "x": -0.0020989825743063027, + "y": 0.019343309317027036, + "z": 0.019175583676901177, + "w": 0.9996267941824433 + }, + "time": 5.757691860198975 + }, + { + "position": { + "x": 1.872467647820974, + "y": -0.03009523466369189, + "z": -0.04726633201725611 + }, + "orientation": { + "x": -0.003635483517779786, + "y": -0.019235215392581977, + "z": 0.018739263992990186, + "w": 0.9996327474294717 + }, + "time": 6.813582897186279 + }, + { + "position": { + "x": 2.162859591662862, + "y": -0.030398470062306653, + "z": -0.05597832873745395 + }, + "orientation": { + "x": -0.003934284484647506, + "y": -0.006071209124240281, + "z": 0.01853082831550358, + "w": 0.9998021155345216 + }, + "time": 8.049739837646484 + }, + { + "position": { + "x": 2.393111580358328, + "y": -0.03989257948635059, + "z": -0.9234051162446075 + }, + "orientation": { + "x": -0.003909174596172657, + "y": 0.0165475127174609, + "z": -0.00684768811835927, + "w": 0.9998319895583836 + }, + "time": 12.501633882522583 + }, + { + "position": { + "x": 2.3797636363685855, + "y": -0.04229294979146367, + "z": -1.4032962409793444 + }, + "orientation": { + "x": -0.003687739388499173, + "y": 0.0022964310214825287, + "z": -0.020488797076448493, + "w": 0.9997806440300423 + }, + "time": 13.921759843826294 + }, + { + "position": { + "x": 2.369197940683504, + "y": -0.04466833149445275, + "z": -1.8853124610632497 + }, + "orientation": { + "x": -0.004248154996519389, + "y": -0.0027740292608110944, + "z": -0.00040671036382975605, + "w": 0.9999870461798321 + }, + "time": 15.889516830444336 + }, + { + "position": { + "x": 2.2424501745037007, + "y": -0.040279156765080815, + "z": -2.243990311287115 + }, + "orientation": { + "x": 0.0011263936936649174, + "y": 0.009418680319289493, + "z": -0.29649446170783844, + "w": 0.954987462679416 + }, + "time": 20.44601798057556 + }, + { + "position": { + "x": 2.0923114787286825, + "y": 0.0010848936598561662, + "z": -2.3249277547296523 + }, + "orientation": { + "x": -0.012975597186423582, + "y": -0.006852986897568668, + "z": -0.7302040372722136, + "w": 0.6830715441296008 + }, + "time": 23.75749683380127 + }, + { + "position": { + "x": 2.0888650658418624, + "y": 0.024745556334627036, + "z": -2.2892424960130913 + }, + "orientation": { + "x": 0.001544044120665113, + "y": 0.003365540763781068, + "z": 0.9997199199178642, + "w": 0.023374575558498128 + }, + "time": 31.1629638671875 + }, + { + "position": { + "x": 2.136651638177406, + "y": 0.03557061923911908, + "z": -2.201828391848036 + }, + "orientation": { + "x": 0.002783726127998523, + "y": 0.0038786987414107465, + "z": 0.6376769076047355, + "w": 0.7702891457579935 + }, + "time": 35.674065828323364 + }, + { + "position": { + "x": 2.0675294797430683, + "y": -0.08524316509199056, + "z": -2.109188558636955 + }, + "orientation": { + "x": -0.003033107097836829, + "y": 0.011605680059298601, + "z": 0.025762087134077245, + "w": 0.9995961301036487 + }, + "time": 39.874109983444214 + }, + { + "position": { + "x": 2.568076136084243, + "y": -0.13335483451457647, + "z": -3.339800487110407 + }, + "orientation": { + "x": -0.00279734935126173, + "y": -0.016462802910692863, + "z": 0.03321314313281664, + "w": 0.9993087801476427 + }, + "time": 45.513861894607544 + }, + { + "position": { + "x": 2.9405802956564595, + "y": -0.13919335035542368, + "z": -3.940442678977471 + }, + "orientation": { + "x": -0.008758342808384164, + "y": -0.007070690996584813, + "z": -0.591068487633774, + "w": 0.8065428318982227 + }, + "time": 49.11393189430237 + }, + { + "position": { + "x": 3.006234575135554, + "y": -0.1355393167276344, + "z": -4.098381407297277 + }, + "orientation": { + "x": 0.011613291506657849, + "y": 0.0039248819216253375, + "z": 0.9980228802945735, + "w": -0.06164460374441589 + }, + "time": 53.37444496154785 + }, + { + "position": { + "x": 3.019887426181069, + "y": -0.12665736891661292, + "z": -4.111853709977599 + }, + "orientation": { + "x": 0.004898717570313261, + "y": 0.003091217156973754, + "z": 0.6973706075437325, + "w": 0.7166872976945664 + }, + "time": 57.478679895401 + }, + { + "position": { + "x": 3.040701743332865, + "y": -0.26097486808273523, + "z": -0.6496153557027784 + }, + "orientation": { + "x": 0.010896089910675413, + "y": 0.0036085831971023967, + "z": 0.9998456636080536, + "w": -0.013300463004678522 + }, + "time": 67.13883876800537 + }, + { + "position": { + "x": 3.04139073528757, + "y": -0.2626517879430744, + "z": -0.5531591253028331 + }, + "orientation": { + "x": 0.008209025799442764, + "y": 0.004532374887844827, + "z": 0.9992582071993521, + "w": -0.03735110196549989 + }, + "time": 67.69046878814697 + }, + { + "position": { + "x": 3.0445673150673804, + "y": -0.26525714040337867, + "z": -0.40923091088474467 + }, + "orientation": { + "x": 0.001209293382760134, + "y": 0.0028444358953865533, + "z": 0.9999110212928488, + "w": -0.012976759651141355 + }, + "time": 69.16714286804199 + } + ], + "timestamp": 1761857371.084349, + "creation_time": "25-10-30_20:49", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:51.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:51.json new file mode 100644 index 0000000..e0b827e --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:51.json @@ -0,0 +1,273 @@ +{ + "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.9406447481953464, + "y": 0.08739069845637637, + "z": 0.14116403135531197 + }, + "orientation": { + "x": -0.01715984661139751, + "y": -0.042134436749423576, + "z": 0.20150670333698298, + "w": 0.9784300063951142 + }, + "time": 5.675861120223999 + }, + { + "position": { + "x": 1.3264504376165203, + "y": 0.18523643724698236, + "z": 0.17992000108313455 + }, + "orientation": { + "x": -0.003931863257726422, + "y": -0.03573046107558235, + "z": 0.21908351659687939, + "w": 0.975043736125818 + }, + "time": 6.81564998626709 + }, + { + "position": { + "x": 1.5556266204474603, + "y": 0.2539432362645839, + "z": 0.20876127028499036 + }, + "orientation": { + "x": -0.0013891667215452331, + "y": -0.0014965203804190733, + "z": 0.1994300221138823, + "w": 0.9799099432714351 + }, + "time": 7.871554136276245 + }, + { + "position": { + "x": 1.6866213598422486, + "y": 0.29188680819104196, + "z": 0.23735866624652546 + }, + "orientation": { + "x": 0.00010854774370805574, + "y": 0.0009017006662371159, + "z": 0.08439124024715665, + "w": 0.9964322825575467 + }, + "time": 8.94003701210022 + }, + { + "position": { + "x": 2.3616529235706145, + "y": 0.4025892977668448, + "z": 0.2526976375791046 + }, + "orientation": { + "x": -0.031564357759705786, + "y": -0.06501411079279312, + "z": -0.19257997284650355, + "w": 0.9786162735084062 + }, + "time": 11.615677118301392 + }, + { + "position": { + "x": 3.2240109094549383, + "y": 0.16838515437516488, + "z": 0.2855916426408751 + }, + "orientation": { + "x": -0.022571206169914426, + "y": -0.005498384014530071, + "z": -0.19124254476048738, + "w": 0.9812678520662934 + }, + "time": 13.643866062164307 + }, + { + "position": { + "x": 3.9664112201619046, + "y": -0.2421731120573476, + "z": 0.22845148447689323 + }, + "orientation": { + "x": -0.040340250836632986, + "y": -0.03565221315880867, + "z": -0.31252804811772017, + "w": 0.9483816758030726 + }, + "time": 15.55638599395752 + }, + { + "position": { + "x": 5.993362987543526, + "y": -0.46567980421906263, + "z": -0.01993898859394072 + }, + "orientation": { + "x": 0.013527923995752374, + "y": -0.03638467771530142, + "z": 0.3421479651344049, + "w": 0.9388439276335044 + }, + "time": 21.8163480758667 + }, + { + "position": { + "x": 6.351343272220647, + "y": -0.24322537910318198, + "z": 0.3673409887633119 + }, + "orientation": { + "x": -0.02210011081924359, + "y": -0.024615440229917, + "z": 0.3816562288122817, + "w": 0.9237121782313211 + }, + "time": 23.988540172576904 + }, + { + "position": { + "x": 7.106119468244757, + "y": 0.31911533284626703, + "z": 0.6346817146845177 + }, + "orientation": { + "x": -0.0005394412931974558, + "y": -0.024008550171204693, + "z": -0.0628928115663486, + "w": 0.9977313229397219 + }, + "time": 26.604424953460693 + }, + { + "position": { + "x": 8.910046775891761, + "y": 0.35571020201489123, + "z": 0.11046602926637594 + }, + "orientation": { + "x": -0.031191389411858058, + "y": -0.0368050594593539, + "z": -0.3079208615894031, + "w": 0.9501879960422521 + }, + "time": 32.376296043395996 + }, + { + "position": { + "x": 9.813745375832129, + "y": -1.3511213399737718, + "z": 0.5114115553194498 + }, + "orientation": { + "x": -0.025572267667502053, + "y": -0.017269951486665387, + "z": -0.8427333378236266, + "w": 0.5374461175063406 + }, + "time": 36.93646311759949 + }, + { + "position": { + "x": 8.001927989549387, + "y": -1.195653508957308, + "z": -0.0670214422278872 + }, + "orientation": { + "x": 0.037868288010172974, + "y": -0.040627774761329956, + "z": 0.9632318186398584, + "w": 0.2628684846855402 + }, + "time": 44.77231216430664 + }, + { + "position": { + "x": 6.834735443311999, + "y": -0.4388612907686045, + "z": -0.04882757702856444 + }, + "orientation": { + "x": 0.029842629720466494, + "y": -0.045974906941984715, + "z": 0.9268723786554939, + "w": 0.37135336146122316 + }, + "time": 47.640583992004395 + }, + { + "position": { + "x": 5.492552761929189, + "y": 0.5977566163627657, + "z": 0.2694824717658273 + }, + "orientation": { + "x": 0.028124290157362095, + "y": -0.024623531729544755, + "z": 0.9869328824145557, + "w": -0.1567360571062235 + }, + "time": 51.45759296417236 + }, + { + "position": { + "x": 4.008990338240755, + "y": 0.05770698566940613, + "z": -0.316679678142779 + }, + "orientation": { + "x": 0.04735661189785108, + "y": -0.0059018459497206495, + "z": 0.9107475407736466, + "w": -0.41019682653416145 + }, + "time": 55.864190101623535 + }, + { + "position": { + "x": 1.526799080608079, + "y": -0.7781403526941433, + "z": -0.0944846013345757 + }, + "orientation": { + "x": -0.004859280207995455, + "y": -0.013845137473032707, + "z": 0.9647528265979103, + "w": 0.26274832660847836 + }, + "time": 64.17690205574036 + }, + { + "position": { + "x": -0.3621758062002085, + "y": -0.7216431661476679, + "z": 0.2601779192197269 + }, + "orientation": { + "x": 0.01392196535006544, + "y": -0.034697938710230615, + "z": 0.1444141782306045, + "w": 0.9888107893100843 + }, + "time": 74.94143104553223 + } + ], + "timestamp": 1761857519.09484, + "creation_time": "25-10-30_20:51", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:55.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:55.json new file mode 100644 index 0000000..fea875e --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:55.json @@ -0,0 +1,203 @@ +{ + "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.00909756339741219, + "y": -0.0008541828977198934, + "z": -0.025963663887893507 + }, + "orientation": { + "x": 0.00022211323948941904, + "y": 0.12249969577437338, + "z": -0.0038550267746618634, + "w": 0.9924610390183896 + }, + "time": 5.9274890422821045 + }, + { + "position": { + "x": 0.4852880326021053, + "y": -0.6694021912716097, + "z": -0.2790057157874717 + }, + "orientation": { + "x": -0.0702198920457258, + "y": 0.182109347828298, + "z": 0.17649835656691631, + "w": 0.9647557630425517 + }, + "time": 10.735570192337036 + }, + { + "position": { + "x": 1.6232304329258658, + "y": -2.2365155570996262, + "z": 0.26070422967681006 + }, + "orientation": { + "x": -0.05683226497437861, + "y": 0.11461249186964137, + "z": 0.3662831341945204, + "w": 0.9216673673131497 + }, + "time": 15.179678201675415 + }, + { + "position": { + "x": 3.0326536959360686, + "y": -3.026452745128763, + "z": 0.835926770790412 + }, + "orientation": { + "x": -0.1723636963272072, + "y": 0.12553571920803996, + "z": 0.7804523939942151, + "w": 0.5877291894231909 + }, + "time": 22.224050998687744 + }, + { + "position": { + "x": 4.398902323370662, + "y": -1.8792246989651424, + "z": 0.24084692620809833 + }, + "orientation": { + "x": -0.22419184407839812, + "y": 0.10094407686189977, + "z": 0.86374986826764, + "w": 0.43986870252708427 + }, + "time": 28.372843980789185 + }, + { + "position": { + "x": 3.8290605106028366, + "y": -0.36500127653604386, + "z": 0.1382573391900035 + }, + "orientation": { + "x": -0.28605206200244304, + "y": 0.0054984057510892624, + "z": 0.9553904017437767, + "w": 0.07330188001826465 + }, + "time": 35.561429977416992 + }, + { + "position": { + "x": 3.3632246419789964, + "y": 1.7673459035532026, + "z": 0.5945285670196189 + }, + "orientation": { + "x": -0.24095846447431302, + "y": -0.10940046184568998, + "z": 0.9018466895606184, + "w": -0.3415305343226059 + }, + "time": 45.82833814620972 + }, + { + "position": { + "x": 2.045761647316842, + "y": 2.8275788068569874, + "z": 0.8094371692373239 + }, + "orientation": { + "x": 0.15491268223097987, + "y": 0.20290080959971998, + "z": -0.6565345011160887, + "w": 0.7097857220260423 + }, + "time": 56.704984188079834 + }, + { + "position": { + "x": -1.0598145383325295, + "y": 2.3378147565748493, + "z": 1.4498818302951226 + }, + "orientation": { + "x": 0.00897836336090645, + "y": 0.18112088186776487, + "z": -0.2638787958440003, + "w": 0.9473555806806244 + }, + "time": 70.63210201263428 + }, + { + "position": { + "x": -1.0207158377878072, + "y": 1.2132096410548803, + "z": 0.9602494586891167 + }, + "orientation": { + "x": -0.01321931638111071, + "y": 0.17409529439516083, + "z": -0.08738719271867046, + "w": 0.9807545853538633 + }, + "time": 80.24848413467407 + }, + { + "position": { + "x": -1.1629874040908321, + "y": -0.17098443944357783, + "z": 0.7556322044178876 + }, + "orientation": { + "x": 0.008021656180177066, + "y": 0.2018719083628605, + "z": 0.04816614845595367, + "w": 0.9781939520304667 + }, + "time": 90.42900800704956 + }, + { + "position": { + "x": -0.20192501242347616, + "y": -0.4034281262731012, + "z": 0.8890160575467482 + }, + "orientation": { + "x": -0.061527610382962165, + "y": 0.17711596644537628, + "z": 0.053319384412662395, + "w": 0.9808166652522465 + }, + "time": 100.741512060165405 + }, + { + "position": { + "x": 0.45875134457123456, + "y": -0.40466723964929396, + "z": 0.7085859458392347 + }, + "orientation": { + "x": -0.0012367493159588802, + "y": -0.06521155044828461, + "z": 0.02122037345303784, + "w": 0.9976450370195674 + }, + "time": 110.00851011276245 + } + ], + "timestamp": 1761857704.478056, + "creation_time": "25-10-30_20:55", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:57.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:57.json new file mode 100644 index 0000000..f1d7f1e --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:57.json @@ -0,0 +1,567 @@ +{ + "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.9077829225710692, + "y": 0.07953609044568033, + "z": 0.10419934582502992 + }, + "orientation": { + "x": -0.004366837914903571, + "y": -0.010570077244589882, + "z": 0.006068971742405724, + "w": 0.999916182375132 + }, + "time": 4.756166934967041 + }, + { + "position": { + "x": 1.5749048617567776, + "y": 0.09337840590247293, + "z": 0.15360563818932668 + }, + "orientation": { + "x": -0.004437170029683688, + "y": -0.015455890399678209, + "z": 0.0071557465024118415, + "w": 0.9998450991358981 + }, + "time": 6.400482892990112 + }, + { + "position": { + "x": 2.2850171967565474, + "y": 0.10389546432332253, + "z": 0.11978863234320067 + }, + "orientation": { + "x": -0.006582746799027536, + "y": -0.012397841234669069, + "z": -0.01030244001398389, + "w": 0.9998483988620765 + }, + "time": 7.972108840942383 + }, + { + "position": { + "x": 2.9235497191624784, + "y": 0.10526988353067335, + "z": 0.005813533076889366 + }, + "orientation": { + "x": -0.004311509813174477, + "y": -0.018849255176374934, + "z": 0.00464039808023731, + "w": 0.999802272035868 + }, + "time": 9.295676946640015 + }, + { + "position": { + "x": 3.63624685265856, + "y": 0.10756269937403129, + "z": -0.10202522819960338 + }, + "orientation": { + "x": -0.004654080263375107, + "y": -0.017804710586227807, + "z": 0.001820415589608926, + "w": 0.9998289943309925 + }, + "time": 10.740186929702759 + }, + { + "position": { + "x": 4.2350445548674935, + "y": 0.10776617372338482, + "z": -0.19640783880213405 + }, + "orientation": { + "x": 0.006477975451609853, + "y": 0.0009061229253040575, + "z": 0.0016225262949940155, + "w": 0.9999772908340042 + }, + "time": 12.256181001663208 + }, + { + "position": { + "x": 4.61727424942696, + "y": 0.10750958809489664, + "z": -0.2668560991425044 + }, + "orientation": { + "x": 0.008234095192909486, + "y": 0.007130362800830821, + "z": -0.11002004650622998, + "w": 0.9938696830920287 + }, + "time": 13.793004035949707 + }, + { + "position": { + "x": 4.91078321633888, + "y": 0.09324445569589734, + "z": -0.31338585479665815 + }, + "orientation": { + "x": -0.0063802504248494825, + "y": -0.011099873198154437, + "z": -0.13438380483772772, + "w": 0.9908466471744437 + }, + "time": 15.28038501739502 + }, + { + "position": { + "x": 5.237755842424761, + "y": -0.0009881384490411094, + "z": -0.33786507513247765 + }, + "orientation": { + "x": -0.014620117841687484, + "y": -0.006043572518242913, + "z": -0.28561953217941033, + "w": 0.9582125078619704 + }, + "time": 16.684540033340454 + }, + { + "position": { + "x": 5.614155570546675, + "y": -0.29093909028628406, + "z": -0.34642332789190666 + }, + "orientation": { + "x": -0.013190099113389272, + "y": -0.0011960752424471327, + "z": -0.41819276096797026, + "w": 0.9082617493670971 + }, + "time": 18.51243495941162 + }, + { + "position": { + "x": 5.831963041200288, + "y": -0.6147211480191972, + "z": -0.3367300276627778 + }, + "orientation": { + "x": -0.0168672124231693, + "y": 0.0013056320123907034, + "z": -0.46337663479141056, + "w": 0.8859999361171013 + }, + "time": 20.320559978485107 + }, + { + "position": { + "x": 5.967055086896183, + "y": -0.9709279551208532, + "z": -0.31436805007785396 + }, + "orientation": { + "x": -0.0197160590144593, + "y": 0.005283526811949903, + "z": -0.5168587080438102, + "w": 0.855827340811597 + }, + "time": 22.176614999771118 + }, + { + "position": { + "x": 6.163426814185239, + "y": -1.7888753766978103, + "z": -0.2613781516976531 + }, + "orientation": { + "x": -0.0341004029502566, + "y": 0.008744305841914018, + "z": -0.5762942545569817, + "w": 0.8164836996527156 + }, + "time": 25.012853860855103 + }, + { + "position": { + "x": 6.397822007694807, + "y": -2.5711220250636146, + "z": -0.21867031409343593 + }, + "orientation": { + "x": -0.04106820632694372, + "y": -0.001319730214798473, + "z": -0.5770679324158646, + "w": 0.8156618552553054 + }, + "time": 26.82483196258545 + }, + { + "position": { + "x": 6.630748470899635, + "y": -3.3118252241180364, + "z": -0.201769497178895 + }, + "orientation": { + "x": -0.04826658495271855, + "y": -0.007372539826045597, + "z": -0.6393366085255109, + "w": 0.7673751907851941 + }, + "time": 28.348453044891357 + }, + { + "position": { + "x": 6.866644720706296, + "y": -4.393626069270286, + "z": -0.17079810469411558 + }, + "orientation": { + "x": -0.04142796319400836, + "y": 0.0030468152929930435, + "z": -0.6708375516639059, + "w": 0.7404400178675803 + }, + "time": 30.412730932235718 + }, + { + "position": { + "x": 6.9813075655432595, + "y": -5.320644159786611, + "z": -0.12372361460577827 + }, + "orientation": { + "x": -0.05073694496840665, + "y": -0.00324303391354969, + "z": -0.7073188340139179, + "w": 0.7050640482931321 + }, + "time": 32.09674596786499 + }, + { + "position": { + "x": 7.018571964509751, + "y": -5.993376856088294, + "z": -0.07883950032061751 + }, + "orientation": { + "x": -0.00658499510489602, + "y": 0.00680090338603551, + "z": -0.7075839595338691, + "w": 0.7065658679578104 + }, + "time": 33.63708782196045 + }, + { + "position": { + "x": 7.031505200648026, + "y": -6.421952273736724, + "z": -0.01723885921224283 + }, + "orientation": { + "x": -0.03236178804815981, + "y": 0.007314806367717739, + "z": -0.8486146205838438, + "w": 0.5279701071211015 + }, + "time": 35.97718691825867 + }, + { + "position": { + "x": 6.684798475016304, + "y": -7.272369116669158, + "z": 0.07843583540796131 + }, + "orientation": { + "x": 0.028848712833953922, + "y": -0.014509467181099514, + "z": 0.8658214542222524, + "w": -0.49930996038373665 + }, + "time": 39.285212993621826 + }, + { + "position": { + "x": 6.260598816758671, + "y": -7.940139289182163, + "z": 0.1090697792034491 + }, + "orientation": { + "x": 0.040503999665358, + "y": -0.012023105848617904, + "z": 0.9342426746948, + "w": -0.3541263838179355 + }, + "time": 41.61704087257385 + }, + { + "position": { + "x": 5.753979758632494, + "y": -8.457612397750623, + "z": 0.02501800013926228 + }, + "orientation": { + "x": 0.028436137273714773, + "y": -0.017581143122406818, + "z": 0.9395100327444788, + "w": -0.3408858868828812 + }, + "time": 43.66929388046265 + }, + { + "position": { + "x": 5.177497858138363, + "y": -8.940678927265038, + "z": -0.04927453254736405 + }, + "orientation": { + "x": 0.04245935021004627, + "y": -0.014509860778175995, + "z": 0.9605052074068243, + "w": -0.2746204909767514 + }, + "time": 45.82110786437988 + }, + { + "position": { + "x": 4.587201677669611, + "y": -9.332875711155355, + "z": -0.05556164048469095 + }, + "orientation": { + "x": 0.039012638156480206, + "y": -0.016261963112195146, + "z": 0.9785306673104064, + "w": -0.2017208361891755 + }, + "time": 47.68132996559143 + }, + { + "position": { + "x": 3.957881331256071, + "y": -9.647517897286598, + "z": -0.02863914330840442 + }, + "orientation": { + "x": 0.044881965784357616, + "y": -0.016730363799777853, + "z": 0.9933121177972867, + "w": -0.10505589327366645 + }, + "time": 49.49001693725586 + }, + { + "position": { + "x": 3.3368630101094277, + "y": -9.85814599680528, + "z": 0.01699643280158361 + }, + "orientation": { + "x": 0.04258577813381902, + "y": -0.01873346242363251, + "z": 0.9967581391811325, + "w": -0.06564084751529113 + }, + "time": 51.10146188735962 + }, + { + "position": { + "x": 2.7185447873787045, + "y": -9.999139986981216, + "z": 0.059341464482343675 + }, + "orientation": { + "x": 0.041021710714089554, + "y": -0.02039853939948907, + "z": 0.9987885917154942, + "w": -0.017957391220248267 + }, + "time": 52.573668003082275 + }, + { + "position": { + "x": 1.5981965749560663, + "y": -10.113831546511303, + "z": 0.14273301727992765 + }, + "orientation": { + "x": 0.05976427690246549, + "y": -0.017847047366301938, + "z": 0.9747595931213039, + "w": 0.2143675575375632 + }, + "time": 54.917738914489746 + }, + { + "position": { + "x": 0.7119729223807925, + "y": -9.852881104195257, + "z": 0.2720458947215305 + }, + "orientation": { + "x": 0.03226125109415501, + "y": -0.018147270161603283, + "z": 0.8245586648799895, + "w": 0.5645643421567208 + }, + "time": 56.969679832458496 + }, + { + "position": { + "x": 0.4777775704733589, + "y": -9.583817956599951, + "z": 0.31861543030245304 + }, + "orientation": { + "x": 0.020407345161769144, + "y": -0.00031247470753710543, + "z": 0.762173607466527, + "w": 0.6470508748966081 + }, + "time": 58.2541868686676 + }, + { + "position": { + "x": 0.20321640928046447, + "y": -8.751138471396661, + "z": 0.1455173435539571 + }, + "orientation": { + "x": 0.03113614159967543, + "y": -0.04090239527866981, + "z": 0.7412088338692077, + "w": 0.6693033686909112 + }, + "time": 60.96210789680481 + }, + { + "position": { + "x": 0.07143132149143838, + "y": -7.761278795069874, + "z": -0.03021591793063738 + }, + "orientation": { + "x": 0.029120807617875406, + "y": -0.03929616498139929, + "z": 0.7441671405470005, + "w": 0.6662004630075982 + }, + "time": 62.885473012924194 + }, + { + "position": { + "x": -0.06898893798702321, + "y": -6.535435486111312, + "z": -0.0763849338003478 + }, + "orientation": { + "x": 0.03271918593941811, + "y": -0.04869389997787845, + "z": 0.743500831442921, + "w": 0.6661567928198971 + }, + "time": 65.07034802436829 + }, + { + "position": { + "x": -0.15326306067457063, + "y": -5.631723913020752, + "z": -0.04636747965944579 + }, + "orientation": { + "x": 0.03122449680957488, + "y": -0.057722122342988, + "z": 0.6893796000807055, + "w": 0.7214214817870177 + }, + "time": 66.62961983680725 + }, + { + "position": { + "x": -0.14137181362196216, + "y": -4.45024191373736, + "z": 0.04335976234109187 + }, + "orientation": { + "x": 0.020949817408630635, + "y": -0.04670541694807464, + "z": 0.6699205566581112, + "w": 0.740666022539942 + }, + "time": 68.63340497016907 + }, + { + "position": { + "x": -0.10151754479934993, + "y": -3.818165176464345, + "z": 0.10674788128573776 + }, + "orientation": { + "x": 0.010503701652960017, + "y": 0.005327931598946881, + "z": 0.697368855815379, + "w": 0.7166156322152146 + }, + "time": 70.31381702423096 + }, + { + "position": { + "x": -0.0848567224777459, + "y": -3.525193750163014, + "z": 0.1641543615433238 + }, + "orientation": { + "x": 0.027016947687549225, + "y": -0.004851185882700972, + "z": 0.6991555482067519, + "w": 0.7144424888994887 + }, + "time": 72.18616104125977 + }, + { + "position": { + "x": -0.11927233863957065, + "y": -3.147279112493745, + "z": 0.28074665302269913 + }, + "orientation": { + "x": 0.022430183088218973, + "y": -0.017256750675120218, + "z": 0.6999147918542096, + "w": 0.7136654507445657 + }, + "time": 76.50200605392456 + }, + { + "position": { + "x": -0.11133893768375591, + "y": -2.50484263519058, + "z": 0.40046455680238496 + }, + "orientation": { + "x": 0.02154928914903508, + "y": -0.015664106855575854, + "z": 0.7012600503950696, + "w": 0.7124076119845245 + }, + "time": 80.4979019165039 + } + ], + "timestamp": 1761857872.122122, + "creation_time": "25-10-30_20:57", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:59.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:59.json new file mode 100644 index 0000000..95773f8 --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_20:59.json @@ -0,0 +1,147 @@ +{ + "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.5200317198347033, + "y": 0.023610390582995855, + "z": 0.09821592272580155 + }, + "orientation": { + "x": -0.027301334216066512, + "y": 0.008038185579790809, + "z": 0.0004006322692450731, + "w": 0.9995948500349491 + }, + "time": 2.38806414604187 + }, + { + "position": { + "x": 1.4020485611265863, + "y": 0.023452101246123958, + "z": 0.06674289484195074 + }, + "orientation": { + "x": -0.026061197606215912, + "y": 0.23921159744716125, + "z": -0.01198924287852595, + "w": 0.970543602153611 + }, + "time": 4.044198989868164 + }, + { + "position": { + "x": 2.015850551567218, + "y": 0.010770359553996522, + "z": -0.07781952008877657 + }, + "orientation": { + "x": -0.02490709967098377, + "y": 0.17864451491325029, + "z": -0.015683105057324453, + "w": 0.9834733417297847 + }, + "time": 5.3078789710998535 + }, + { + "position": { + "x": 2.890480690286094, + "y": -0.026296489674567436, + "z": -0.37214559277657916 + }, + "orientation": { + "x": 0.023088287996336342, + "y": 0.07307602602101487, + "z": -0.05279646524067839, + "w": 0.9956602626581338 + }, + "time": 7.28800106048584 + }, + { + "position": { + "x": 2.4943562406595405, + "y": 0.06318926589841034, + "z": -0.4205770265971936 + }, + "orientation": { + "x": 0.032782064750422814, + "y": 0.028431389500994938, + "z": -0.07630194164877933, + "w": 0.9961400534173737 + }, + "time": 9.36461615562439 + }, + { + "position": { + "x": 1.2941206560619327, + "y": 0.26676424375654517, + "z": -0.36516900727902385 + }, + "orientation": { + "x": 0.02659357156075687, + "y": -0.025285015333578808, + "z": -0.1466948643810513, + "w": 0.9885009189249389 + }, + "time": 11.65622615814209 + }, + { + "position": { + "x": 0.34451876360668143, + "y": 0.6108968461621607, + "z": -0.41926184674281247 + }, + "orientation": { + "x": -0.03829229380397772, + "y": -0.19432474825563667, + "z": -0.335971663009169, + "w": 0.9208119428555793 + }, + "time": 13.552045106887817 + }, + { + "position": { + "x": -0.04610645893805415, + "y": 1.1230674789984973, + "z": -0.5409359975243166 + }, + "orientation": { + "x": -0.026052202588500863, + "y": -0.1294497107613332, + "z": -0.4025936454384206, + "w": 0.9058048419923014 + }, + "time": 15.236552000045776 + }, + { + "position": { + "x": -0.19144814340305533, + "y": 1.729785400722767, + "z": -0.6756800877284699 + }, + "orientation": { + "x": -0.03738912873445252, + "y": -0.1556163765303628, + "z": -0.43942237393473615, + "w": 0.8839081251428792 + }, + "time": 16.72416615486145 + } + ], + "timestamp": 1761857980.345046, + "creation_time": "25-10-30_20:59", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_21:00.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_21:00.json new file mode 100644 index 0000000..c103e37 --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_10-30_21:00.json @@ -0,0 +1,217 @@ +{ + "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.004226997789300024, + "y": -0.013624783614539726, + "z": 0.019283704229672516 + }, + "orientation": { + "x": 0.013126238692783338, + "y": -0.005764319355481616, + "z": -0.5460168311705669, + "w": 0.8376515352809907 + }, + "time": 1.4441039562225342 + }, + { + "position": { + "x": -0.010844445452342422, + "y": -0.013338034263594312, + "z": 0.042266280835536296 + }, + "orientation": { + "x": 0.0005263798538640509, + "y": 0.007052660322110912, + "z": 0.9539138917972413, + "w": -0.29999711655759526 + }, + "time": 2.896523952484131 + }, + { + "position": { + "x": -0.012688725125030054, + "y": -0.007856757282622806, + "z": 0.06481695735894266 + }, + "orientation": { + "x": -0.001843175447987672, + "y": -0.003221046794965683, + "z": 0.9407468410248169, + "w": 0.3390893815259342 + }, + "time": 4.280035972595215 + }, + { + "position": { + "x": -0.005825228054926468, + "y": -0.00484358877269897, + "z": 0.08856769413202192 + }, + "orientation": { + "x": 0.00412471517820014, + "y": -0.001546108734894739, + "z": 0.5694134352197655, + "w": 0.8220394978732497 + }, + "time": 5.6285998821258545 + }, + { + "position": { + "x": -0.0022581274955696395, + "y": -0.009148353583487978, + "z": 0.11813605788060166 + }, + "orientation": { + "x": -0.0037861651609666864, + "y": -0.011314676253504238, + "z": -0.014709312089592148, + "w": 0.9998206235082887 + }, + "time": 7.187243938446045 + }, + { + "position": { + "x": 0.010900545566854491, + "y": -0.011931276879437624, + "z": 0.134815431899723 + }, + "orientation": { + "x": 0.002038810416409717, + "y": 0.07044506057968185, + "z": -0.04887842905023771, + "w": 0.9963153295345765 + }, + "time": 8.340046882629395 + }, + { + "position": { + "x": 0.04225264394212258, + "y": -0.01765333862775781, + "z": 0.11408468984364084 + }, + "orientation": { + "x": 0.01171701378079908, + "y": 0.3236164798345412, + "z": -0.08744044813586696, + "w": 0.9420664804552592 + }, + "time": 9.203778982162476 + }, + { + "position": { + "x": 0.03835580320338239, + "y": -0.02553173260176922, + "z": 0.09042374877846515 + }, + "orientation": { + "x": 0.04324692919829532, + "y": 0.41527598850631064, + "z": -0.14213466417451812, + "w": 0.8974816397704307 + }, + "time": 10.007938861846924 + }, + { + "position": { + "x": -0.06091068054943527, + "y": 0.0013442778241735676, + "z": 0.14973608327833143 + }, + "orientation": { + "x": -0.011969045532060718, + "y": -0.18461091901767906, + "z": -0.025331092748389267, + "w": 0.9824122791723808 + }, + "time": 12.080523014068604 + }, + { + "position": { + "x": -0.0775352192796337, + "y": 0.0013859338884047445, + "z": 0.16145221531730714 + }, + "orientation": { + "x": -0.016503644278321673, + "y": -0.40270134664772966, + "z": 0.007404989531406037, + "w": 0.9151526764773621 + }, + "time": 12.739748001098633 + }, + { + "position": { + "x": -0.07015878600183822, + "y": -0.0030364306675356167, + "z": 0.16836979976966115 + }, + "orientation": { + "x": -0.0009425295392567333, + "y": -0.4665206364389124, + "z": 0.04296159873163783, + "w": 0.8834658501884051 + }, + "time": 13.292497873306274 + }, + { + "position": { + "x": 0.08442026499513589, + "y": 0.008099637602952181, + "z": 0.2129652602213311 + }, + "orientation": { + "x": -0.020536623685844673, + "y": 0.24481832463693293, + "z": 0.08176046900856719, + "w": 0.9658972309293833 + }, + "time": 15.571942806243896 + }, + { + "position": { + "x": 0.04917405835521671, + "y": -0.0005665415781220817, + "z": 0.2502427579982638 + }, + "orientation": { + "x": 0.011926090461490011, + "y": -0.13901778063594228, + "z": 0.08282230519373515, + "w": 0.9867483421803946 + }, + "time": 17.360164880752563 + }, + { + "position": { + "x": 0.09046402032644218, + "y": 0.004661017127749467, + "z": 0.29795725972775233 + }, + "orientation": { + "x": -0.0046873531586833226, + "y": 0.06030253308088481, + "z": 0.07758904381910675, + "w": 0.9951490207519833 + }, + "time": 20.084028959274292 + } + ], + "timestamp": 1761858009.03032, + "creation_time": "25-10-30_21:00", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:10.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:10.json new file mode 100644 index 0000000..5c90b1b --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:10.json @@ -0,0 +1,162 @@ +{ + "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": 1.0022002721549865, + "y": -0.02603426312949697, + "z": 0.09155200186464096 + }, + "orientation": { + "x": -0.024817041279830827, + "y": -0.040647552171572704, + "z": 0.0009773184020356316, + "w": 0.9988648235939213 + }, + "time": 3.267390012741089 + }, + { + "position": { + "x": 2.484053648141947, + "y": 0.0191611643732017, + "z": 0.20647034303424794 + }, + "orientation": { + "x": -0.025624870244377176, + "y": -0.07353542368598344, + "z": 0.166359002181014, + "w": 0.982985549172326 + }, + "time": 5.371349096298218 + }, + { + "position": { + "x": 4.221890296164822, + "y": 0.4909651012308147, + "z": 0.01607419448658698 + }, + "orientation": { + "x": -0.019585270858069864, + "y": -0.04231332754119972, + "z": 0.3818661048276896, + "w": 0.9230407777891154 + }, + "time": 8.111510992050171 + }, + { + "position": { + "x": 5.284331901760153, + "y": 1.3554110098810288, + "z": -0.3072167458596602 + }, + "orientation": { + "x": -0.00044630166201201305, + "y": -0.05340379439826864, + "z": 0.5719742516351225, + "w": 0.818531179018329 + }, + "time": 10.77541708946228 + }, + { + "position": { + "x": 5.7538409998716515, + "y": 2.4635261415850596, + "z": -0.07987246849182217 + }, + "orientation": { + "x": 0.014199099217429957, + "y": -0.04867202827229216, + "z": 0.760897549069132, + "w": 0.646888196727891 + }, + "time": 13.267362117767334 + }, + { + "position": { + "x": 5.6513359013095865, + "y": 3.701463426072013, + "z": 0.20527108776916925 + }, + "orientation": { + "x": 0.026699014017340466, + "y": -0.04819585401838185, + "z": 0.8605751706669378, + "w": 0.5063345711459092 + }, + "time": 15.803203105926514 + }, + { + "position": { + "x": 5.2180772190460125, + "y": 4.708464776729119, + "z": 0.38108063876458187 + }, + "orientation": { + "x": 0.03541646414287175, + "y": -0.04723147246369431, + "z": 0.9167637581696548, + "w": 0.39504312901653993 + }, + "time": 17.967602968215942 + }, + { + "position": { + "x": 4.632593952724703, + "y": 5.480896759603502, + "z": 0.5136286631178751 + }, + "orientation": { + "x": 0.04199838957955188, + "y": -0.045436284860204255, + "z": 0.9531560301893126, + "w": 0.2960832001390432 + }, + "time": 19.887784004211426 + }, + { + "position": { + "x": 3.349817865713424, + "y": 6.404204357749668, + "z": 1.0408277052946677 + }, + "orientation": { + "x": 0.051432995877745184, + "y": -0.03514534626534856, + "z": 0.9977608983893129, + "w": 0.024344223468408362 + }, + "time": 23.199914932250977 + }, + { + "position": { + "x": 2.2892405213064726, + "y": 6.561722849935432, + "z": 1.3257708739735032 + }, + "orientation": { + "x": 0.06494573694885063, + "y": -0.0331712394140999, + "z": 0.9712672317708294, + "w": -0.22654289796004054 + }, + "time": 25.339532136917114 + } + ], + "timestamp": 1762085412.92448, + "creation_time": "25-11-02_12:10", + "time_ratio": 10.0 + +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:11.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:11.json new file mode 100644 index 0000000..34ad93b --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:11.json @@ -0,0 +1,175 @@ +{ + "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.019938538163095654, + "y": -0.0015812507701191691, + "z": 0.17919825459438155 + }, + "orientation": { + "x": 0.0018848701630318767, + "y": -0.01088706614611144, + "z": 0.010144088533916799, + "w": 0.9998875019336 + }, + "time": 1.6193878650665283 + }, + { + "position": { + "x": -0.07606500171775377, + "y": -0.004404884813583809, + "z": 0.6102996204035513 + }, + "orientation": { + "x": 0.0013148151210794282, + "y": -0.001792579323424909, + "z": 0.006724616389860331, + "w": 0.9999749184129448 + }, + "time": 3.467604875564575 + }, + { + "position": { + "x": -0.11217165911013671, + "y": -0.0060001096085606775, + "z": 0.871063545075601 + }, + "orientation": { + "x": 0.000196780161796873, + "y": 0.004553543711111205, + "z": 0.010469389089211736, + "w": 0.9999348070796104 + }, + "time": 4.50373387336731 + }, + { + "position": { + "x": -0.1953843548408729, + "y": -0.006232807764515513, + "z": 1.0033654611645515 + }, + "orientation": { + "x": 0.003876177893392136, + "y": 0.024358343383393894, + "z": -0.032199873992470415, + "w": 0.9991770686256888 + }, + "time": 6.4599769115448 + }, + { + "position": { + "x": -0.2597663871509779, + "y": -0.00043717698925416215, + "z": 0.7319219014591583 + }, + "orientation": { + "x": 0.0008072509800267742, + "y": 0.021053764399377242, + "z": -0.051593796705408895, + "w": 0.9984458760954404 + }, + "time": 7.491727828979492 + }, + { + "position": { + "x": -0.3135808295350304, + "y": 0.004522419631166841, + "z": 0.4355119774748243 + }, + "orientation": { + "x": -0.002324617040328345, + "y": -0.004781233586633826, + "z": -0.03937324016323462, + "w": 0.9992104302498318 + }, + "time": 8.644004821777344 + }, + { + "position": { + "x": -0.334681628824383, + "y": 0.005763434303842185, + "z": 0.23163870449275414 + }, + "orientation": { + "x": -0.0008951463930989883, + "y": -0.007428255538252943, + "z": -0.052793363985377276, + "w": 0.9985774283708303 + }, + "time": 9.5117769241333 + }, + { + "position": { + "x": -0.3505982203008795, + "y": 0.006683485342603657, + "z": -0.04569636025706769 + }, + "orientation": { + "x": -0.0008475366924223788, + "y": 0.00481592557443192, + "z": -0.05814709589697245, + "w": 0.9982960501680676 + }, + "time": 10.687611818313599 + }, + { + "position": { + "x": -0.36462221577268483, + "y": 0.00742687454689861, + "z": -0.29177320349713565 + }, + "orientation": { + "x": -0.000674923361709242, + "y": -0.0002543066517176897, + "z": -0.0580975296032396, + "w": 0.9983106514810824 + }, + "time": 12.127909898757935 + }, + { + "position": { + "x": -0.376760436041522, + "y": 0.008323224221304937, + "z": -0.43644191366354246 + }, + "orientation": { + "x": 0.00022389233429256968, + "y": 0.007551737712638125, + "z": -0.06252162694513853, + "w": 0.9980150135613567 + }, + "time": 13.159991025924683 + }, + { + "position": { + "x": -0.394160075631099, + "y": 0.010251017587584066, + "z": -0.5296104306627365 + }, + "orientation": { + "x": 0.00037269006831359877, + "y": 0.008006576784197783, + "z": -0.05218141405291102, + "w": 0.9986054555517662 + }, + "time": 14.1321120262146 + } + ], + "timestamp": 1762085495.008906, + "creation_time": "25-11-02_12:11", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:12.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:12.json new file mode 100644 index 0000000..3f8cac5 --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:12.json @@ -0,0 +1,49 @@ +{ + "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.00582974584851037, + "y": -0.013167430288016702, + "z": -0.15280694891821678 + }, + "orientation": { + "x": -0.14328067434564587, + "y": -0.029577294024725153, + "z": -0.05042770909851532, + "w": 0.9879538846486244 + }, + "time": 1.1636478900909424 + }, + { + "position": { + "x": 7.458956335160138e-05, + "y": -0.0048077445904703975, + "z": -0.3266655974395115 + }, + "orientation": { + "x": -0.541169395770977, + "y": -0.012289801208047058, + "z": -0.050754172098496454, + "w": 0.8392905693987861 + }, + "time": 2.6402299404144287 + } + ], + "timestamp": 1762085555.461045, + "creation_time": "25-11-02_12:12", + "time_ratio": 1.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:13.json b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:13.json new file mode 100644 index 0000000..125b5b4 --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/output_trajectory_11-02_12:13.json @@ -0,0 +1,343 @@ +{ + "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.1301866724046019, + "y": 0.000503007148309198, + "z": 0.041552088517684105 + }, + "orientation": { + "x": -0.01749083963009345, + "y": -0.01574849331977254, + "z": -0.00019655598944840462, + "w": 0.9997229700536716 + }, + "time": 1.4761419296264648 + }, + { + "position": { + "x": 0.4176012758176233, + "y": -0.0007971300554183713, + "z": 0.06432979031111663 + }, + "orientation": { + "x": -0.0160551835228428, + "y": -0.023529839563698998, + "z": -0.0030825060998731593, + "w": 0.9995894536700047 + }, + "time": 2.4605088233947754 + }, + { + "position": { + "x": 0.8191391401201267, + "y": -0.0027385652681473083, + "z": 0.08026538144066087 + }, + "orientation": { + "x": -0.015663784707454727, + "y": -0.023148185436591577, + "z": -0.0027567880044772953, + "w": 0.999605525934871 + }, + "time": 3.4679999351501465 + }, + { + "position": { + "x": 1.3236288708338249, + "y": -0.010458166367253607, + "z": 0.10504753335097705 + }, + "orientation": { + "x": -0.018366875888121834, + "y": -0.030709896917371943, + "z": -0.08628802752570386, + "w": 0.9956274084250384 + }, + "time": 4.500128984451294 + }, + { + "position": { + "x": 1.7776025136160576, + "y": -0.04304040171486001, + "z": 0.13166395036608436 + }, + "orientation": { + "x": -0.016039816795912657, + "y": -0.04653438948176989, + "z": -0.18121845849120718, + "w": 0.9822103365240986 + }, + "time": 5.364400863647461 + }, + { + "position": { + "x": 2.284126895948441, + "y": -0.12671178557701257, + "z": 0.16869556040416245 + }, + "orientation": { + "x": -0.02036742686727549, + "y": -0.04872912314680464, + "z": -0.2976894522747102, + "w": 0.9532007293768363 + }, + "time": 6.279946804046631 + }, + { + "position": { + "x": 2.284126895948441, + "y": -0.12671178557701257, + "z": 0.16869556040416245 + }, + "orientation": { + "x": -0.02036742686727549, + "y": -0.04872912314680464, + "z": -0.2976894522747102, + "w": 0.9532007293768363 + }, + "time": 6.781966924667358 + }, + { + "position": { + "x": 2.3019695047018316, + "y": -0.13102726847467627, + "z": 0.1704647904659362 + }, + "orientation": { + "x": -0.02058269764074111, + "y": -0.04861427510307109, + "z": -0.3060326591088711, + "w": 0.9505561616089742 + }, + "time": 7.2893688678741455 + }, + { + "position": { + "x": 3.0104532373152777, + "y": -0.4793862129270092, + "z": 0.44812490109733527 + }, + "orientation": { + "x": -0.040743715711445264, + "y": -0.03134148661378729, + "z": -0.668297072068438, + "w": 0.7421163549684212 + }, + "time": 7.988250017166138 + }, + { + "position": { + "x": 3.1826187905808365, + "y": -0.7578436771017145, + "z": 0.6650366025795534 + }, + "orientation": { + "x": -0.043835923663329356, + "y": -0.009759653333217279, + "z": -0.8360351515213852, + "w": 0.5468348803651909 + }, + "time": 8.912213802337646 + }, + { + "position": { + "x": 3.241469717535881, + "y": -0.9758733505357529, + "z": 0.8884203220044754 + }, + "orientation": { + "x": 0.022883348261812948, + "y": 0.027239479003594764, + "z": 0.9416019780219697, + "w": -0.33484336359118066 + }, + "time": 9.799850940704346 + }, + { + "position": { + "x": 3.241469717535881, + "y": -0.9758733505357529, + "z": 0.8884203220044754 + }, + "orientation": { + "x": 0.022883348261812948, + "y": 0.027239479003594764, + "z": 0.9416019780219697, + "w": -0.33484336359118066 + }, + "time": 10.301966905593872 + }, + { + "position": { + "x": 2.947913580385636, + "y": -1.0360166203034575, + "z": 1.0316211083569877 + }, + "orientation": { + "x": 0.002826076618301301, + "y": -0.0012532494318798226, + "z": 0.9103012665710258, + "w": 0.4139348339243697 + }, + "time": 12.643908023834229 + }, + { + "position": { + "x": 2.770235399202532, + "y": -0.9852979480013806, + "z": 0.7777399130380405 + }, + "orientation": { + "x": 0.007644750719875997, + "y": -0.0018349401328492602, + "z": 0.8289225256077567, + "w": 0.5593081774131305 + }, + "time": 13.567907810211182 + }, + { + "position": { + "x": 2.6147263051144236, + "y": -0.8520214041192298, + "z": 0.5932352714975486 + }, + "orientation": { + "x": 0.014271334177091703, + "y": -0.026898083750373347, + "z": 0.8597895453802239, + "w": 0.5097398942266842 + }, + "time": 14.636124849319458 + }, + { + "position": { + "x": 2.422178082295101, + "y": -0.5707113213383539, + "z": 0.5170137126323149 + }, + "orientation": { + "x": 0.02044525904111847, + "y": -0.029412622915224625, + "z": 0.8642261548504776, + "w": 0.501826705415671 + }, + "time": 15.860925912857056 + }, + { + "position": { + "x": 2.284523329400124, + "y": -0.34291378529438976, + "z": 0.4891171720398698 + }, + "orientation": { + "x": 0.015614318795390433, + "y": -0.025182717726253395, + "z": 0.8234991497495017, + "w": 0.5665431794120582 + }, + "time": 16.66412591934204 + }, + { + "position": { + "x": 2.11584072487871, + "y": -0.0011348605480310248, + "z": 0.47122327104608497 + }, + "orientation": { + "x": 0.018898731559731752, + "y": -0.025119551880499157, + "z": 0.7156367207427176, + "w": 0.6977649532495639 + }, + "time": 17.68403196334839 + }, + { + "position": { + "x": 1.999552189083157, + "y": 0.45598445801192167, + "z": 0.4743039952511202 + }, + "orientation": { + "x": 0.0148563184808681, + "y": -0.0039365608906962056, + "z": 0.3515030447476496, + "w": 0.9360605764707115 + }, + "time": 19.20769691467285 + }, + { + "position": { + "x": 1.9253102348628475, + "y": 0.5250265224385338, + "z": 0.48021998078222494 + }, + "orientation": { + "x": 0.02626964828554848, + "y": 0.017821556221603863, + "z": 0.198369009191632, + "w": 0.9796132062733353 + }, + "time": 19.831849813461304 + }, + { + "position": { + "x": 1.4155996394919965, + "y": 0.46704801198828605, + "z": 0.5105966995037302 + }, + "orientation": { + "x": 0.017620328115480553, + "y": 0.06308979048051382, + "z": 0.07903315440852024, + "w": 0.9947175291903056 + }, + "time": 21.223687887191772 + }, + { + "position": { + "x": 0.8872485648261432, + "y": 0.39225010549992545, + "z": 0.5864623785281706 + }, + "orientation": { + "x": 0.03138349702805556, + "y": 0.05840826800067079, + "z": 0.028531571574304786, + "w": 0.9973913473491497 + }, + "time": 22.136053800582886 + }, + { + "position": { + "x": 0.2027053871240816, + "y": 0.33043638752990523, + "z": 0.6468530366828242 + }, + "orientation": { + "x": 0.008174327536259544, + "y": 0.06408353981669543, + "z": -0.019289971217747438, + "w": 0.9977245999294146 + }, + "time": 23.180047035217285 + } + ], + "timestamp": 1762085595.033629, + "creation_time": "25-11-02_12:13", + "time_ratio": 10.0 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/xyz_calibration_x.json b/src/trajectory/rov_position_snapshot/output/xyz_calibration_x.json new file mode 100644 index 0000000..e1d7dbb --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/xyz_calibration_x.json @@ -0,0 +1,202 @@ +{ + "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, + "time_ratio": 5 + }, + { + "position": { + "x": 0.00027613155814325274, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 2.887434959411621, + "time_ratio": 5 + }, + { + "position": { + "x": 0.16363366342940888, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 5.767106056213379, + "time_ratio": 5 + }, + { + "position": { + "x": 0.8301451155531631, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 8.17122197151184, + "time_ratio": 5 + }, + { + "position": { + "x": 1.5543725482752513, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 10.3350989818573, + "time_ratio": 5 + }, + { + "position": { + "x": 1.749765408728733, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 12.26731014251709, + "time_ratio": 5 + }, + { + "position": { + "x": 1.6816279836389398, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 14.187486171722412, + "time_ratio": 5 + }, + { + "position": { + "x": 1.399188275234206, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 15.546983003616333, + "time_ratio": 5 + }, + { + "position": { + "x": 0.9948490483378832, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 17.03468108177185, + "time_ratio": 5 + }, + { + "position": { + "x": 0.7428573073182263, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 18.426846027374268, + "time_ratio": 5 + }, + { + "position": { + "x": 0.5759510068885973, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 19.795199155807495, + "time_ratio": 5 + }, + { + "position": { + "x": 0.4485355403640259, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 21.343104124069214, + "time_ratio": 5 + }, + { + "position": { + "x": 0.44497612541303355, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 23.59838604927063, + "time_ratio": 5 + } + ], + "timestamp": 1762098659.053085, + "creation_time": "25-11-02_15:50", + "time_ratio": 5 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/xyz_calibration_xy.json b/src/trajectory/rov_position_snapshot/output/xyz_calibration_xy.json new file mode 100644 index 0000000..618c6ab --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/xyz_calibration_xy.json @@ -0,0 +1,202 @@ +{ + "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, + "time_ratio": 5 + }, + { + "position": { + "x": 0.00027613155814325274, + "y": -1.549839007531956, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 2.887434959411621, + "time_ratio": 5 + }, + { + "position": { + "x": 0.16363366342940888, + "y": -1.8572987486597692, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 5.767106056213379, + "time_ratio": 5 + }, + { + "position": { + "x": 0.8301451155531631, + "y": -1.8516593953338627, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 8.17122197151184, + "time_ratio": 5 + }, + { + "position": { + "x": 1.5543725482752513, + "y": -1.1936207548460422, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 10.3350989818573, + "time_ratio": 5 + }, + { + "position": { + "x": 1.749765408728733, + "y": -0.9623575744324022, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 12.26731014251709, + "time_ratio": 5 + }, + { + "position": { + "x": 1.6816279836389398, + "y": -0.24804959504565202, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 14.187486171722412, + "time_ratio": 5 + }, + { + "position": { + "x": 1.399188275234206, + "y": 0.2245400118328879, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 15.546983003616333, + "time_ratio": 5 + }, + { + "position": { + "x": 0.9948490483378832, + "y": 0.7063565778032567, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 17.03468108177185, + "time_ratio": 5 + }, + { + "position": { + "x": 0.7428573073182263, + "y": 0.973140667925043, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 18.426846027374268, + "time_ratio": 5 + }, + { + "position": { + "x": 0.5759510068885973, + "y": 0.5223893461132783, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 19.795199155807495, + "time_ratio": 5 + }, + { + "position": { + "x": 0.4485355403640259, + "y": 0.18255332458366938, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 21.343104124069214, + "time_ratio": 5 + }, + { + "position": { + "x": 0.44497612541303355, + "y": -0.10604587402272936, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 23.59838604927063, + "time_ratio": 5 + } + ], + "timestamp": 1762098659.053085, + "creation_time": "25-11-02_15:50", + "time_ratio": 5 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/xyz_calibration_y.json b/src/trajectory/rov_position_snapshot/output/xyz_calibration_y.json new file mode 100644 index 0000000..01aab33 --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/xyz_calibration_y.json @@ -0,0 +1,202 @@ +{ + "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, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -1.549839007531956, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 5.76078200340271, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -1.8572987486597692, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 7.272604942321777, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -1.8516593953338627, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 10.45302414894104, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -1.1936207548460422, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 14.3648841381073, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -0.9623575744324022, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 15.408432960510254, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -0.24804959504565202, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 17.88844895362854, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.2245400118328879, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 19.572774171829224, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.7063565778032567, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 21.21728014945984, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.973140667925043, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 22.04473304748535, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.5223893461132783, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 25.47338104248047, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.18255332458366938, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 26.745052099227905, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": -0.10604587402272936, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 27.825186014175415, + "time_ratio": 5 + } + ], + "timestamp": 1762098964.499571, + "creation_time": "25-11-02_15:56", + "time_ratio": 5 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/output/xyz_calibration_z.json b/src/trajectory/rov_position_snapshot/output/xyz_calibration_z.json new file mode 100644 index 0000000..8ed920f --- /dev/null +++ b/src/trajectory/rov_position_snapshot/output/xyz_calibration_z.json @@ -0,0 +1,217 @@ +{ + "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, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.6263190898642172 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 3.980091094970703, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.9905022447343887 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 6.4956090450286865, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -1.0409406153175216 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 7.516885042190552, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.7364262356625837 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 9.880182981491089, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.46936695151553387 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 11.26132607460022, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.032851654391305996 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 14.06861400604248, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.26942307771196217 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 16.217167139053345, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.4018400109060447 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 17.321348905563354, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.37723971244156496 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 22.829764127731323, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.14942126638330944 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 24.88176393508911, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.11262039843566618 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 26.95827293395996, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.2830422475186056 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 28.266556978225708, + "time_ratio": 5 + }, + { + "position": { + "x": 0.0, + "y": 0.0, + "z": -0.452771737268018 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 1.0 + }, + "time": 29.621618032455444, + "time_ratio": 5 + } + ], + "timestamp": 1762099289.085644, + "creation_time": "25-11-02_16:01", + "time_ratio": 2 +} \ No newline at end of file diff --git a/src/trajectory/rov_position_snapshot/rov_position_snapshot/PositionSnapshot.py b/src/trajectory/rov_position_snapshot/rov_position_snapshot/PositionSnapshot.py index 35f7d0a..a89ca90 100644 --- a/src/trajectory/rov_position_snapshot/rov_position_snapshot/PositionSnapshot.py +++ b/src/trajectory/rov_position_snapshot/rov_position_snapshot/PositionSnapshot.py @@ -22,6 +22,7 @@ def __init__(self): self.declare_parameter('ds4_snapshot_key', 'button_cross') self.declare_parameter('ds4_save_key', 'button_triangle') self.declare_parameter('parent_frame', 'world_ned') + self.declare_parameter('time_ratio', 5) self.output_dir = self.get_parameter('output_dir').value self.filename = self.get_parameter('filename').value @@ -31,6 +32,8 @@ def __init__(self): self.parent_frame = self.get_parameter('parent_frame').value + self.time_ratio = self.get_parameter('time_ratio').value + self.sub_ds4 = self.create_subscription(Status, 'status', self.cb_ds4, 0) self.pub_waypoints = self.create_publisher(Path, 'position_snapshot/waypoints', 0) @@ -123,9 +126,10 @@ def take_snapshot(self): def save(self): now = datetime.now() + filename = self.filename if not re.match(r'.*\.json', self.filename): - self.filename += now.strftime("%m-%d_%H:%M") - self.filename += '.json' + filename += now.strftime("%m-%d_%H:%M") + filename += '.json' dt = 0 snapshots_dict = [] @@ -141,7 +145,7 @@ def save(self): d = { 'position': message_to_ordereddict(s.pose.position), 'orientation': message_to_ordereddict(s.pose.orientation), - 'time': t - dt + 'time': t - dt, } snapshots_dict.append(d) @@ -150,8 +154,9 @@ def save(self): data['data'] = snapshots_dict data['timestamp'] = now.timestamp() data['creation_time'] = now.strftime("%y-%m-%d_%H:%M") + data['time_ratio'] = self.time_ratio - output_path = os.path.join(self.output_dir, self.filename) + output_path = os.path.join(self.output_dir, filename) with open(output_path, 'w') as json_file: json.dump(data, json_file, indent=4) diff --git a/src/trajectory/rov_position_snapshot/rviz/tf_basic.rviz b/src/trajectory/rov_position_snapshot/rviz/tf_basic.rviz index 945cc62..8b93076 100644 --- a/src/trajectory/rov_position_snapshot/rviz/tf_basic.rviz +++ b/src/trajectory/rov_position_snapshot/rviz/tf_basic.rviz @@ -87,24 +87,24 @@ Visualization Manager: Show Axes: true Show Names: true Tree: + base_link: + t200_link_t1: + {} + t200_link_t2: + {} + t200_link_t3: + {} + t200_link_t4: + {} + t200_link_t5: + {} + t200_link_t6: + {} + t200_link_t7: + {} + t200_link_t8: + {} world_ned: - base_link: - t200_link_t1: - {} - t200_link_t2: - {} - t200_link_t3: - {} - t200_link_t4: - {} - t200_link_t5: - {} - t200_link_t6: - {} - t200_link_t7: - {} - t200_link_t8: - {} position_snapshot_base: position_snapshot_node: {} @@ -157,10 +157,24 @@ Visualization Manager: Reliability Policy: Reliable Value: /traj_gen/path Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /bluerov2/left/image_color + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: world_ned Frame Rate: 30 Name: root Tools: @@ -229,7 +243,9 @@ Window Geometry: Height: 1403 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000026b000004cefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000280000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004ce000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014d000004cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004ce000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000051fc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000063c000004ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000026b000004cefc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000280000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004ce000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000a190000005b0000085600000759000000010000014d000004cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004ce000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000051fc0100000002fb0000000800540069006d0065010000000000000a000000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000063c000004ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/src/trajectory/traj_gen b/src/trajectory/traj_gen index 33cea96..bd9c033 160000 --- a/src/trajectory/traj_gen +++ b/src/trajectory/traj_gen @@ -1 +1 @@ -Subproject commit 33cea9667e4cd457aeb9abdb9a731b792c24f5bc +Subproject commit bd9c033c6121d2a9a6d007924ac105da8eee3d5f diff --git a/src/trajectory/traj_gen_interfaces/CMakeLists.txt b/src/trajectory/traj_gen_interfaces/CMakeLists.txt new file mode 100644 index 0000000..3d4c9e6 --- /dev/null +++ b/src/trajectory/traj_gen_interfaces/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.8) +project(traj_gen_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/AddPoint.srv" +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/trajectory/traj_gen_interfaces/package.xml b/src/trajectory/traj_gen_interfaces/package.xml new file mode 100644 index 0000000..bbce964 --- /dev/null +++ b/src/trajectory/traj_gen_interfaces/package.xml @@ -0,0 +1,19 @@ + + + + traj_gen_interfaces + 0.0.0 + TODO: Package description + dev + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/trajectory/traj_gen_interfaces/srv/AddPoint.srv b/src/trajectory/traj_gen_interfaces/srv/AddPoint.srv new file mode 100644 index 0000000..c153ea2 --- /dev/null +++ b/src/trajectory/traj_gen_interfaces/srv/AddPoint.srv @@ -0,0 +1,6 @@ +float64 x +float64 y +float64 z +--- +bool success +string message \ No newline at end of file