diff --git a/motion/colav/CMakeLists.txt b/motion/colav/CMakeLists.txt index 46d5b5d4..10b7e9b2 100644 --- a/motion/colav/CMakeLists.txt +++ b/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} ) diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index cff90df0..520fdc25 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/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): diff --git a/motion/colav/scripts/colav_sim.py b/motion/colav/scripts/colav_sim.py new file mode 100755 index 00000000..8874aeb1 --- /dev/null +++ b/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() + + + \ No newline at end of file diff --git a/motion/colav/scripts/cone_visualizer.py b/motion/colav/scripts/cone_visualizer.py new file mode 100644 index 00000000..3025b66a --- /dev/null +++ b/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() \ No newline at end of file