From 44f283ea53c033290e709b1ca730756fe548f5cb Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 18 Mar 2024 19:13:27 +0100 Subject: [PATCH 01/10] feat: coded the foundation for colav --- motion/colav/CMakeLists.txt | 26 +++++++++++++++++++ motion/colav/LICENSE | 17 ++++++++++++ motion/colav/launch/__init__.py | 0 .../colav/launch/colav_controller.launch.py | 18 +++++++++++++ motion/colav/package.xml | 18 +++++++++++++ motion/colav/scripts/VO_math.py | 0 motion/colav/scripts/colav_controller.py | 0 7 files changed, 79 insertions(+) create mode 100644 motion/colav/CMakeLists.txt create mode 100644 motion/colav/LICENSE create mode 100644 motion/colav/launch/__init__.py create mode 100644 motion/colav/launch/colav_controller.launch.py create mode 100644 motion/colav/package.xml create mode 100644 motion/colav/scripts/VO_math.py create mode 100644 motion/colav/scripts/colav_controller.py diff --git a/motion/colav/CMakeLists.txt b/motion/colav/CMakeLists.txt new file mode 100644 index 00000000..ba84079d --- /dev/null +++ b/motion/colav/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(colav) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/motion/colav/LICENSE b/motion/colav/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/motion/colav/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/motion/colav/launch/__init__.py b/motion/colav/launch/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/colav/launch/colav_controller.launch.py b/motion/colav/launch/colav_controller.launch.py new file mode 100644 index 00000000..3c1d7819 --- /dev/null +++ b/motion/colav/launch/colav_controller.launch.py @@ -0,0 +1,18 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + colav_node = Node( + package='colav', + executable='colav_controller.py', + name='colav', + output='screen' + ) + + return LaunchDescription([ + colav_node + ]) \ No newline at end of file diff --git a/motion/colav/package.xml b/motion/colav/package.xml new file mode 100644 index 00000000..1ea086dd --- /dev/null +++ b/motion/colav/package.xml @@ -0,0 +1,18 @@ + + + + colav + 0.0.0 + Collision avoidance package + alice + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/motion/colav/scripts/VO_math.py b/motion/colav/scripts/VO_math.py new file mode 100644 index 00000000..e69de29b diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py new file mode 100644 index 00000000..e69de29b From a8b004a617447d2318f8134beb3aa745ee387f2c Mon Sep 17 00:00:00 2001 From: Alice Date: Wed, 3 Apr 2024 19:06:24 +0200 Subject: [PATCH 02/10] feat: added code for the calov controller --- motion/colav/CMakeLists.txt | 20 ++- motion/colav/package.xml | 7 + motion/colav/scripts/VO_math.py | 84 ++++++++++ motion/colav/{launch => scripts}/__init__.py | 0 motion/colav/scripts/colav_controller.py | 154 +++++++++++++++++++ 5 files changed, 262 insertions(+), 3 deletions(-) rename motion/colav/{launch => scripts}/__init__.py (100%) diff --git a/motion/colav/CMakeLists.txt b/motion/colav/CMakeLists.txt index ba84079d..c082fc21 100644 --- a/motion/colav/CMakeLists.txt +++ b/motion/colav/CMakeLists.txt @@ -7,9 +7,23 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) + +ament_python_install_package(scripts) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/colav_controller.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/motion/colav/package.xml b/motion/colav/package.xml index 1ea086dd..8997e8c0 100644 --- a/motion/colav/package.xml +++ b/motion/colav/package.xml @@ -8,6 +8,13 @@ MIT ament_cmake + nav_msgs + rclpy + geometry_msgs + ros2launch + nav_msgs + vortex_msgs + ament_lint_auto ament_lint_common diff --git a/motion/colav/scripts/VO_math.py b/motion/colav/scripts/VO_math.py index e69de29b..5121668d 100644 --- a/motion/colav/scripts/VO_math.py +++ b/motion/colav/scripts/VO_math.py @@ -0,0 +1,84 @@ +#!/usr/bin/python3 + +from geometry_msgs.msg import Point, Vector3 +from nav_msgs.msg import Odometry +import math +from enum import Enum +import numpy as np + + +class Zones(Enum): + NOCOL = 1 + COLIMM = 2 + STOPNOW = 3 + + +class Approaches(Enum): + FRONT = 1 + RIGHT = 2 + LEFT = 3 + BEHIND = 4 + + +class Obstacle: + + def __init__(self) -> None: + self.vx = 0 + self.vy = 0 + self.r = 0 + self.x = 0 + self.y = 0 + self.heading = 0 + self.speed = 0 + + +class VelocityObstacle: + """ + The Velocity Obstacle class + + A computational class used by the collision avoidance system + to determine if a collison can occur, and how the UAV should respond + + obstacle: An odometry of the object to avoid + radius_o: The radius of obstacle + vessel: An odometry of the UAV vessel + """ + + def __init__(self, vessel: Obstacle, obstacle: Obstacle) -> None: + + self.vessel = vessel + self.obstacle = obstacle + + self.left_angle = 0.0 + self.right_angle = 0.0 + + self.truncated_time = 5 #placeholder + + def set_cone_angles(self) -> None: + """ + Calculates the largest and smallest heading-angle where a collision can occur + """ + theta_ro = math.atan2(self.obstacle.y - self.vessel.y, + self.obstacle.x - self.vessel.x) + print("ob", self.vessel.r, self.obstacle.r) + theta_ray = math.asin( + (self.vessel.r + self.obstacle.r) / + (math.sqrt((self.obstacle.x - self.vessel.x)**2 + + (self.obstacle.y - self.vessel.y)**2))) + self.right_angle = theta_ro - theta_ray + self.left_angle = theta_ro + theta_ray + + def check_if_collision(self) -> bool: + """ + Returns true if the current velocity results in a collision. + Uses a truncated VO collision cone + + """ + + bouffer = 0 + dvx = self.obstacle.vx - self.vessel.vx + dvy = self.obstacle.vy - self.vessel.vy + angle = math.atan2(-dvy, -dvx) + print("vels", dvx, dvy) + + return angle > self.right_angle - bouffer and angle < self.left_angle + bouffer \ No newline at end of file diff --git a/motion/colav/launch/__init__.py b/motion/colav/scripts/__init__.py similarity index 100% rename from motion/colav/launch/__init__.py rename to motion/colav/scripts/__init__.py diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index e69de29b..b7725261 100644 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from vortex_msgs.msg import GuidanceData, OdometryArray +import math +from scripts.VO_math import VelocityObstacle, Obstacle, Zones, Approaches +from tf2_ros import Buffer, TransformListener +from transformations import euler_from_quaternion +from geometry_msgs.msg import Quaternion + +class ColavController(Node): + def __init__(self): + super().__init__("colav_controller") + + + self.declare_parameters(namespace='', parameters=[ + ('guidance_interface_colav_data_topic', '/guidance/colav_data'), + ('stop_zone_radius', 0.0), + ('colimm_max_radius', float('inf')), + ]) + + self.obstacle_sub = self.create_subscription(OdometryArray, "/tracking/mul_tracked_cv_objects", self.obst_callback, 10) + self.vessel_sub = self.create_subscription(Odometry, "/pose_gt", self.vessel_callback, 10) + self.colav_pub = self.create_publisher(GuidanceData, self.get_parameter("/guidance_interface/colav_data").get_parameter_value().string_value, 10) + + self.obstacles = [] + self.vessel_odom = Odometry() + self.vessel = Obstacle() + self.stop_zone_radius = self.get_parameter('stop_zone_radius').value + self.colimm_max_radius = self.get_parameter('colimm_max_radius').value + + def vessel_callback(self, msg): + self.vessel_odom = msg + self.vessel = self.odometry_to_obstacle(msg) + self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9 + + def obst_callback(self, msg): + self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odometry_array] + if not self.obstacles: + self.get_logger().info('No obstacles detected!') + return + colav_data = self.gen_colav_data() + if colav_data: + self.colav_pub.publish(colav_data) + + def quaternion_to_euler(quaternion: Quaternion): + """Convert a ROS Quaternion message to Euler angles (roll, pitch, yaw).""" + q = (quaternion.x, quaternion.y, quaternion.z, quaternion.w) + return euler_from_quaternion(q) + + def odometry_to_obstacle(self, odometry: Odometry): + # Convert Odometry message to an Obstacle object + # Assuming Obstacle class exists and is properly defined elsewhere + quaternion = ( + odometry.pose.pose.orientation.x, + odometry.pose.pose.orientation.y, + odometry.pose.pose.orientation.z, + odometry.pose.pose.orientation.w) + _, _, yaw = euler_from_quaternion(quaternion) + + return Obstacle( + x=odometry.pose.pose.position.x, + y=odometry.pose.pose.position.y, + vx=odometry.twist.twist.linear.x, + vy=odometry.twist.twist.linear.y, + heading=yaw, + speed=math.hypot(odometry.twist.twist.linear.x, odometry.twist.twist.linear.y), + radius=2) # Assuming a fixed radius for simplicity + + def get_distance(self, obstacle1: Obstacle, obstacle2: Obstacle): + return math.hypot(obstacle1.x - obstacle2.x, obstacle1.y - obstacle2.y) + + def get_closest_obst(self, obstacles: list[Obstacle], vessel: Obstacle): + return min(obstacles, key=lambda obs: self.get_distance(obs, vessel), default=None) + + def gen_colav_data(self): + closest_obst = self.get_closest_obst(self.obstacles) + if closest_obst is None: + self.get_logger().info('No obstacles detected.') + return None + + VO = VelocityObstacle(self.vessel, closest_obst) + VO.set_cone_angles() + + self.get_logger().info(f'Closest obstacle at {closest_obst.x}, {closest_obst.y}') + self.get_logger().info(f'Vessel at {self.vessel.x}, {self.vessel.y}') + self.get_logger().info(f'VO Angles: Left {VO.left_angle}, Right {VO.right_angle}') + + zone = self.get_zone(closest_obst, self.vessel) + self.get_logger().info(f'Zone: {zone}') + + if zone == Zones.NOCOL: + return None + 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 + + approach = self.gen_approach(closest_obst, self.vessel) + + if approach in [Approaches.FRONT, Approaches.RIGHT]: + buffer = math.pi / 6 # 30 degrees + new_heading = VO.right_angle - buffer + return self.create_guidance_data(self.vessel.speed, new_heading, self.vessel.heading, self.vessel_odom) + elif approach in [Approaches.BEHIND, Approaches.LEFT]: + return None + return None + + def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom): + data = GuidanceData() + data.u_d = speed + data.psi_d = psi_d + data.u = speed # Assuming this is the desired speed + data.t = self.get_clock().now().to_msg() + 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) + data.psi = yaw + return data + + def gen_approach(self, obstacle: Obstacle, vessel: Obstacle): + dx = obstacle.x - vessel.x + dy = obstacle.y - vessel.y + buffer = 10 * math.pi / 180 # 10 degrees in radians + phi = math.atan2(dy, dx) + + if vessel.heading + buffer > phi > vessel.heading - buffer: + return Approaches.FRONT + elif phi < vessel.heading - buffer: + return Approaches.RIGHT + elif phi > vessel.heading + buffer: + return Approaches.LEFT + return Approaches.BEHIND + + def get_zone(self, obstacle: Obstacle, vessel: Obstacle): + distance = self.get_distance(obstacle, vessel) + if distance > self.colimm_max_radius: + return Zones.NOCOL + elif self.stop_zone_radius < distance <= self.colimm_max_radius: + return Zones.COLIMM + return Zones.STOPNOW + + + +if __name__ == '__main__': + rclpy.init() + colav_controller = ColavController() + rclpy.spin(colav_controller) + rclpy.shutdown() \ No newline at end of file From 9c6fac9c0eb3bc8a27a1909c5fe40450672c251f Mon Sep 17 00:00:00 2001 From: Alice Date: Wed, 3 Apr 2024 19:45:21 +0200 Subject: [PATCH 03/10] feat: changed the parameters for colav controller --- motion/colav/launch/colav_controller.launch.py | 3 +++ motion/colav/scripts/colav_controller.py | 18 ++++++++++-------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/motion/colav/launch/colav_controller.launch.py b/motion/colav/launch/colav_controller.launch.py index 3c1d7819..73f1e5bb 100644 --- a/motion/colav/launch/colav_controller.launch.py +++ b/motion/colav/launch/colav_controller.launch.py @@ -10,6 +10,9 @@ def generate_launch_description(): package='colav', executable='colav_controller.py', name='colav', + parameters=[ + {'guidance_interface/colav_data': 'guidance/collision_avoidance'} + ], output='screen' ) diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index b7725261..4cd83907 100644 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -14,12 +14,12 @@ class ColavController(Node): def __init__(self): super().__init__("colav_controller") - - self.declare_parameters(namespace='', parameters=[ - ('guidance_interface_colav_data_topic', '/guidance/colav_data'), - ('stop_zone_radius', 0.0), - ('colimm_max_radius', float('inf')), - ]) + self.declare_parameter('guidance_interface/colav_data_topic', 'guidance/collision_avoidance') # Provide a default topic here + self.declare_parameter('stop_zone_radius', 0.0) + self.declare_parameter('colimm_max_radius', math.inf) + + stop_zone_radius = self.get_parameter('stop_zone_radius').get_parameter_value().double_value + colimm_max_radius = self.get_parameter('colimm_max_radius').get_parameter_value().double_value self.obstacle_sub = self.create_subscription(OdometryArray, "/tracking/mul_tracked_cv_objects", self.obst_callback, 10) self.vessel_sub = self.create_subscription(Odometry, "/pose_gt", self.vessel_callback, 10) @@ -28,8 +28,10 @@ def __init__(self): self.obstacles = [] self.vessel_odom = Odometry() self.vessel = Obstacle() - self.stop_zone_radius = self.get_parameter('stop_zone_radius').value - self.colimm_max_radius = self.get_parameter('colimm_max_radius').value + self.vessel.radius = stop_zone_radius + self.stop_zone_radius = stop_zone_radius + self.colimm_max_radius = colimm_max_radius + self.current_time = self.get_clock().now() def vessel_callback(self, msg): self.vessel_odom = msg From c1cf1bffa485791e160d637e0b6b6eb8de3b0007 Mon Sep 17 00:00:00 2001 From: Alice Date: Sun, 7 Apr 2024 13:45:38 +0200 Subject: [PATCH 04/10] feat(#128): fully translated the code to ros2 humble --- motion/colav/CMakeLists.txt | 1 + .../colav/launch/colav_controller.launch.py | 3 --- motion/colav/package.xml | 1 + motion/colav/scripts/colav_controller.py | 20 ++++++++++--------- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/motion/colav/CMakeLists.txt b/motion/colav/CMakeLists.txt index c082fc21..ce44ac87 100644 --- a/motion/colav/CMakeLists.txt +++ b/motion/colav/CMakeLists.txt @@ -21,6 +21,7 @@ install(DIRECTORY install(PROGRAMS scripts/colav_controller.py + scripts/VO_math.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/motion/colav/launch/colav_controller.launch.py b/motion/colav/launch/colav_controller.launch.py index 73f1e5bb..3c1d7819 100644 --- a/motion/colav/launch/colav_controller.launch.py +++ b/motion/colav/launch/colav_controller.launch.py @@ -10,9 +10,6 @@ def generate_launch_description(): package='colav', executable='colav_controller.py', name='colav', - parameters=[ - {'guidance_interface/colav_data': 'guidance/collision_avoidance'} - ], output='screen' ) diff --git a/motion/colav/package.xml b/motion/colav/package.xml index 8997e8c0..c1321504 100644 --- a/motion/colav/package.xml +++ b/motion/colav/package.xml @@ -8,6 +8,7 @@ MIT ament_cmake + nav_msgs rclpy geometry_msgs diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index 4cd83907..e0886ae0 100644 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -5,7 +5,7 @@ from nav_msgs.msg import Odometry from vortex_msgs.msg import GuidanceData, OdometryArray import math -from scripts.VO_math import VelocityObstacle, Obstacle, Zones, Approaches +from VO_math import VelocityObstacle, Obstacle, Zones, Approaches from tf2_ros import Buffer, TransformListener from transformations import euler_from_quaternion from geometry_msgs.msg import Quaternion @@ -14,16 +14,17 @@ class ColavController(Node): def __init__(self): super().__init__("colav_controller") - self.declare_parameter('guidance_interface/colav_data_topic', 'guidance/collision_avoidance') # Provide a default topic here + 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) - stop_zone_radius = self.get_parameter('stop_zone_radius').get_parameter_value().double_value - colimm_max_radius = self.get_parameter('colimm_max_radius').get_parameter_value().double_value + stop_zone_radius = self.get_parameter('stop_zone_radius').value + colimm_max_radius = self.get_parameter('colimm_max_radius').value + colav_data_topic = self.get_parameter('guidance_interface/colav_data_topic').get_parameter_value().string_value self.obstacle_sub = self.create_subscription(OdometryArray, "/tracking/mul_tracked_cv_objects", self.obst_callback, 10) self.vessel_sub = self.create_subscription(Odometry, "/pose_gt", self.vessel_callback, 10) - self.colav_pub = self.create_publisher(GuidanceData, self.get_parameter("/guidance_interface/colav_data").get_parameter_value().string_value, 10) + self.colav_pub = self.create_publisher(GuidanceData, colav_data_topic, 10) self.obstacles = [] self.vessel_odom = Odometry() @@ -147,10 +148,11 @@ def get_zone(self, obstacle: Obstacle, vessel: Obstacle): return Zones.COLIMM return Zones.STOPNOW - - -if __name__ == '__main__': +def main(args=None): rclpy.init() colav_controller = ColavController() rclpy.spin(colav_controller) - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 2c34ca369a83d8178d213c0f96279ca691bba3f3 Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 8 Apr 2024 19:20:28 +0200 Subject: [PATCH 05/10] feat(#128): one can add obstacles now --- motion/colav/scripts/VO_math.py | 12 ++++--- motion/colav/scripts/colav_controller.py | 45 +++++++++++------------- 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/motion/colav/scripts/VO_math.py b/motion/colav/scripts/VO_math.py index 5121668d..c0d13ac8 100644 --- a/motion/colav/scripts/VO_math.py +++ b/motion/colav/scripts/VO_math.py @@ -61,10 +61,14 @@ def set_cone_angles(self) -> None: theta_ro = math.atan2(self.obstacle.y - self.vessel.y, self.obstacle.x - self.vessel.x) print("ob", self.vessel.r, self.obstacle.r) - theta_ray = math.asin( - (self.vessel.r + self.obstacle.r) / - (math.sqrt((self.obstacle.x - self.vessel.x)**2 + - (self.obstacle.y - self.vessel.y)**2))) + distance = math.sqrt((self.obstacle.x - self.vessel.x) ** 2 + (self.obstacle.y - self.vessel.y) ** 2) + if distance == 0: + # Handle the case where distance is zero + # This could be setting theta_ray to some default value or skipping calculations + return + else: + theta_ray = math.asin((self.vessel.r + self.obstacle.r) / distance) + self.right_angle = theta_ro - theta_ray self.left_angle = theta_ro + theta_ray diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index e0886ae0..d57733c5 100644 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -9,6 +9,7 @@ from tf2_ros import Buffer, TransformListener from transformations import euler_from_quaternion from geometry_msgs.msg import Quaternion +import numpy as np class ColavController(Node): def __init__(self): @@ -40,7 +41,7 @@ def vessel_callback(self, msg): self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9 def obst_callback(self, msg): - self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odometry_array] + self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odoms] if not self.obstacles: self.get_logger().info('No obstacles detected!') return @@ -53,24 +54,20 @@ def quaternion_to_euler(quaternion: Quaternion): q = (quaternion.x, quaternion.y, quaternion.z, quaternion.w) return euler_from_quaternion(q) - def odometry_to_obstacle(self, odometry: Odometry): - # Convert Odometry message to an Obstacle object - # Assuming Obstacle class exists and is properly defined elsewhere - quaternion = ( - odometry.pose.pose.orientation.x, - odometry.pose.pose.orientation.y, - odometry.pose.pose.orientation.z, - odometry.pose.pose.orientation.w) - _, _, yaw = euler_from_quaternion(quaternion) + def odometry_to_obstacle(self, odom: Odometry) -> Obstacle: + obstacle = Obstacle() + obstacle.x = odom.pose.pose.position.x + obstacle.y = odom.pose.pose.position.y + obstacle.vx = odom.twist.twist.linear.x + obstacle.vy = odom.twist.twist.linear.y + obstacle.heading = np.arctan2(obstacle.vy, obstacle.vx) # Assuming heading is direction of velocity + obstacle.speed = np.sqrt(obstacle.vx**2 + obstacle.vy**2) - return Obstacle( - x=odometry.pose.pose.position.x, - y=odometry.pose.pose.position.y, - vx=odometry.twist.twist.linear.x, - vy=odometry.twist.twist.linear.y, - heading=yaw, - speed=math.hypot(odometry.twist.twist.linear.x, odometry.twist.twist.linear.y), - radius=2) # Assuming a fixed radius for simplicity + # Assuming 'r' (radius) needs to be calculated or set here. You might have a different way to determine it. + # obstacle.r = + + return obstacle + def get_distance(self, obstacle1: Obstacle, obstacle2: Obstacle): return math.hypot(obstacle1.x - obstacle2.x, obstacle1.y - obstacle2.y) @@ -79,7 +76,7 @@ def get_closest_obst(self, obstacles: list[Obstacle], vessel: Obstacle): return min(obstacles, key=lambda obs: self.get_distance(obs, vessel), default=None) def gen_colav_data(self): - closest_obst = self.get_closest_obst(self.obstacles) + closest_obst = self.get_closest_obst(self.obstacles, self.vessel) if closest_obst is None: self.get_logger().info('No obstacles detected.') return None @@ -113,17 +110,17 @@ def gen_colav_data(self): def create_guidance_data(self, speed, psi_d, vessel_heading, vessel_odom): data = GuidanceData() - data.u_d = speed - data.psi_d = psi_d - data.u = speed # Assuming this is the desired speed - data.t = self.get_clock().now().to_msg() + 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) - data.psi = yaw + data.psi = float(yaw) return data def gen_approach(self, obstacle: Obstacle, vessel: Obstacle): From 83f0329432356f010717d5ec39b231cfdd334591 Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 8 Apr 2024 19:49:48 +0200 Subject: [PATCH 06/10] fix(#128): changed a function --- motion/colav/scripts/colav_controller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index d57733c5..f0e7b3f0 100644 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -49,6 +49,7 @@ def obst_callback(self, msg): if colav_data: self.colav_pub.publish(colav_data) + @staticmethod def quaternion_to_euler(quaternion: Quaternion): """Convert a ROS Quaternion message to Euler angles (roll, pitch, yaw).""" q = (quaternion.x, quaternion.y, quaternion.z, quaternion.w) From e046fb24932e53edb1c37fee2b03294559245970 Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 15 Apr 2024 19:13:38 +0200 Subject: [PATCH 07/10] feat(#128): started on plotting --- motion/colav/CMakeLists.txt | 1 + motion/colav/scripts/VO_visualize.py | 125 +++++++++++++++++++++++ motion/colav/scripts/colav_controller.py | 31 ++++++ 3 files changed, 157 insertions(+) create mode 100644 motion/colav/scripts/VO_visualize.py mode change 100644 => 100755 motion/colav/scripts/colav_controller.py diff --git a/motion/colav/CMakeLists.txt b/motion/colav/CMakeLists.txt index ce44ac87..46d5b5d4 100644 --- a/motion/colav/CMakeLists.txt +++ b/motion/colav/CMakeLists.txt @@ -22,6 +22,7 @@ install(DIRECTORY install(PROGRAMS scripts/colav_controller.py scripts/VO_math.py + scripts/VO_visualize.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/motion/colav/scripts/VO_visualize.py b/motion/colav/scripts/VO_visualize.py new file mode 100644 index 00000000..3cd36015 --- /dev/null +++ b/motion/colav/scripts/VO_visualize.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +import numpy as np +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from vortex_msgs.msg import GuidanceData, OdometryArray +from matplotlib.patches import Circle, Arrow, Wedge + +# Assuming VO_math.py and your defined classes are imported correctly +from enum import Enum +import math + +class Zones(Enum): + NOCOL = 1 + COLIMM = 2 + STOPNOW = 3 + +class Approaches(Enum): + FRONT = 1 + RIGHT = 2 + LEFT = 3 + BEHIND = 4 + +class Obstacle: + def __init__(self) -> None: + self.vx = 0 + self.vy = 0 + self.r = 2 # Example radius + self.x = 0 + self.y = 0 + self.heading = 0 + self.speed = 0 + +class VelocityObstacle: + def __init__(self, vessel: Obstacle, obstacle: Obstacle) -> None: + self.vessel = vessel + self.obstacle = obstacle + self.left_angle = 0.0 + self.right_angle = 0.0 + self.truncated_time = 5 # placeholder + + def set_cone_angles(self) -> None: + theta_ro = math.atan2(self.obstacle.y - self.vessel.y, self.obstacle.x - self.vessel.x) + distance = math.sqrt((self.obstacle.x - self.vessel.x) ** 2 + (self.obstacle.y - self.vessel.y) ** 2) + if distance == 0: + return + theta_ray = math.asin((self.vessel.r + self.obstacle.r) / distance) + self.right_angle = theta_ro - theta_ray + self.left_angle = theta_ro + theta_ray + + def check_if_collision(self) -> bool: + dvx = self.obstacle.vx - self.vessel.vx + dvy = self.obstacle.vy - self.vessel.vy + angle = math.atan2(-dvy, -dvx) + return angle > self.right_angle and angle < self.left_angle + +class PlotterNode(Node): + def __init__(self): + super().__init__('plotter_node') + self.fig, self.ax = plt.subplots() + self.vessel_pos, = self.ax.plot([], [], 'ro', label='Vessel') # Red dot for vessel + self.obstacle_pos, = self.ax.plot([], [], 'go', label='Obstacles') # Green dots for obstacles + self.collision_zones = [] + + self.ax.axis('equal') + self.ax.set_xlim(-50, 50) + self.ax.set_ylim(-50, 50) + self.ax.legend() + + self.vessel_subscription = self.create_subscription(Odometry, '/pose_gt', self.vessel_callback, 10) + self.obstacle_subscription = self.create_subscription(OdometryArray, '/tracking/mul_tracked_cv_objects', self.obstacle_callback, 10) + + self.vessel = Obstacle() + self.obstacles = [] + + def vessel_callback(self, msg): + self.vessel.x = msg.pose.pose.position.x + self.vessel.y = msg.pose.pose.position.y + self.vessel.vx = msg.twist.twist.linear.x + self.vessel.vy = msg.twist.twist.linear.y + + def obstacle_callback(self, msg): + self.obstacles = [Obstacle() for _ in msg.odoms] + for obs, odom in zip(self.obstacles, msg.odoms): + obs.x = odom.pose.pose.position.x + obs.y = odom.pose.pose.position.y + obs.vx = odom.twist.twist.linear.x + obs.vy = odom.twist.twist.linear.y + + def update_plot(self, frame): + self.vessel_pos.set_data(self.vessel.x, self.vessel.y) + self.obstacle_pos.set_data([obs.x for obs in self.obstacles], [obs.y for obs in self.obstacles]) + + # Clear previous collision zones + for patch in self.collision_zones: + patch.remove() + self.collision_zones = [] + + # Collision detection and path planning + for obstacle in self.obstacles: + vo = VelocityObstacle(self.vessel, obstacle) + vo.set_cone_angles() + if vo.check_if_collision(): + wedge = Wedge((self.vessel.x, self.vessel.y), 10, np.degrees(vo.right_angle), np.degrees(vo.left_angle), color='orange', alpha=0.5) + self.ax.add_patch(wedge) + self.collision_zones.append(wedge) + + return [self.vessel_pos, self.obstacle_pos] + self.collision_zones + + def start_animation(self): + ani = FuncAnimation(self.fig, self.update_plot, frames=100, interval=100, blit=False) + plt.show() + +def main(args=None): + rclpy.init(args=args) + plotter_node = PlotterNode() + rclpy.spin_once(plotter_node, timeout_sec=0.1) # Spin briefly to update positions + plotter_node.start_animation() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py old mode 100644 new mode 100755 index f0e7b3f0..38b8ef2b --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -11,10 +11,20 @@ from geometry_msgs.msg import Quaternion import numpy as np +import matplotlib +matplotlib.use('Qt5Agg') +import matplotlib.pyplot as plt + +plt.ion() # Turn interactive mode on +fig, ax = plt.subplots() # Pre-create the figure and axes + class ColavController(Node): + def __init__(self): super().__init__("colav_controller") + self.vessel_path = [] # To store vessel positions over time + 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) @@ -40,6 +50,9 @@ def vessel_callback(self, msg): self.vessel = self.odometry_to_obstacle(msg) self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9 + self.vessel_path.append((self.vessel.x, self.vessel.y)) + self.plot_positions() + def obst_callback(self, msg): self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odoms] if not self.obstacles: @@ -146,6 +159,24 @@ def get_zone(self, obstacle: Obstacle, vessel: Obstacle): return Zones.COLIMM return Zones.STOPNOW + def plot_positions(self): + ax.clear() # Clear previous drawings + if self.vessel_path: + path_x, path_y = zip(*self.vessel_path) + ax.plot(path_x, path_y, 'b-', label='Vessel Path') + ax.plot(self.vessel.x, self.vessel.y, 'bo', label='Vessel Current') + for obstacle in self.obstacles: + ax.plot(obstacle.x, obstacle.y, 'ro', label='Obstacle') + ax.set_xlabel('X position') + ax.set_ylabel('Y position') + ax.legend() + ax.grid(True) + plt.title('Vessel and Obstacles Position and Path') + fig.canvas.draw_idle() # Redraw the current figure + fig.canvas.flush_events() # Process events like key presses or resizes + + + def main(args=None): rclpy.init() colav_controller = ColavController() From dabe7f12b22d1124ab1fff8021a94b67225c2a8a Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 15 Apr 2024 19:20:18 +0200 Subject: [PATCH 08/10] feat(#128): removed unneeded code --- motion/colav/scripts/colav_controller.py | 29 ------------------------ 1 file changed, 29 deletions(-) diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index 38b8ef2b..f1738eb3 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -10,21 +10,12 @@ from transformations import euler_from_quaternion from geometry_msgs.msg import Quaternion import numpy as np - -import matplotlib -matplotlib.use('Qt5Agg') import matplotlib.pyplot as plt -plt.ion() # Turn interactive mode on -fig, ax = plt.subplots() # Pre-create the figure and axes - class ColavController(Node): - def __init__(self): super().__init__("colav_controller") - self.vessel_path = [] # To store vessel positions over time - 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) @@ -50,9 +41,6 @@ def vessel_callback(self, msg): self.vessel = self.odometry_to_obstacle(msg) self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9 - self.vessel_path.append((self.vessel.x, self.vessel.y)) - self.plot_positions() - def obst_callback(self, msg): self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odoms] if not self.obstacles: @@ -159,23 +147,6 @@ def get_zone(self, obstacle: Obstacle, vessel: Obstacle): return Zones.COLIMM return Zones.STOPNOW - def plot_positions(self): - ax.clear() # Clear previous drawings - if self.vessel_path: - path_x, path_y = zip(*self.vessel_path) - ax.plot(path_x, path_y, 'b-', label='Vessel Path') - ax.plot(self.vessel.x, self.vessel.y, 'bo', label='Vessel Current') - for obstacle in self.obstacles: - ax.plot(obstacle.x, obstacle.y, 'ro', label='Obstacle') - ax.set_xlabel('X position') - ax.set_ylabel('Y position') - ax.legend() - ax.grid(True) - plt.title('Vessel and Obstacles Position and Path') - fig.canvas.draw_idle() # Redraw the current figure - fig.canvas.flush_events() # Process events like key presses or resizes - - def main(args=None): rclpy.init() From db724e0d62cb458b68340276bd24c3c31990ea5c Mon Sep 17 00:00:00 2001 From: Alice Date: Mon, 15 Apr 2024 20:13:55 +0200 Subject: [PATCH 09/10] feat(#128): changed odometryarray to odometry --- motion/colav/scripts/colav_controller.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/motion/colav/scripts/colav_controller.py b/motion/colav/scripts/colav_controller.py index f1738eb3..cff90df0 100755 --- a/motion/colav/scripts/colav_controller.py +++ b/motion/colav/scripts/colav_controller.py @@ -24,7 +24,7 @@ def __init__(self): colimm_max_radius = self.get_parameter('colimm_max_radius').value colav_data_topic = self.get_parameter('guidance_interface/colav_data_topic').get_parameter_value().string_value - self.obstacle_sub = self.create_subscription(OdometryArray, "/tracking/mul_tracked_cv_objects", self.obst_callback, 10) + self.obstacle_sub = self.create_subscription(Odometry, "/obstacle", self.obst_callback, 10) self.vessel_sub = self.create_subscription(Odometry, "/pose_gt", self.vessel_callback, 10) self.colav_pub = self.create_publisher(GuidanceData, colav_data_topic, 10) @@ -42,7 +42,7 @@ def vessel_callback(self, msg): self.t = msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9 def obst_callback(self, msg): - self.obstacles = [self.odometry_to_obstacle(odom) for odom in msg.odoms] + self.obstacles = [self.odometry_to_obstacle(msg)] if not self.obstacles: self.get_logger().info('No obstacles detected!') return @@ -86,9 +86,9 @@ def gen_colav_data(self): VO = VelocityObstacle(self.vessel, closest_obst) VO.set_cone_angles() - self.get_logger().info(f'Closest obstacle at {closest_obst.x}, {closest_obst.y}') - self.get_logger().info(f'Vessel at {self.vessel.x}, {self.vessel.y}') - self.get_logger().info(f'VO Angles: Left {VO.left_angle}, Right {VO.right_angle}') + # self.get_logger().info(f'Closest obstacle at {closest_obst.x}, {closest_obst.y}') + # self.get_logger().info(f'Vessel at {self.vessel.x}, {self.vessel.y}') + # self.get_logger().info(f'VO Angles: Left {VO.left_angle}, Right {VO.right_angle}') zone = self.get_zone(closest_obst, self.vessel) self.get_logger().info(f'Zone: {zone}') From e3ea783e79b01d2cb546aa599bbe5ebc094b0a3b Mon Sep 17 00:00:00 2001 From: Andeshog <157955984+Andeshog@users.noreply.github.com> Date: Sun, 21 Apr 2024 14:15:30 +0200 Subject: [PATCH 10/10] Current --- motion/colav/CMakeLists.txt | 2 + motion/colav/scripts/colav_controller.py | 32 ++++--- motion/colav/scripts/colav_sim.py | 111 +++++++++++++++++++++++ motion/colav/scripts/cone_visualizer.py | 42 +++++++++ 4 files changed, 176 insertions(+), 11 deletions(-) create mode 100755 motion/colav/scripts/colav_sim.py create mode 100644 motion/colav/scripts/cone_visualizer.py 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