Finished hybridpath simulator

guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py

@@ -23,7 +23,7 @@ def __init__(self, waypoints):
         self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1)
 
         # Publishers
-        self.guidance_publisher_ = self.create_publisher(HybridpathReference, "guidance/Hybridpath/reference", 1)
+        self.guidance_publisher_ = self.create_publisher(HybridpathReference, "guidance/hybridpath/reference", 1)
 
         # Init parameters
         self.waypoints = waypoints

motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py

@@ -1,6 +1,4 @@
 import numpy as np
-import matplotlib.pyplot as plt
-from hybridpath_controller.hybridpath import HybridPathGenerator, HybridPathSignals
 
 class AdaptiveBackstep:
     def __init__(self, kappa):

motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py

@@ -10,7 +10,7 @@ class HybridPathControllerNode(Node):
     def __init__(self):
         super().__init__("hybridpath_controller_node")
 
-        self.hpref_subscriber_ = self.create_subscription(HybridpathReference, "guidance/Hybridpath/reference", self.hpref_cb, 1)
+        self.hpref_subscriber_ = self.create_subscription(HybridpathReference, "guidance/hybridpath/reference", self.hpref_cb, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
 
         self.declare_parameters(
@@ -21,7 +21,7 @@ def __init__(self):
 
         self.kappa = self.get_parameter('hybridpath_controller.kappa').get_parameter_value().double_value
 
-        self.AB_controller = AdaptiveBackstep(self.kappa)
+        self.AB_controller = AdaptiveBackstep()
 
         self.get_logger().info("hybridpath_controller_node started")
 

motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py

@@ -0,0 +1,15 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    vessel_hybridpath_simulator_node = Node(
+            package='vessel_hybridpath_simulator',
+            executable='vessel_hybridpath_simulator_node',
+            name='vessel_hybridpath_simulator_node',
+            output='screen',
+        )
+    return LaunchDescription([
+        vessel_hybridpath_simulator_node
+    ])

motion/vessel_hybridpath_simulator/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vessel_hybridpath_simulator</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">vortex</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>python-transforms3d-pip</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

motion/vessel_hybridpath_simulator/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/vessel_hybridpath_simulator
+[install]
+install_scripts=$base/lib/vessel_hybridpath_simulator

motion/vessel_hybridpath_simulator/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'vessel_hybridpath_simulator'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='martin',
+    maintainer_email='[email protected]',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'vessel_hybridpath_simulator_node = vessel_hybridpath_simulator.vessel_hybridpath_simulator_node:main'
+        ],
+    },
+)

motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py

