diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py index fd45ff26..fbc04c65 100644 --- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py +++ b/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 diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index 54ba2c80..c5d8caf3 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/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): diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py index eb0d4210..e61b417d 100644 --- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py +++ b/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") diff --git a/motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py b/motion/vessel_hybridpath_simulator/launch/vessel_hybridpath_simulator.launch.py new file mode 100644 index 00000000..5fb51358 --- /dev/null +++ b/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 + ]) diff --git a/motion/vessel_hybridpath_simulator/package.xml b/motion/vessel_hybridpath_simulator/package.xml new file mode 100644 index 00000000..d684d04c --- /dev/null +++ b/motion/vessel_hybridpath_simulator/package.xml @@ -0,0 +1,24 @@ + + + + vessel_hybridpath_simulator + 0.0.0 + TODO: Package description + vortex + MIT + + rclpy + nav_msgs + python-transforms3d-pip + geometry_msgs + vortex_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/motion/vessel_hybridpath_simulator/resource/vessel_hybridpath_simulator b/motion/vessel_hybridpath_simulator/resource/vessel_hybridpath_simulator new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_hybridpath_simulator/setup.cfg b/motion/vessel_hybridpath_simulator/setup.cfg new file mode 100644 index 00000000..a931c412 --- /dev/null +++ b/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 diff --git a/motion/vessel_hybridpath_simulator/setup.py b/motion/vessel_hybridpath_simulator/setup.py new file mode 100644 index 00000000..a8add765 --- /dev/null +++ b/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='martiniuss.wan@gmail.com', + 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' + ], + }, +) diff --git a/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/__init__.py b/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py b/motion/vessel_hybridpath_simulator/vessel_hybridpath_simulator/vessel_hybridpath_simulator_node.py new file mode 100644 index 00000000..d5595202 --- /dev/null +++ b/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()