Current

motion/colav/CMakeLists.txt

@@ -23,6 +23,8 @@ install(PROGRAMS
   scripts/colav_controller.py
   scripts/VO_math.py
   scripts/VO_visualize.py
+  scripts/colav_sim.py
+  scripts/cone_visualizer.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

motion/colav/scripts/colav_controller.py

@@ -8,6 +8,7 @@
 from VO_math import VelocityObstacle, Obstacle, Zones, Approaches
 from tf2_ros import Buffer, TransformListener
 from transformations import euler_from_quaternion
+from transforms3d.euler import quat2euler
 from geometry_msgs.msg import Quaternion
 import numpy as np
 import matplotlib.pyplot as plt
@@ -17,8 +18,8 @@ def __init__(self):
         super().__init__("colav_controller")
 
         self.declare_parameter('guidance_interface/colav_data_topic', 'guidance/collision_avoidance')    
-        self.declare_parameter('stop_zone_radius', 0.0)
-        self.declare_parameter('colimm_max_radius', math.inf)
+        self.declare_parameter('stop_zone_radius', 0.9)
+        self.declare_parameter('colimm_max_radius', 2.0)
 
         stop_zone_radius = self.get_parameter('stop_zone_radius').value
         colimm_max_radius = self.get_parameter('colimm_max_radius').value
@@ -94,11 +95,11 @@ def gen_colav_data(self):
         self.get_logger().info(f'Zone: {zone}')
 
         if zone == Zones.NOCOL:
-            return None
+            return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom, is_colav=False)
         elif zone == Zones.STOPNOW:
             return self.create_guidance_data(0, 0, self.vessel.heading, self.vessel_odom)
         elif zone == Zones.COLIMM and not VO.check_if_collision():
-            return None
+            return self.create_guidance_data(self.vessel.speed, self.vessel.heading, self.vessel.heading, self.vessel_odom)
 
         approach = self.gen_approach(closest_obst, self.vessel)
 
@@ -110,19 +111,28 @@ def gen_colav_data(self):
             return None
         return None
 
-    def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom):
+    def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom, is_colav=True):
         data = GuidanceData()
         data.u_d = float(speed)
         data.psi_d = float(psi_d)
         data.u = float(speed)  # Assuming this is the desired speed
         data.t = float(self.get_clock().now().seconds_nanoseconds()[0]) + float(self.get_clock().now().seconds_nanoseconds()[1]) * 1e-9
-        orientation_q = Quaternion(
-            x=vessel_odom.pose.pose.orientation.x,
-            y=vessel_odom.pose.pose.orientation.y,
-            z=vessel_odom.pose.pose.orientation.z,
-            w=vessel_odom.pose.pose.orientation.w)
-        _, _, yaw = self.quaternion_to_euler(orientation_q)
+        # orientation_q = Quaternion(
+        #     x=vessel_odom.pose.pose.orientation.x,
+        #     y=vessel_odom.pose.pose.orientation.y,
+        #     z=vessel_odom.pose.pose.orientation.z,
+        #     w=vessel_odom.pose.pose.orientation.w)
+        orientation_list = [
+            vessel_odom.pose.pose.orientation.w,
+            vessel_odom.pose.pose.orientation.x,
+            vessel_odom.pose.pose.orientation.y,
+            vessel_odom.pose.pose.orientation.z
+        ]
+        # _, _, yaw = self.quaternion_to_euler(orientation_q)
+        yaw = quat2euler(orientation_list)[2]
+        self.get_logger().info(f'Current yaw: {yaw}')
         data.psi = float(yaw)
+        data.is_colav = is_colav
         return data
 
     def gen_approach(self, obstacle: Obstacle, vessel: Obstacle):