@@ -0,0 +1,189 @@
+import rclpy
+import numpy as np
+import time
+
+from rclpy.node import Node
+import matplotlib.pyplot as plt
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Wrench
+from vortex_msgs.msg import HybridpathReference
+from hybridpath_controller.adaptive_backstep import AdaptiveBackstep
+from hybridpath_guidance.hp_guidance_utils import Rot
+from transforms3d.euler import euler2quat
+
+
+class VesselHybridpathSimulatorNode(Node):
+    def __init__(self):
+        super().__init__("vessel_hybridpath_simulator_node")
+
+        self.wrench_subscriber_ = self.create_subscription(Wrench, "thrust/wrench_input", self.wrench_input_cb, 1)
+        self.hp_ref_subscriber_ = self.create_subscription(HybridpathReference, "guidance/hybridpath/reference", self.hp_ref_cb, 1)
+        self.state_publisher_ = self.create_publisher(Odometry, "/sensor/seapath/odometry/ned", 1)
+
+        T = 200.0
+        self.dt = 0.05
+
+        self.time = np.arange(0, T, self.dt)
+        self.i = 1 # Step counter
+        self.i_ref = 1 # Step counter for guidance ref
+
+        waypoints = np.array([[10, 0],
+            [10, 10],
+            [0, 20],
+            [30, 30],
+            [40,0],
+            [0,-10],
+            [0, 0],
+            [10, 0]])
+
+
+        self.eta_d = np.zeros((3, len(self.time)))
+        self.eta = np.zeros((3, len(self.time)))
+        self.eta_d[:,0] = np.array([waypoints[0,0], waypoints[0,1], np.pi/2])
+        self.eta[:,0] = np.array([waypoints[0,0], waypoints[0,1], np.pi/2])
+        self.nu = np.zeros((3, len(self.time)))
+        self.tau = np.zeros((3, len(self.time)))
+
+        self.AB = AdaptiveBackstep()
+
+        time.sleep(1)
+        odometry_msg = self.state_to_odometrymsg()
+        self.state_publisher_.publish(odometry_msg)
+
+
+        self.get_logger().info("vessel_hybridpath_simulator_node started")
+        self.get_logger().info("""\
+
+                                       ._ o o
+                                       \_`-)|_
+                                    ,""       \ 
+                                  ,"  ## |   ಠ ಠ. 
+                                ," ##   ,-\__    `.
+                              ,"       /     `--._;)
+                            ,"     ## /           U
+                          ,"   ##    /
+
+                Simulation time: """ + str(self.T) + """ secs. Pleas wait for sim to funish:)
+                    """)
+
+    def wrench_input_cb(self, msg: Wrench):
+        if self.i == len(self.time):
+            self.plot_simulation()
+            return
+        if self.i_ref > len(self.time):
+            return
+
+        self.tau[:, self.i] = np.array([msg.force.x, msg.force.y, msg.torque.z])
+
+        # Step in nu and eta
+        nu_dot = np.linalg.inv(self.AB.M) @ self.tau[:, self.i] - np.linalg.inv(self.AB.M) @ self.AB.D @ self.nu[:, self.i-1]
+        self.nu[:, self.i] = self.nu[:, self.i-1] + nu_dot * self.dt
+        R, R_trsp = Rot(self.eta[2,self.i-1])
+        eta_dot = R @ self.nu[:, self.i]
+        self.eta[:, self.i] = self.eta[:, self.i-1] + eta_dot * self.dt
+
+        psi0 = self.eta[2, self.i-1]
+        psi1 = self.eta[2, self.i]
+        psi_vec = np.array([psi0, psi1])
+        psi_vec = np.unwrap(psi_vec, period=2*np.pi)
+        self.eta[2, self.i] = psi_vec[1]
+
+        # Publish state
+        odometry_msg = self.state_to_odometrymsg()
+        self.state_publisher_.publish(odometry_msg)
+
+        self.i += 1
+
+
+    def hp_ref_cb(self, msg: HybridpathReference):
+        self.eta_d[:, self.i_ref] = np.array([msg.eta_d.x, msg.eta_d.y, msg.eta_d.theta])
+        self.i_ref += 1
+
+    def state_to_odometrymsg(self):
+        orientation_list_next = euler2quat(0, 0, self.eta[2, self.i])
+
+        odometry_msg = Odometry()
+        odometry_msg.pose.pose.position.x = self.eta[0, self.i]
+        odometry_msg.pose.pose.position.y = self.eta[1, self.i]
+        odometry_msg.pose.pose.position.z = 0.0
+        odometry_msg.pose.pose.orientation.w = orientation_list_next[0]
+        odometry_msg.pose.pose.orientation.x = orientation_list_next[1]
+        odometry_msg.pose.pose.orientation.y = orientation_list_next[2]
+        odometry_msg.pose.pose.orientation.z = orientation_list_next[3]
+        odometry_msg.twist.twist.linear.x = self.nu[0, self.i]
+        odometry_msg.twist.twist.linear.y = self.nu[1, self.i]
+        odometry_msg.twist.twist.linear.z = 0.0
+        odometry_msg.twist.twist.angular.x = 0.0
+        odometry_msg.twist.twist.angular.y = 0.0
+        odometry_msg.twist.twist.angular.z = self.nu[2, self.i]
+
+        return odometry_msg
+
+
+    def plot_simulation(self):
+             # Plotting
+        plt.figure()
+        plt.plot(self.eta_d[1,:], self.eta_d[0,:], label='Reference path', zorder = 0)
+        plt.plot(self.eta[1,:], self.eta[0,:], label='Actual path', zorder = 1)
+        for i in range(0, len(self.eta_d[2]), 100):
+            plt.quiver(self.eta[1,i], self.eta[0,i], np.sin(self.eta[2,i]), np.cos(self.eta[2,i]), zorder = 2)
+        plt.title('Actual path vs reference path')
+        plt.xlabel('y [m]')
+        plt.ylabel('x [m]')
+        plt.gca().set_axisbelow(True)
+        plt.grid()
+        plt.legend()
+
+        plt.figure()
+        plt.plot(self.time, self.eta[0,:], label='Actual x')
+        plt.plot(self.time, self.eta_d[0,:], label='Reference x')
+        plt.plot(self.time, self.eta[1,:], label='Actual y')
+        plt.plot(self.time, self.eta_d[1,:], label='Reference y')
+        plt.title('Actual position vs reference position')
+        plt.xlabel('Time [s]')
+        plt.ylabel('Position [m]')
+        plt.gca().set_axisbelow(True)
+        plt.grid()
+        plt.legend()
+
+        plt.figure()
+        plt.plot(self.time, self.eta[2,:], label='Actual heading')
+        plt.plot(self.time, self.eta_d[2,:], label='Reference heading')
+        plt.title('Actual heading vs reference heading')
+        plt.xlabel('Time [s]')
+        plt.ylabel('Heading [rad]')
+        plt.gca().set_axisbelow(True)
+        plt.grid()
+        plt.legend()
+
+        plt.figure()
+        plt.plot(self.time, self.nu[0,:], label='Surge velocity')
+        plt.plot(self.time, self.nu[1,:], label='Sway velocity')
+        plt.title('velocity')
+        plt.xlabel('Time [s]')
+        plt.ylabel('Velocity [m/s]')
+        plt.gca().set_axisbelow(True)
+        plt.grid()
+        plt.legend()
+
+        plt.figure()
+        plt.plot(self.time, self.tau[0,:], label='Surge force')
+        plt.plot(self.time, self.tau[1,:], label='Sway force')
+        plt.title('Force')
+        plt.xlabel('Time [s]')
+        plt.ylabel('Force [N]')
+        plt.gca().set_axisbelow(True)
+        plt.grid()
+        plt.legend()
+
+        plt.show()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VesselHybridpathSimulatorNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()