motion/colav/scripts/colav_sim.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import colav_controller
+import rclpy
+from rclpy.node import Node
+import numpy as np
+from nav_msgs.msg import Odometry
+from vortex_msgs.msg import GuidanceData
+from transforms3d.euler import quat2euler, euler2quat
+
+
+
+
+class Simulator(Node):
+    def __init__(self):
+        super().__init__("simulator")
+
+        self.vessel_pub = self.create_publisher(Odometry, "/pose_gt", 10)
+        self.obstacle_pub = self.create_publisher(Odometry, "/obstacle", 10)
+        self.colav_sub = self.create_subscription(GuidanceData, "/guidance/collision_avoidance", self.colav_callback, 10)
+
+        self.pos_x = 0
+        self.pos_y = 0
+        self.speed = 0.5
+        self.obstacle_pos_x = 5
+        self.obstacle_pos_y = 5
+        self.obstacle_speed = 0.1
+        self.obstacle_heading = 5*np.pi/4
+        self.goal_pos_x = 8
+        self.goal_pos_y = 8
+
+        self.dt = 0.1
+
+        self.first_iteration = False
+
+        obs_timer = 0.1
+        self.create_timer(obs_timer, self.obstacle_callback)
+
+        self.get_logger().info("Simulator node started")
+
+    def step(self, speed, heading):
+        speed_x = speed*np.cos(heading)
+        speed_y = speed*np.sin(heading)
+        self.pos_x += speed_x*self.dt
+        self.pos_y += speed_y*self.dt
+        odom_msg = Odometry()
+        odom_msg.pose.pose.position.x = self.pos_x
+        odom_msg.pose.pose.position.y = self.pos_y
+        odom_msg.twist.twist.linear.x = speed_x
+        odom_msg.twist.twist.linear.y = speed_y
+        quat = self.heading_to_quaternion(heading)
+        odom_msg.pose.pose.orientation.x = quat[0]
+        odom_msg.pose.pose.orientation.y = quat[1]
+        odom_msg.pose.pose.orientation.z = quat[2]
+        odom_msg.pose.pose.orientation.w = quat[3]
+        self.vessel_pub.publish(odom_msg)
+
+    def colav_callback(self, msg: GuidanceData):
+        if msg.is_colav:
+            speed = msg.u
+            heading = msg.psi
+            self.step(speed, heading)
+        else:
+            speed = self.speed
+            heading = np.arctan2(self.goal_pos_y - self.pos_y, self.goal_pos_x - self.pos_x)
+            self.step(speed, heading)
+
+    def obstacle_callback(self):
+        obs_speed_x = self.obstacle_speed*np.cos(self.obstacle_heading)
+        obs_speed_y = self.obstacle_speed*np.sin(self.obstacle_heading)
+        self.obstacle_pos_x += obs_speed_x*self.dt
+        self.obstacle_pos_y += obs_speed_y*self.dt
+        obs_msg = Odometry()
+        obs_msg.pose.pose.position.x = self.obstacle_pos_x
+        obs_msg.pose.pose.position.y = self.obstacle_pos_y
+        obs_msg.twist.twist.linear.x = obs_speed_x
+        obs_msg.twist.twist.linear.y = obs_speed_y
+        quaternion = euler2quat(0, 0, self.obstacle_heading)
+        # self.get_logger().info(f"Quat: {quaternion}")
+        obs_msg.pose.pose.orientation.x = quaternion[0]
+        obs_msg.pose.pose.orientation.y = quaternion[1]
+        obs_msg.pose.pose.orientation.z = quaternion[2]
+        obs_msg.pose.pose.orientation.w = quaternion[3]
+        self.obstacle_pub.publish(obs_msg)
+
+    @staticmethod
+    def get_heading(msg: Odometry):
+        orientation_q = msg.pose.pose.orientation
+        orientation_list = [
+            orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z
+        ]
+        heading = quat2euler(orientation_list)[2]
+        return heading
+
+    @staticmethod
+    def heading_to_quaternion(heading):
+        return euler2quat(0, 0, heading)
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    simulator = Simulator()
+    rclpy.spin(simulator)
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+
+
+

motion/colav/scripts/cone_visualizer.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import colav_controller
+import rclpy
+from rclpy.node import Node
+from visualization_msgs.msg import Marker
+from geometry_msgs.msg import Point
+from std_msgs.msg import ColorRGBA
+
+class ConeVisualizer(Node):
+    def __init__(self):
+        super().__init__('cone_visualizer')
+        self.publisher = self.create_publisher(Marker, 'visualization_marker', 10)
+        self.timer = self.create_timer(0.1, self.publish_marker)
+
+    def publish_marker(self):
+        marker = Marker()
+        marker.header.frame_id = "world"
+        marker.header.stamp = self.get_clock().now().to_msg()
+        marker.ns = "cone"
+        marker.id = 0
+        marker.type = Marker.CYLINDER
+        marker.action = Marker.ADD
+        marker.pose.position.x = 1.0
+        marker.pose.position.y = 1.0
+        marker.pose.position.z = 0.5
+        marker.pose.orientation.w = 1.0
+        marker.scale.x = 1.0
+        marker.scale.y = 1.0
+        marker.scale.z = 1.0
+        marker.color.a = 0.8
+        marker.color.r = 1.0
+        marker.color.g = 0.0
+        marker.color.b = 0.0
+        self.publisher.publish(marker)
+
+if __name__ == '__main__':
+    rclpy.init()
+    node = ConeVisualizer()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()