From 9ad0f0655e65272c664143e36af0db609c8b4ec5 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Fri, 31 Jan 2025 15:56:08 -0300 Subject: [PATCH 01/49] Add ObstacleMemoryNode to manage and publish obstacle positions --- .../gpg_fproject/obstacle_memory.py | 67 +++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 src/gpg_fproject/gpg_fproject/obstacle_memory.py diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py new file mode 100644 index 0000000..e4fdee1 --- /dev/null +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -0,0 +1,67 @@ +import cv2 +import rclpy +import numpy as np + +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError +from geometry_msgs.msg import PointStamped, Vector3Stamped +from tf2_geometry_msgs import do_transform_point + + +class ObstacleMemoryNode(Node): + def __init__(self): + super().__init__('obstacle_memory_node') + self.create_subscription(PointStamped, '/obstacle_position', self.obstacle_callback, 10) + self.publisher_ = self.create_publisher(PointStamped, '/obstacle_memory', 10) + self.obstacle_memory = None + self.obstacle_memory_list = [] + + def obstacle_callback(self, msg): + """ + Callback function for the obstacle topic + """ + self.obstacle_memory = msg + self.publish_obstacle_memory() + + # Save into a list memory all the coordinates of the obstacles sended by the obstacle_node over the topic /obstacle_position + # /obstacle_position is a PointStamped message that contains the x, y, z coordinates of the obstacle + def save_obstacle_memory(self): + """ + Save the obstacle memory + """ + if self.obstacle_memory is not None: + obstacle_dict = { + 'X': self.obstacle_memory.point.x, + 'Y': self.obstacle_memory.point.y, + 'Z': self.obstacle_memory.point.z + } + self.obstacle_memory_list.append(obstacle_dict) + return self.obstacle_memory_list + + def publish_obstacle_memory(self): + """ + Publish the obstacle memory + """ + self.obstacle_memory_list = self.save_obstacle_memory() + for obstacle in self.obstacle_memory_list: + obstacle_msg = PointStamped() + obstacle_msg.header.stamp = self.get_clock().now().to_msg() + obstacle_msg.point.x = obstacle['X'] + obstacle_msg.point.y = obstacle['Y'] + obstacle_msg.point.z = obstacle['Z'] + self.publisher_.publish(obstacle_msg) + +def main(args=None): + rclpy.init(args=args) + + obstacle_memory_node = ObstacleMemoryNode() + + rclpy.spin(obstacle_memory_node) + + obstacle_memory_node.destroy_node() + rclpy.shutdown() + + + + From ebc199bae3cb03af88bff220cd49f90f4fa9f6ee Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 15:53:55 -0300 Subject: [PATCH 02/49] Refactor ObstacleMemoryNode to store and publish obstacle coordinates; replace PointStamped with UnboundedFloat for memory management --- .../gpg_fproject/obstacle_memory.py | 112 +++++++++++++----- 1 file changed, 82 insertions(+), 30 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index e4fdee1..7c853f5 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -7,60 +7,112 @@ from cv_bridge import CvBridge, CvBridgeError from geometry_msgs.msg import PointStamped, Vector3Stamped from tf2_geometry_msgs import do_transform_point +from custom_msg_fproject.msg import UnboundedFloat class ObstacleMemoryNode(Node): def __init__(self): super().__init__('obstacle_memory_node') self.create_subscription(PointStamped, '/obstacle_position', self.obstacle_callback, 10) - self.publisher_ = self.create_publisher(PointStamped, '/obstacle_memory', 10) - self.obstacle_memory = None - self.obstacle_memory_list = [] + self.memory_publisher = self.create_publisher(UnboundedFloat, '/obstacle_memory', 10) + self.publishers = {} + + self.memory_size = 5 + self.x_coords, self.y_coords, self.z_coords = [], [], [] def obstacle_callback(self, msg): """ Callback function for the obstacle topic """ - self.obstacle_memory = msg + self.x_coords.append(float(msg.point.x)) + self.y_coords.append(float(msg.point.y)) + self.z_coords.append(float(msg.point.z)) + + if len(self.x_coords) > self.memory_size: + self.x_coords.pop(0) + self.y_coords.pop(0) + self.z_coords.pop(0) + + self.get_logger().info('Obstacle position received in the obstacle memory node') self.publish_obstacle_memory() - # Save into a list memory all the coordinates of the obstacles sended by the obstacle_node over the topic /obstacle_position - # /obstacle_position is a PointStamped message that contains the x, y, z coordinates of the obstacle - def save_obstacle_memory(self): - """ - Save the obstacle memory - """ - if self.obstacle_memory is not None: - obstacle_dict = { - 'X': self.obstacle_memory.point.x, - 'Y': self.obstacle_memory.point.y, - 'Z': self.obstacle_memory.point.z - } - self.obstacle_memory_list.append(obstacle_dict) - return self.obstacle_memory_list - def publish_obstacle_memory(self): """ Publish the obstacle memory """ - self.obstacle_memory_list = self.save_obstacle_memory() - for obstacle in self.obstacle_memory_list: - obstacle_msg = PointStamped() - obstacle_msg.header.stamp = self.get_clock().now().to_msg() - obstacle_msg.point.x = obstacle['X'] - obstacle_msg.point.y = obstacle['Y'] - obstacle_msg.point.z = obstacle['Z'] - self.publisher_.publish(obstacle_msg) + obstacle_msg = UnboundedFloat() + obstacle_msg.header.stamp = self.get_clock().now().to_msg() + obstacle_msg.float_storage = np.array([self.x_coords, self.y_coords, self.z_coords]).T.tolist() # convert matrix to list of lists with each list containing x, y, z coordinates + self.memory_publisher.publish(obstacle_msg) + + for i, obs_coords in enumerate(obstacle_msg.float_storage): # obs_coords is a 1D array of x, y, z coordinates + topic_name = f'/obstacle_position_{i}' + if topic_name not in self.publishers: + self.publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) + + PS = PointStamped() + PS.header.stamp = self.get_clock().now().to_msg() + PS.point.x, PS.point.y, PS.point.z = obs_coords + + try: + self.publishers[topic_name].publish(PS) + except Exception as e: + self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') def main(args=None): rclpy.init(args=args) - obstacle_memory_node = ObstacleMemoryNode() - rclpy.spin(obstacle_memory_node) - obstacle_memory_node.destroy_node() rclpy.shutdown() + + + + + + + + + + + + + + + + + + + + + + # def save_obstacle_memory(self): + # """ + # Save the obstacle memory + # """ + # if self.obstacle_memory is not None: + # obstacle_dict = { + # 'X': self.obstacle_memory.point.x, + # 'Y': self.obstacle_memory.point.y, + # 'Z': self.obstacle_memory.point.z + # } + # self.obstacle_memory_list.append(obstacle_dict) + # return self.obstacle_memory_list + + # def publish_obstacle_memory(self): + # """ + # Publish the obstacle memory + # """ + # self.obstacle_memory_list = self.save_obstacle_memory() + # for obstacle in self.obstacle_memory_list: + # obstacle_msg = PointStamped() + # obstacle_msg.header.stamp = self.get_clock().now().to_msg() + # obstacle_msg.point.x = obstacle['X'] + # obstacle_msg.point.y = obstacle['Y'] + # obstacle_msg.point.z = obstacle['Z'] + # self.publisher_.publish(obstacle_msg) + + From 258626dc8d78972f02639548d6a6a9719243174b Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 15:54:38 -0300 Subject: [PATCH 03/49] Add UnboundedFloat message definition for flexible float array storage --- src/custom_msg_fproject/msg/UnboundedFloat.msg | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 src/custom_msg_fproject/msg/UnboundedFloat.msg diff --git a/src/custom_msg_fproject/msg/UnboundedFloat.msg b/src/custom_msg_fproject/msg/UnboundedFloat.msg new file mode 100644 index 0000000..6055c25 --- /dev/null +++ b/src/custom_msg_fproject/msg/UnboundedFloat.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float32[] float_storage \ No newline at end of file From d4d1fcbd6c502b4a18e9c3be31623e8fa34eb24b Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 15:54:46 -0300 Subject: [PATCH 04/49] Add UnboundedInter message definition for dynamic integer array storage --- src/custom_msg_fproject/msg/UnboundedInter.msg | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/custom_msg_fproject/msg/UnboundedInter.msg diff --git a/src/custom_msg_fproject/msg/UnboundedInter.msg b/src/custom_msg_fproject/msg/UnboundedInter.msg new file mode 100644 index 0000000..7ed12d8 --- /dev/null +++ b/src/custom_msg_fproject/msg/UnboundedInter.msg @@ -0,0 +1 @@ +int32[] storage \ No newline at end of file From 193d75a38be029482ce28912350bd780d1a98763 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 15:55:01 -0300 Subject: [PATCH 05/49] Add CMakeLists.txt for custom message project with UnboundedInter and UnboundedFloat message definitions --- src/custom_msg_fproject/CMakeLists.txt | 32 ++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/custom_msg_fproject/CMakeLists.txt diff --git a/src/custom_msg_fproject/CMakeLists.txt b/src/custom_msg_fproject/CMakeLists.txt new file mode 100644 index 0000000..bbc0445 --- /dev/null +++ b/src/custom_msg_fproject/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(custom_msg_fproject) + +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(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/UnboundedInter.msg" + "msg/UnboundedFloat.msg" + DEPENDENCIES std_msgs +) +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() From f9ef2ffd28a4845d40fa23eef11f8b26b4af211f Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 15:55:12 -0300 Subject: [PATCH 06/49] Add package.xml for custom message project configuration --- src/custom_msg_fproject/package.xml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 src/custom_msg_fproject/package.xml diff --git a/src/custom_msg_fproject/package.xml b/src/custom_msg_fproject/package.xml new file mode 100644 index 0000000..422a496 --- /dev/null +++ b/src/custom_msg_fproject/package.xml @@ -0,0 +1,21 @@ + + + + custom_msg_fproject + 0.0.0 + TODO: Package description + root + Apache-2.0 + + ament_cmake + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_lint_auto + ament_lint_common + + + ament_cmake + + From d2c265db92300b669408b6d6c940a19704e5db5c Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Sat, 1 Feb 2025 16:00:57 -0300 Subject: [PATCH 07/49] Remove unused imports in obstacle_memory.py to streamline code --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 7c853f5..0f3e34f 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -1,15 +1,9 @@ -import cv2 import rclpy import numpy as np - from rclpy.node import Node -from sensor_msgs.msg import Image -from cv_bridge import CvBridge, CvBridgeError -from geometry_msgs.msg import PointStamped, Vector3Stamped -from tf2_geometry_msgs import do_transform_point +from geometry_msgs.msg import PointStamped from custom_msg_fproject.msg import UnboundedFloat - class ObstacleMemoryNode(Node): def __init__(self): super().__init__('obstacle_memory_node') From 8131f58881c96847617275f038854093c5500021 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:18:58 -0300 Subject: [PATCH 08/49] Add duplicate obstacle verification in obstacle_memory.py --- .../gpg_fproject/obstacle_memory.py | 45 ++++++++++++++++--- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 0f3e34f..b153664 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -12,15 +12,21 @@ def __init__(self): self.publishers = {} self.memory_size = 5 + self.duplicate_threshold = 0.05 self.x_coords, self.y_coords, self.z_coords = [], [], [] def obstacle_callback(self, msg): """ Callback function for the obstacle topic """ - self.x_coords.append(float(msg.point.x)) - self.y_coords.append(float(msg.point.y)) - self.z_coords.append(float(msg.point.z)) + x_get_time_step, y_get_time_step, z_get_time_step = float(msg.point.x), float(msg.point.y), float(msg.point.z) + if self.verify_duplicated_obstacle(x_get_time_step, y_get_time_step, z_get_time_step): + self.get_logger().info('Obstacle is duplicated in the obstacle memory node') + return + + self.x_coords.append(x_get_time_step) + self.y_coords.append(y_get_time_step) + self.z_coords.append(z_get_time_step) if len(self.x_coords) > self.memory_size: self.x_coords.pop(0) @@ -30,16 +36,29 @@ def obstacle_callback(self, msg): self.get_logger().info('Obstacle position received in the obstacle memory node') self.publish_obstacle_memory() + def verify_duplicated_obstacle(self, x_get_time_step, y_get_time_step, z_get_time_step): + """ + Verify if the obstacle is already in the memory + """ + if not self.x_coords: + return False + + distance_eucluiden = np.sqrt((x_get_time_step - self.x_coords)**2 + + (y_get_time_step - self.y_coords)**2 + + (z_get_time_step - self.z_coords)**2) + return np.any(distance_eucluiden < self.duplicate_threshold) + def publish_obstacle_memory(self): """ Publish the obstacle memory """ obstacle_msg = UnboundedFloat() obstacle_msg.header.stamp = self.get_clock().now().to_msg() - obstacle_msg.float_storage = np.array([self.x_coords, self.y_coords, self.z_coords]).T.tolist() # convert matrix to list of lists with each list containing x, y, z coordinates + obstacle_msg.float_storage = [val for sublist in zip(self.x_coords, self.y_coords, self.z_coords) for val in sublist] self.memory_publisher.publish(obstacle_msg) - for i, obs_coords in enumerate(obstacle_msg.float_storage): # obs_coords is a 1D array of x, y, z coordinates + arraye_format = np.array(obstacle_msg.float_storage).reshape(-1, 3) + for i, obs_coords in enumerate(arraye_format): topic_name = f'/obstacle_position_{i}' if topic_name not in self.publishers: self.publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) @@ -51,7 +70,7 @@ def publish_obstacle_memory(self): try: self.publishers[topic_name].publish(PS) except Exception as e: - self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') + self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') def main(args=None): rclpy.init(args=args) @@ -62,7 +81,19 @@ def main(args=None): - +# for i, obs_coords in enumerate(obstacle_msg.float_storage[:3]): + # topic_name = f'/obstacle_position_{i}' + # if topic_name not in self.publishers: + # self.publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) + + # PS = PointStamped() + # PS.header.stamp = self.get_clock().now().to_msg() + # PS.point.x, PS.point.y, PS.point.z = obs_coords + + # try: + # self.publishers[topic_name].publish(PS) + # except Exception as e: + # self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') From f0c657f5802e66efac6c93bca125c60be0a5d84b Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:19:15 -0300 Subject: [PATCH 09/49] Remove unused import of get_hsv_value_based_on_click in image_node.py --- src/gpg_fproject/gpg_fproject/image_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 49ad28f..2c21e26 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -11,7 +11,6 @@ from gpg_fproject.utils import get_shape from geometry_msgs.msg import PointStamped, Vector3Stamped from tf2_geometry_msgs import do_transform_point -from gpg_fproject.utils import get_hsv_value_based_on_click class ImageNode(Node): def __init__(self): From 4779c76300deba07857e01636f7284357ddaadb2 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:20:03 -0300 Subject: [PATCH 10/49] Refactor obstacle detection logic in obstacle_node.py for improved clarity and performance --- .../gpg_fproject/obstacle_node.py | 23 ++++++++----------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index da7a434..435acba 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -7,7 +7,7 @@ from rclpy.node import Node from sensor_msgs.msg import Image, CameraInfo from cv_bridge import CvBridge, CvBridgeError -from geometry_msgs.msg import PointStamped, Vector3Stamped, TwistStamped +from geometry_msgs.msg import PointStamped, Vector3Stamped from tf2_geometry_msgs import do_transform_point @@ -19,7 +19,6 @@ def __init__(self): self.create_subscription(CameraInfo, '/camera_info', self.camera_info_callback, 10) self.publisher_ = self.create_publisher(PointStamped, '/obstacle_position', 10) - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) self.bridge = CvBridge() @@ -47,7 +46,7 @@ def image_callback(self, msg): except CvBridgeError as e: self.get_logger().error(f'CvBridge Error: {e}') return - + def process_image(self, cv_image): """ Process the image to detect obstacles @@ -61,24 +60,20 @@ def process_image(self, cv_image): self.get_logger().info('No red color detected from the obstacle node') return - # find contours in the mask and initialize the current contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) if not contours: self.get_logger().info('No contours detected from the obstacle node') return - max_contour = max(contours, key=cv2.contourArea) - if contours: - M = cv2.moments(max_contour) - if M["m00"] != 0: - cX = int(M["m10"] / M["m00"]) - cY = int(M["m01"] / M["m00"]) - - bottom_point = tuple(max_contour[max_contour[:, :, 1].argmax()][0]) + for contour in contours: + M = cv2.moments(contour) + if M["m00"] == 0: + continue + cX = int(M["m10"] / M["m00"]) + cY = int(M["m01"] / M["m00"]) + bottom_point = tuple(contour[contour[:, :, 1].argmax()][0]) bottom_point = (cX, bottom_point[1]) self.publish_obstacle_position(cv_image, bottom_point, cX, cY) - #else: - #self.get_logger().info('No obstacle detected from the obstacle node') def publish_obstacle_position(self, cv_image, bottom_point, x, y): """ From 27217ab062cbb7bc320b9b678a48ed8b63232a6d Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:21:34 -0300 Subject: [PATCH 11/49] Remove commented-out code in obstacle_memory.py to enhance code readability --- .../gpg_fproject/obstacle_memory.py | 66 +------------------ 1 file changed, 1 insertion(+), 65 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index b153664..3a70c53 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -77,68 +77,4 @@ def main(args=None): obstacle_memory_node = ObstacleMemoryNode() rclpy.spin(obstacle_memory_node) obstacle_memory_node.destroy_node() - rclpy.shutdown() - - - -# for i, obs_coords in enumerate(obstacle_msg.float_storage[:3]): - # topic_name = f'/obstacle_position_{i}' - # if topic_name not in self.publishers: - # self.publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) - - # PS = PointStamped() - # PS.header.stamp = self.get_clock().now().to_msg() - # PS.point.x, PS.point.y, PS.point.z = obs_coords - - # try: - # self.publishers[topic_name].publish(PS) - # except Exception as e: - # self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') - - - - - - - - - - - - - - - - - - # def save_obstacle_memory(self): - # """ - # Save the obstacle memory - # """ - # if self.obstacle_memory is not None: - # obstacle_dict = { - # 'X': self.obstacle_memory.point.x, - # 'Y': self.obstacle_memory.point.y, - # 'Z': self.obstacle_memory.point.z - # } - # self.obstacle_memory_list.append(obstacle_dict) - # return self.obstacle_memory_list - - # def publish_obstacle_memory(self): - # """ - # Publish the obstacle memory - # """ - # self.obstacle_memory_list = self.save_obstacle_memory() - # for obstacle in self.obstacle_memory_list: - # obstacle_msg = PointStamped() - # obstacle_msg.header.stamp = self.get_clock().now().to_msg() - # obstacle_msg.point.x = obstacle['X'] - # obstacle_msg.point.y = obstacle['Y'] - # obstacle_msg.point.z = obstacle['Z'] - # self.publisher_.publish(obstacle_msg) - - - - - - + rclpy.shutdown() \ No newline at end of file From 1091d069f90b0aa97ef2668ad32478c22d75bfaa Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:27:54 -0300 Subject: [PATCH 12/49] Add obstacle_memory.py to package data in setup.py --- src/gpg_fproject/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/gpg_fproject/setup.py b/src/gpg_fproject/setup.py index 68c14d7..dcf62b7 100644 --- a/src/gpg_fproject/setup.py +++ b/src/gpg_fproject/setup.py @@ -16,6 +16,7 @@ ('share/' + package_name, ['gpg_fproject/robot.urdf']), ('share/' + package_name, ['gpg_fproject/image_node.py']), ('share/' + package_name, ['gpg_fproject/obstacle_node.py']), + ('share/' + package_name, ['gpg_fproject/obstacle_memory.py']), ('share/' + package_name, ['gpg_fproject/user_node.py']), ('share/' + package_name, ['gpg_fproject/utils.py']), ], From 899efe4d56f2288cfaf02b290b4f1f5c9d850675 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:28:03 -0300 Subject: [PATCH 13/49] Add obstacle_memory_node to launch description for improved functionality --- src/gpg_fproject/launch/gpg_fproject_launch.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/gpg_fproject/launch/gpg_fproject_launch.py b/src/gpg_fproject/launch/gpg_fproject_launch.py index 4be587b..d37551f 100644 --- a/src/gpg_fproject/launch/gpg_fproject_launch.py +++ b/src/gpg_fproject/launch/gpg_fproject_launch.py @@ -68,6 +68,12 @@ def generate_launch_description(): output='screen' ) + obstacle_memory_node = Node( + package='gpg_fproject', + executable='obstacle_memory_node', + output='screen' + ) + use_sim_time = LaunchConfiguration('use_sim_time', default='True') robot_publisher = Node( @@ -128,6 +134,7 @@ def generate_launch_description(): controller_node, reach_goal_node, obstacle_node, + obstacle_memory_node, image_bridge_node, robot_publisher, sim_node, From 412108edd00867d77edfb89cad71b3b8ca3bb663 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Tue, 4 Feb 2025 19:29:11 -0300 Subject: [PATCH 14/49] Add obstacle_memory_node entry point to setup.py for improved functionality --- src/gpg_fproject/setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gpg_fproject/setup.py b/src/gpg_fproject/setup.py index dcf62b7..8dd2039 100644 --- a/src/gpg_fproject/setup.py +++ b/src/gpg_fproject/setup.py @@ -30,7 +30,8 @@ entry_points={ 'console_scripts': [ 'image_node = gpg_fproject.image_node:main', 'user_node = gpg_fproject.user_node:main', - 'obstacle_node = gpg_fproject.obstacle_node:main' + 'obstacle_node = gpg_fproject.obstacle_node:main', + 'obstacle_memory_node = gpg_fproject.obstacle_memory_node:main' ], }, ) From dc4449ae11cd2345bda632b1b49a45dc7df0b665 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Tue, 11 Feb 2025 21:54:54 -0300 Subject: [PATCH 15/49] Refactor obstacle_memory.py to improve variable naming and enhance code clarity --- .../gpg_fproject/obstacle_memory.py | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 3a70c53..6348567 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -9,7 +9,7 @@ def __init__(self): super().__init__('obstacle_memory_node') self.create_subscription(PointStamped, '/obstacle_position', self.obstacle_callback, 10) self.memory_publisher = self.create_publisher(UnboundedFloat, '/obstacle_memory', 10) - self.publishers = {} + self.obstacle_publishers = {} self.memory_size = 5 self.duplicate_threshold = 0.05 @@ -43,10 +43,12 @@ def verify_duplicated_obstacle(self, x_get_time_step, y_get_time_step, z_get_tim if not self.x_coords: return False - distance_eucluiden = np.sqrt((x_get_time_step - self.x_coords)**2 + - (y_get_time_step - self.y_coords)**2 + - (z_get_time_step - self.z_coords)**2) - return np.any(distance_eucluiden < self.duplicate_threshold) + distance_euclideen = np.sqrt( + (x_get_time_step - np.array(self.x_coords))**2 + + (y_get_time_step - np.array(self.y_coords))**2 + + (z_get_time_step - np.array(self.z_coords))**2 + ) + return np.any(distance_euclideen < self.duplicate_threshold) def publish_obstacle_memory(self): """ @@ -57,18 +59,24 @@ def publish_obstacle_memory(self): obstacle_msg.float_storage = [val for sublist in zip(self.x_coords, self.y_coords, self.z_coords) for val in sublist] self.memory_publisher.publish(obstacle_msg) - arraye_format = np.array(obstacle_msg.float_storage).reshape(-1, 3) + float_array = np.array(obstacle_msg.float_storage) + if float_array.size % 3 == 0: + arraye_format = float_array.reshape(-1, 3) + else: + self.get_logger().error('Obstacle memory storage has incorrect shape!') + return + for i, obs_coords in enumerate(arraye_format): topic_name = f'/obstacle_position_{i}' - if topic_name not in self.publishers: - self.publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) + if topic_name not in self.obstacle_publishers: + self.obstacle_publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) PS = PointStamped() PS.header.stamp = self.get_clock().now().to_msg() PS.point.x, PS.point.y, PS.point.z = obs_coords try: - self.publishers[topic_name].publish(PS) + self.obstacle_publishers[topic_name].publish(PS) except Exception as e: self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') @@ -77,4 +85,4 @@ def main(args=None): obstacle_memory_node = ObstacleMemoryNode() rclpy.spin(obstacle_memory_node) obstacle_memory_node.destroy_node() - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() From 9e60b0fbb8fa57700a8d4b1f5bd2ff0cb5bc5337 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Tue, 11 Feb 2025 21:55:44 -0300 Subject: [PATCH 16/49] Refactor obstacle_node.py to streamline transform_direction assignment for improved readability --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 435acba..374b30f 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -104,8 +104,7 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): target_frame = 'odom' try: transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) - transform_direction = self.tf_buffer.transform( - direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) + transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return From e56b959725a8625f31b5414b7184416e9e6af5e8 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Tue, 11 Feb 2025 21:56:56 -0300 Subject: [PATCH 17/49] Update .gitignore and refactor obstacle_memory_node references for consistency --- src/gpg_fproject/launch/gpg_fproject_launch.py | 2 +- src/gpg_fproject/setup.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gpg_fproject/launch/gpg_fproject_launch.py b/src/gpg_fproject/launch/gpg_fproject_launch.py index d37551f..9d34c5c 100644 --- a/src/gpg_fproject/launch/gpg_fproject_launch.py +++ b/src/gpg_fproject/launch/gpg_fproject_launch.py @@ -70,7 +70,7 @@ def generate_launch_description(): obstacle_memory_node = Node( package='gpg_fproject', - executable='obstacle_memory_node', + executable='obstacle_memory', output='screen' ) diff --git a/src/gpg_fproject/setup.py b/src/gpg_fproject/setup.py index 8dd2039..1d07eb2 100644 --- a/src/gpg_fproject/setup.py +++ b/src/gpg_fproject/setup.py @@ -31,7 +31,7 @@ 'console_scripts': [ 'image_node = gpg_fproject.image_node:main', 'user_node = gpg_fproject.user_node:main', 'obstacle_node = gpg_fproject.obstacle_node:main', - 'obstacle_memory_node = gpg_fproject.obstacle_memory_node:main' + 'obstacle_memory = gpg_fproject.obstacle_memory:main' ], }, ) From c79c790edc21d97aa07cf931b3fd9bfc87d7bb00 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Tue, 11 Feb 2025 21:58:10 -0300 Subject: [PATCH 18/49] Update .gitignore to include additional files and add new graph visualization for frame data --- src/gpg_fproject/gpg_fproject/image_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 2c21e26..5321a78 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -172,7 +172,7 @@ def process_image(self, cv_image): direction.vector.y = ray[1] direction.vector.z = ray[2] - target_frame = 'odom' + target_frame = "odom" # Transform origin and direction to odom frame try: From d0d50809d8b18ebc8cea7ff24b88a4fc7a5fae44 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Tue, 11 Feb 2025 21:58:42 -0300 Subject: [PATCH 19/49] Remove redundant comment in ImageNode class to enhance code clarity --- src/gpg_fproject/gpg_fproject/image_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 5321a78..53d014c 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -174,7 +174,6 @@ def process_image(self, cv_image): target_frame = "odom" - # Transform origin and direction to odom frame try: transform_origin = self.tf_buffer.transform( origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) From 23d1aff4a180cb1cf951e323250de146aacd6c9c Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 00:26:06 -0300 Subject: [PATCH 20/49] Consistently use double quotes for target_frame assignment in obstacle_node.py --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 374b30f..ac933ab 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -101,7 +101,7 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): direction.vector.y = ray[1] direction.vector.z = ray[2] - target_frame = 'odom' + target_frame = "odom" try: transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) From 950146fa1fd7159c8d3daec4a2f7fd1c5ff1cc65 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 00:26:18 -0300 Subject: [PATCH 21/49] Update timestamp assignment in ImageNode to use the clock instance for improved accuracy --- src/gpg_fproject/gpg_fproject/image_node.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 53d014c..330d584 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -163,11 +163,11 @@ def process_image(self, cv_image): origin = PointStamped() origin.header.frame_id = "camera_link" - origin.header.stamp = rclpy.time.Time().to_msg() + origin.header.stamp = self.get_clock().now().to_msg() direction = Vector3Stamped() direction.header.frame_id = "camera_link" - direction.header.stamp = rclpy.time.Time().to_msg() + direction.header.stamp = self.get_clock().now().to_msg() direction.vector.x = ray[0] direction.vector.y = ray[1] direction.vector.z = ray[2] @@ -175,10 +175,8 @@ def process_image(self, cv_image): target_frame = "odom" try: - transform_origin = self.tf_buffer.transform( - origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) - transform_direction = self.tf_buffer.transform( - direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) + transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) + transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return From 2231670049ffe6eb72740a460871a2b3e382f32a Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:21:13 -0300 Subject: [PATCH 22/49] Decrease duplicate threshold in ObstacleMemoryNode for improved obstacle detection sensitivity --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 6348567..597fcc4 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -12,7 +12,7 @@ def __init__(self): self.obstacle_publishers = {} self.memory_size = 5 - self.duplicate_threshold = 0.05 + self.duplicate_threshold = 0.01 self.x_coords, self.y_coords, self.z_coords = [], [], [] def obstacle_callback(self, msg): From bf119d26b01588104293e31df0ceab5f8f53c47c Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:21:23 -0300 Subject: [PATCH 23/49] Update poses of models in final_project_new_world.sdf for improved positioning accuracy --- .../worlds/final_project_new_world.sdf | 77 ++++--------------- 1 file changed, 14 insertions(+), 63 deletions(-) diff --git a/src/gpg_fgazebo/worlds/final_project_new_world.sdf b/src/gpg_fgazebo/worlds/final_project_new_world.sdf index d02ebbb..2fe9b01 100644 --- a/src/gpg_fgazebo/worlds/final_project_new_world.sdf +++ b/src/gpg_fgazebo/worlds/final_project_new_world.sdf @@ -70,7 +70,7 @@ false - 2.5363732600268438 -0.80112278462533015 0.49999999846970899 6.5729138181508515e-12 -1.3573746487752694e-05 2.9434920695967671e-11 + 3.5084900856018066 -4.8863000869750968 0.50007998943328835 1.5065299587321439e-06 -1.636899968423854e-05 -6.2029800553207759e-07 @@ -119,7 +119,7 @@ false - 3.0250799655914307 -3.766556978225708 0.49999999990199806 4.3528643514705118e-21 -7.19647729771648e-21 1.4704217161817701e-22 + 3.0250799655914307 -7.103431224822998 0.49616199731826782 6.8396199260892533e-19 -1.784369977462839e-19 -9.0752201588740355e-20 @@ -166,7 +166,7 @@ false - 3.8625005118640274 0.93321859835144638 0.49999999832908254 1.4294517723464686e-11 -1.095599094382217e-05 7.2967956065578558e-09 + 3.6618199348449703 1.9262722730636597 0.49999800324440002 1.2282500100848701e-11 -1.1096400158295009e-05 7.2934400063360099e-09 @@ -215,7 +215,7 @@ false - 4.453384447351727 -2.4193280444908121 0.49999999771937531 4.9310286018203178e-12 -1.1413900008258369e-05 0.00039735288789561593 + 3.6361923217773433 -2.5664587020874023 0.49999985098838806 2.9413502168548196e-07 -1.2823601078716922e-06 0.00039737702790333288 @@ -264,7 +264,7 @@ false - 6.8430671858149577 0.78498223335679773 0.49999999767414682 1.5448136740589461e-11 -8.0348490882515501e-06 1.5838780830737034e-11 + 3.6360399723052979 -0.2170737236738205 0.49999979138374329 4.6287199357060178e-07 -2.0394199677252063e-06 -1.2406700326614776e-07 @@ -312,57 +312,8 @@ false false - - 8.3753870428920862 -1.0539718866350367 0.49999999328581984 4.5424763573070339e-13 -6.0198071594412506e-06 3.9727298803672929e-14 - - - - 0.074999999999999997 - 0 - 0 - 0.074999999999999997 - 0 - 0.074999999999999997 - - 1 - 0 0 0 0 0 0 - - - - - 0.5 - 1 - - - - - - - - - - - - - - 0.5 - 1 - - - - 1 0.5 0 1 - 1 0.5 0 1 - 1 1 1 1 - - - 0 0 0 0 0 0 - false - - false - false - - 0.93528187274932861 -3.721019983291626 0.049999901152185701 3.9158531513353579e-18 7.7454060110091739e-18 3.7197256459834753e-20 + 1.0488699674606323 -2.4860496520996094 0.049999900162220001 3.9156699469856512e-18 7.7465497452815544e-18 3.7198800053824108e-20 @@ -458,7 +409,7 @@ false - 0.013356563008388056 -3.8117073265420482 0.049999901152185278 3.9376194225938313e-18 7.6919996772288558e-18 3.7170246867682256e-20 + 0.30450239777565002 -2.0890100002288818 0.049999900162220001 3.9402401063106201e-18 7.6927002874043523e-18 3.7160901610837255e-20 @@ -505,7 +456,7 @@ false - 0.8416714580967537 -2.5757948134049022 0.049999901152185278 3.9376962056216585e-18 7.69307262342453e-18 3.7173909086980559e-20 + 1.7968200445175171 -2.7600438594818115 0.049999900162220001 3.9402599586453214e-18 7.6943199070437311e-18 3.71722010272517e-20 @@ -552,7 +503,7 @@ false - 4.1581422306674831 -1.488395035635325 0.049999901152185278 3.9377765141972875e-18 7.6950985280545685e-18 3.7171162566952042e-20 + 1.0895800590515137 -0.93555700778961182 0.049999900162220001 3.9396201344415116e-18 7.7009299073186424e-18 3.7178799085106517e-20 @@ -599,7 +550,7 @@ false - 0.13756405494454182 -1.0513805918298864 0.049999901152185278 3.9453594697948961e-18 7.6901497063274743e-18 3.7168130591222518e-20 + 0.34709501266479492 -0.44078299403190613 0.049999900162220001 3.9387499404371058e-18 7.6873798617044112e-18 3.717899941791112e-20 @@ -646,7 +597,7 @@ false - 1.0996535508002552 0.24602853746299003 0.049999901152185278 3.9453958552831698e-18 7.6900072206275729e-18 3.7172416794418986e-20 + 1.0996500253677368 0.760772705078125 0.049999900162220001 3.9388798077932766e-18 7.6870200381379506e-18 3.7183700776470748e-20 @@ -693,7 +644,7 @@ false - 2.9735159531075981 0.039177734632563022 0.049999901152185278 3.9447443518316567e-18 7.705324462983486e-18 3.7175843135111715e-20 + 1.8049399852752686 0.29998359084129333 0.049999900162220001 3.9370798627803613e-18 7.7054802278682966e-18 3.7180899348380572e-20 @@ -740,7 +691,7 @@ false - 2.1315328186118649 -2.2259022002784734 0.049999901152185278 3.9419464567571984e-18 7.6933586260454131e-18 3.716253038745501e-20 + 1.7610900402069092 -1.2950215339660645 0.049999900162220001 3.939379838473565e-18 7.6900500007217324e-18 3.7144700503058586e-20 @@ -787,7 +738,7 @@ false - -0.44637602705250679 1.0239373472672866 0.049999901152185278 3.9183953316390883e-18 7.7378141686947243e-18 3.7207537265826434e-20 + 0.42512500286102295 1.654688835144043 0.049999900162220001 3.9117400118954118e-18 7.7347103091740829e-18 3.7218600689737096e-20 From d7ad31ec4a6ab65baad09c1bba3a18f6f61fd2df Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:21:33 -0300 Subject: [PATCH 24/49] Fix indentation in robot.urdf for improved readability --- src/gpg_fproject/gpg_fproject/robot.urdf | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/robot.urdf b/src/gpg_fproject/gpg_fproject/robot.urdf index 3447cc5..d9dc015 100644 --- a/src/gpg_fproject/gpg_fproject/robot.urdf +++ b/src/gpg_fproject/gpg_fproject/robot.urdf @@ -4,10 +4,11 @@ - $(find gpg_fproject)/controllers.yaml + $(find gpg_fproject)/controllers.yaml + gz_ros2_control/GazeboSimSystem From f5a0355ecad2e1b7cd94b45014d1edc72411763d Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:21:45 -0300 Subject: [PATCH 25/49] Add import for rclpy.time in obstacle_node.py for time handling --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index ac933ab..1f72bb5 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -1,5 +1,6 @@ import cv2 import rclpy +import rclpy.time import tf2_ros import numpy as np import image_geometry From 75d8b44c4c755bf005d559929ffde5bdd1b39bdd Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:22:02 -0300 Subject: [PATCH 26/49] Fix syntax error in parameter_bridge arguments for clock topic --- src/gpg_fproject/launch/gpg_fproject_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/launch/gpg_fproject_launch.py b/src/gpg_fproject/launch/gpg_fproject_launch.py index 9d34c5c..b9120c1 100644 --- a/src/gpg_fproject/launch/gpg_fproject_launch.py +++ b/src/gpg_fproject/launch/gpg_fproject_launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): time_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"], output="both" ) From 4cd0efbb538becd823e76a9f447bdf77461a0c70 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Wed, 12 Feb 2025 07:22:19 -0300 Subject: [PATCH 27/49] Clarify image processing log messages and update timestamp retrieval method --- src/gpg_fproject/gpg_fproject/image_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 330d584..69617a2 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -98,7 +98,7 @@ def check_condition(self, user_form, approx_object_form): def process_image(self, cv_image): """ - Function to process the image and detect the object + Function to process the image and detect the goal object Args: @@ -115,7 +115,7 @@ def process_image(self, cv_image): lower_hsv, upper_hsv = self.get_hsv_limit(user_color) mask = cv2.inRange(hsv_img, lower_hsv, upper_hsv) if np.all(mask == 0): - self.get_logger().info('No object detected') + self.get_logger().info('No goal object detected') return contours, approx_object_form, max_contours, text, coords = get_shape(cv_image, mask, 0.04, user_form) @@ -192,7 +192,7 @@ def process_image(self, cv_image): intersection_point = PointStamped() intersection_point.header.frame_id = target_frame - intersection_point.header.stamp = rclpy.time.Time().to_msg() + intersection_point.header.stamp = self.get_clock().now().to_msg() intersection_point.point.x = x intersection_point.point.y = y intersection_point.point.z = z From 99aaf6de688ce6359f381041ee597b7bc8131202 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 13:31:20 -0300 Subject: [PATCH 28/49] Refactor image_node.py for improved readability and error handling in transformations --- src/gpg_fproject/gpg_fproject/image_node.py | 42 +++++++++++---------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 69617a2..13196d6 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -1,16 +1,16 @@ import cv2 import rclpy +import tf2_ros import numpy as np import image_geometry from rclpy.node import Node +from gpg_fproject.utils import get_shape from sensor_msgs.msg import Image, CameraInfo -import tf2_ros from cv_bridge import CvBridge, CvBridgeError +from tf2_geometry_msgs import do_transform_point from std_msgs.msg import Float64MultiArray, String -from gpg_fproject.utils import get_shape from geometry_msgs.msg import PointStamped, Vector3Stamped -from tf2_geometry_msgs import do_transform_point class ImageNode(Node): def __init__(self): @@ -23,8 +23,8 @@ def __init__(self): self.color_subscription = self.create_subscription(String, '/object_color', self.color_callback, 10) self.form_subscription = self.create_subscription(String, '/object_form', self.form_callback, 10) - self.tf_buffer = tf2_ros.Buffer() self.bridge = CvBridge() + self.tf_buffer = tf2_ros.Buffer() self.camera_model = image_geometry.PinholeCameraModel() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) self.current_servo_position = 0.0 @@ -93,7 +93,7 @@ def check_condition(self, user_form, approx_object_form): # self.get_logger().info('Circle detected') #else: else: - self.get_logger().info('Object not detected') + self.get_logger().info('No object detected') return def process_image(self, cv_image): @@ -140,16 +140,13 @@ def process_image(self, cv_image): bottom_point = tuple(max_contours[max_contours[:, :, 1].argmax()][0]) bottom_point = [cX, bottom_point[1]] - #self.get_logger().info(f'Bottom point of the object: {bottom_point}') cv2.circle(cv_image, bottom_point, 5, (255, 0, 0), -1) - cv2.putText(cv_image, "Bottom Point", (bottom_point[0] - 25, bottom_point[1] - 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) + cv2.putText(cv_image, "Bottom Point", (bottom_point[0] - 25, bottom_point[1] - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) cv2.drawContours(cv_image, [max_contours], -1, (0, 255, 0), 3) cv2.putText(cv_image, text, (coords[0], coords[1] - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - cv2.putText(cv_image, "centroid", (cX - 25, cY - 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) + cv2.putText(cv_image, "centroid", (cX - 25, cY - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.circle(cv_image, (cX, cY), 5, (255, 255, 255), -1) cv2.imshow("Original Image", cv_image) @@ -164,6 +161,9 @@ def process_image(self, cv_image): origin = PointStamped() origin.header.frame_id = "camera_link" origin.header.stamp = self.get_clock().now().to_msg() + origin.point.x = 0.0 + origin.point.y = 0.0 + origin.point.z = 0.0 direction = Vector3Stamped() direction.header.frame_id = "camera_link" @@ -175,20 +175,22 @@ def process_image(self, cv_image): target_frame = "odom" try: - transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) - transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - self.get_logger().warn(f"Transformation failed: {e}") - return - - if transform_direction.vector.z == 0: - self.get_logger().warn("Ray is parallel to the XY plane.") - return + transform_origin = self.tf_buffer.transform( + origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_direction = self.tf_buffer.transform( + direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + except Exception as e: + self.get_logger().warn(f"Transformation failed: {e}") + return + + #if transform_direction.vector.z == 0: + # self.get_logger().warn("Ray is parallel to the XY plane.") + # return lamda = - transform_origin.point.z / transform_direction.vector.z x = transform_origin.point.x + transform_direction.vector.x * lamda y = transform_origin.point.y + transform_direction.vector.y * lamda - z = 0.0 + z = transform_origin.point.z + transform_direction.vector.z * lamda intersection_point = PointStamped() intersection_point.header.frame_id = target_frame From 2e759fba2047e59ab0d597d2bb14232aa040992b Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 13:40:44 -0300 Subject: [PATCH 29/49] Reorganize imports and enhance docstring for get_hsv_limit function in image_node.py --- src/gpg_fproject/gpg_fproject/image_node.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 13196d6..0eddc72 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -23,11 +23,13 @@ def __init__(self): self.color_subscription = self.create_subscription(String, '/object_color', self.color_callback, 10) self.form_subscription = self.create_subscription(String, '/object_form', self.form_callback, 10) - self.bridge = CvBridge() self.tf_buffer = tf2_ros.Buffer() - self.camera_model = image_geometry.PinholeCameraModel() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + self.bridge = CvBridge() + self.camera_model = image_geometry.PinholeCameraModel() self.current_servo_position = 0.0 + self.camera_info = None self.user_color = None self.user_form = None @@ -65,7 +67,11 @@ def image_callback(self, msg): def get_hsv_limit(self, user_color): """ - Function to get the HSV limits for the color asked by the user""" + Function to get the HSV limits for the color asked by the user + + Args: + user_color: The color asked by the user + """ if user_color=='blue': lower_hsv = np.array([100, 150, 50]) upper_hsv = np.array([140, 255, 255]) @@ -202,8 +208,6 @@ def process_image(self, cv_image): self.get_logger().info(f"Intersection Point (odom): x={x}, y={y}, z={z}") self.point_publisher.publish(intersection_point) - - def main(args=None): """ Main function to run the image node""" From 9a731aa34427e3cbf4b923cdc397c389c4b06507 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 14:41:20 -0300 Subject: [PATCH 30/49] Refactor obstacle_node.py to improve contour detection logic and enhance logging for obstacle position publishing --- .../gpg_fproject/obstacle_node.py | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 1f72bb5..637e77a 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -12,7 +12,6 @@ from tf2_geometry_msgs import do_transform_point - class ObstacleNode(Node): def __init__(self): super().__init__('obstacle_node') @@ -22,6 +21,7 @@ def __init__(self): self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + self.bridge = CvBridge() self.camera_model = image_geometry.PinholeCameraModel() self.camera_info = None @@ -61,39 +61,40 @@ def process_image(self, cv_image): self.get_logger().info('No red color detected from the obstacle node') return - contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if not contours: self.get_logger().info('No contours detected from the obstacle node') return for contour in contours: M = cv2.moments(contour) - if M["m00"] == 0: - continue - cX = int(M["m10"] / M["m00"]) - cY = int(M["m01"] / M["m00"]) - bottom_point = tuple(contour[contour[:, :, 1].argmax()][0]) - bottom_point = (cX, bottom_point[1]) - self.publish_obstacle_position(cv_image, bottom_point, cX, cY) + if M["m00"] != 0: + cX = int(M["m10"] / M["m00"]) + cY = int(M["m01"] / M["m00"]) + bottom_point = tuple(contour[contour[:, :, 1].argmax()][0]) + bottom_point = (cX, bottom_point[1]) + self.publish_obstacle_position(cv_image, bottom_point, cX, cY) def publish_obstacle_position(self, cv_image, bottom_point, x, y): """ Publish the position of the obstacle """ cv2.circle(cv_image, bottom_point, 5, (255, 0, 0), -1) - cv2.putText(cv_image, "obstacle bottom Point", (bottom_point[0] - 25, bottom_point[1] - 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) - cv2.putText(cv_image, "centroid", (x - 25, y - 25), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) + cv2.putText(cv_image, "obstacle bottom Point", (bottom_point[0] - 25, bottom_point[1] - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) + cv2.putText(cv_image, "centroid", (x - 25, y - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) cv2.circle(cv_image, (x, y), 5, (255, 255, 255), -1) cv2.imshow("Original Image", cv_image) cv2.waitKey(1) ray = self.camera_model.projectPixelTo3dRay(bottom_point) self.get_logger().info(f'Obstacle ray direction: {ray}') + origin = PointStamped() origin.header.stamp = self.get_clock().now().to_msg() origin.header.frame_id = 'camera_link' + origin.point.x = 0.0 + origin.point.y = 0.0 + origin.point.z = 0.0 direction = Vector3Stamped() direction.header.stamp = self.get_clock().now().to_msg() @@ -114,11 +115,10 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): self.get_logger().warn("Ray is parallel to the XY plane.") return - lamda = - transform_origin.point.z / transform_direction.vector.z x = transform_origin.point.x + transform_direction.vector.x * lamda y = transform_origin.point.y + transform_direction.vector.y * lamda - z = 0.0 + z = transform_origin.point.z + transform_direction.vector.z * lamda intersection_point = PointStamped() intersection_point.header.frame_id = target_frame @@ -129,7 +129,8 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): self.get_logger().info(f"Intersection Point of the obstacle (odom): x={x}, y={y}, z={z}") self.publisher_.publish(intersection_point) - + #verify if intersection point is published + self.get_logger().info(f'Published obstacle position: x={x:.2f}, y={y:.2f}, z={z:.2f}') def main(args=None): From b0d27a1e76eacc0674107a24011cc8c8d2fb3845 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 14:43:58 -0300 Subject: [PATCH 31/49] Enhance header frame_id assignment and add logging for intersection point publication in image_node.py --- src/gpg_fproject/gpg_fproject/image_node.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 0eddc72..3523037 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -165,15 +165,15 @@ def process_image(self, cv_image): self.get_logger().info(f"Ray direction: {ray}") origin = PointStamped() - origin.header.frame_id = "camera_link" origin.header.stamp = self.get_clock().now().to_msg() + origin.header.frame_id = "camera_link" origin.point.x = 0.0 origin.point.y = 0.0 origin.point.z = 0.0 direction = Vector3Stamped() - direction.header.frame_id = "camera_link" direction.header.stamp = self.get_clock().now().to_msg() + direction.header.frame_id = "camera_link" direction.vector.x = ray[0] direction.vector.y = ray[1] direction.vector.z = ray[2] @@ -199,14 +199,16 @@ def process_image(self, cv_image): z = transform_origin.point.z + transform_direction.vector.z * lamda intersection_point = PointStamped() - intersection_point.header.frame_id = target_frame intersection_point.header.stamp = self.get_clock().now().to_msg() + intersection_point.header.frame_id = target_frame intersection_point.point.x = x intersection_point.point.y = y intersection_point.point.z = z self.get_logger().info(f"Intersection Point (odom): x={x}, y={y}, z={z}") self.point_publisher.publish(intersection_point) + #verify that intersection point is published + self.get_logger().info('Intersection image node published.') def main(args=None): """ From f6442bb023b671931b434df74c3b4441f8bcc024 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 14:44:43 -0300 Subject: [PATCH 32/49] Update logging message for published obstacle position in obstacle_node.py --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 637e77a..26385d2 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -130,7 +130,7 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): self.get_logger().info(f"Intersection Point of the obstacle (odom): x={x}, y={y}, z={z}") self.publisher_.publish(intersection_point) #verify if intersection point is published - self.get_logger().info(f'Published obstacle position: x={x:.2f}, y={y:.2f}, z={z:.2f}') + self.get_logger().info(f'Intersection obstacle node published.') def main(args=None): From 0110683711a1149a1f4cce9de769481783c52fab Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 15:18:33 -0300 Subject: [PATCH 33/49] Refactor obstacle_memory.py to improve duplicate threshold and enhance obstacle publishing logging --- .../gpg_fproject/obstacle_memory.py | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 597fcc4..47060b4 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -12,7 +12,7 @@ def __init__(self): self.obstacle_publishers = {} self.memory_size = 5 - self.duplicate_threshold = 0.01 + self.duplicate_threshold = 0.001 self.x_coords, self.y_coords, self.z_coords = [], [], [] def obstacle_callback(self, msg): @@ -61,24 +61,24 @@ def publish_obstacle_memory(self): float_array = np.array(obstacle_msg.float_storage) if float_array.size % 3 == 0: - arraye_format = float_array.reshape(-1, 3) + array_format = float_array.reshape(-1, 3) + for i, obs_coords in enumerate(array_format): + self.get_logger().info(f'Obstacle {i} position: {obs_coords}') + topic_name = f'/obstacle_position_{i}' + if topic_name not in self.obstacle_publishers: + self.obstacle_publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) + PS = PointStamped() + PS.header.stamp = self.get_clock().now().to_msg() + PS.header.frame_id = '' + PS.point.x, PS.point.y, PS.point.z = obs_coords + try: + self.obstacle_publishers[topic_name].publish(PS) + self.get_logger().info(f'Obstacle {i} published. Well done buddy!') + except Exception as e: + self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e} ! Check it out!') else: self.get_logger().error('Obstacle memory storage has incorrect shape!') - return - - for i, obs_coords in enumerate(arraye_format): - topic_name = f'/obstacle_position_{i}' - if topic_name not in self.obstacle_publishers: - self.obstacle_publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) - - PS = PointStamped() - PS.header.stamp = self.get_clock().now().to_msg() - PS.point.x, PS.point.y, PS.point.z = obs_coords - - try: - self.obstacle_publishers[topic_name].publish(PS) - except Exception as e: - self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e}') + return def main(args=None): rclpy.init(args=args) @@ -86,3 +86,6 @@ def main(args=None): rclpy.spin(obstacle_memory_node) obstacle_memory_node.destroy_node() rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 140d3cb69052e1613bed4691447c4e25e6338c72 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 15:19:35 -0300 Subject: [PATCH 34/49] Increase duplicate threshold in obstacle_memory.py to improve obstacle detection accuracy --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 47060b4..4f17f14 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -12,7 +12,7 @@ def __init__(self): self.obstacle_publishers = {} self.memory_size = 5 - self.duplicate_threshold = 0.001 + self.duplicate_threshold = 0.05 self.x_coords, self.y_coords, self.z_coords = [], [], [] def obstacle_callback(self, msg): From bf5a7d9c93e12faa188750cfc6937f862579523a Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 15:40:38 -0300 Subject: [PATCH 35/49] Set header frame_id to "odom" in obstacle_memory.py for improved obstacle position publishing --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 4f17f14..b4f92de 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -69,7 +69,7 @@ def publish_obstacle_memory(self): self.obstacle_publishers[topic_name] = self.create_publisher(PointStamped, topic_name, 10) PS = PointStamped() PS.header.stamp = self.get_clock().now().to_msg() - PS.header.frame_id = '' + PS.header.frame_id = "odom" PS.point.x, PS.point.y, PS.point.z = obs_coords try: self.obstacle_publishers[topic_name].publish(PS) From 0af01f5a1f9ba230f557d32e00c9f5f8db69108b Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 17:26:23 -0300 Subject: [PATCH 36/49] Add memory size publisher and log memory size in obstacle_memory.py --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index b4f92de..1fb0c3a 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -2,6 +2,7 @@ import numpy as np from rclpy.node import Node from geometry_msgs.msg import PointStamped +from std_msgs.msg import Int32 from custom_msg_fproject.msg import UnboundedFloat class ObstacleMemoryNode(Node): @@ -9,6 +10,7 @@ def __init__(self): super().__init__('obstacle_memory_node') self.create_subscription(PointStamped, '/obstacle_position', self.obstacle_callback, 10) self.memory_publisher = self.create_publisher(UnboundedFloat, '/obstacle_memory', 10) + self.memory_size_publisher = self.create_publisher(Int32, '/obstacle_memory_size', 10) self.obstacle_publishers = {} self.memory_size = 5 @@ -71,9 +73,13 @@ def publish_obstacle_memory(self): PS.header.stamp = self.get_clock().now().to_msg() PS.header.frame_id = "odom" PS.point.x, PS.point.y, PS.point.z = obs_coords + memo_size = Int32() + memo_size.data = self.memory_size try: self.obstacle_publishers[topic_name].publish(PS) + self.memory_size_publisher.publish(memo_size) self.get_logger().info(f'Obstacle {i} published. Well done buddy!') + self.logger.info(f'Obstacle memory size: {self.memory_size}') except Exception as e: self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e} ! Check it out!') else: From cf9eb7f8ed075c9d5a0c3120b086455f07dcece1 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Wed, 12 Feb 2025 17:26:35 -0300 Subject: [PATCH 37/49] Reduce transformation timeout to 1 second in obstacle_node.py for improved responsiveness --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 26385d2..680c4bf 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -105,8 +105,8 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): target_frame = "odom" try: - transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) - transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=2.0)) + transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return From bd668b1ec27cdeb690bf72b513771ca6a62206b2 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 10:44:25 -0300 Subject: [PATCH 38/49] Fix argument syntax in gpg_fproject_launch.py for parameter_bridge node --- src/gpg_fproject/launch/gpg_fproject_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpg_fproject/launch/gpg_fproject_launch.py b/src/gpg_fproject/launch/gpg_fproject_launch.py index b9120c1..9d34c5c 100644 --- a/src/gpg_fproject/launch/gpg_fproject_launch.py +++ b/src/gpg_fproject/launch/gpg_fproject_launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): time_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock]"], + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], output="both" ) From 22ff9bb5955ee6315805320c018eeda4f3829796 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 11:26:20 -0300 Subject: [PATCH 39/49] Rename float_storage to float32_values in UnboundedFloat.msg for clarity --- src/custom_msg_fproject/msg/UnboundedFloat.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/custom_msg_fproject/msg/UnboundedFloat.msg b/src/custom_msg_fproject/msg/UnboundedFloat.msg index 6055c25..3e87d43 100644 --- a/src/custom_msg_fproject/msg/UnboundedFloat.msg +++ b/src/custom_msg_fproject/msg/UnboundedFloat.msg @@ -1,2 +1,2 @@ std_msgs/Header header -float32[] float_storage \ No newline at end of file +float32[] float32_values \ No newline at end of file From 99c5349b06890967796496067d31bcfacfc0c578 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 11:28:30 -0300 Subject: [PATCH 40/49] Enhance logging messages in image_node.py for improved clarity and emphasis --- src/gpg_fproject/gpg_fproject/image_node.py | 24 ++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 3523037..a166292 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -44,13 +44,13 @@ def color_callback(self, msg): """ Callback function for the color topic""" self.user_color = msg.data - self.get_logger().info(f'I heard the color asked by the user: {self.user_color}') + self.get_logger().info(f'I heard the color asked by the user: {self.user_color}.**') def form_callback(self, msg): """ Callback function for the form topic""" self.user_form = msg.data - self.get_logger().info(f'I heard the form asked by the user: {self.user_form}') + self.get_logger().info(f'**I heard the form asked by the user: {self.user_form}.**') def image_callback(self, msg): """ @@ -59,11 +59,11 @@ def image_callback(self, msg): return try: cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8') - self.get_logger().info('Image received and processing and will be processed') + self.get_logger().info('**Image received and processing and will be processed.**') self.process_image(cv_image) except CvBridgeError as e: - self.get_logger().error(f'Error while converting image: {e}') + self.get_logger().error(f'**Error while converting image: {e}.**') def get_hsv_limit(self, user_color): """ @@ -94,12 +94,12 @@ def check_condition(self, user_form, approx_object_form): if user_form == 'triangle' and approx_object_form in ['triangle', 'triangle approximation distorted']: self.get_logger().info('Triangle detected') elif user_form == 'box' and approx_object_form in ['box', 'box approximation distorted']: - self.get_logger().info('box detected') + self.get_logger().info('**box detected**') #elif user_form == 'circle' and approx_object_form in ['circle', 'circle approximation distorted']: # self.get_logger().info('Circle detected') #else: else: - self.get_logger().info('No object detected') + self.get_logger().info('**No object detected**') return def process_image(self, cv_image): @@ -115,13 +115,13 @@ def process_image(self, cv_image): user_form = self.user_form if user_color is None or user_form is None: - self.get_logger().info('User color or form not provided yet or not already received') + self.get_logger().info('**User color or form not provided yet or not already received**') return lower_hsv, upper_hsv = self.get_hsv_limit(user_color) mask = cv2.inRange(hsv_img, lower_hsv, upper_hsv) if np.all(mask == 0): - self.get_logger().info('No goal object detected') + self.get_logger().info('**No goal object detected**') return contours, approx_object_form, max_contours, text, coords = get_shape(cv_image, mask, 0.04, user_form) @@ -162,7 +162,7 @@ def process_image(self, cv_image): ray = self.camera_model.projectPixelTo3dRay(bottom_point) - self.get_logger().info(f"Ray direction: {ray}") + self.get_logger().info(f"**Goal object Ray direction: {ray}**") origin = PointStamped() origin.header.stamp = self.get_clock().now().to_msg() @@ -185,7 +185,7 @@ def process_image(self, cv_image): origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) transform_direction = self.tf_buffer.transform( direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) - except Exception as e: + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return @@ -205,10 +205,10 @@ def process_image(self, cv_image): intersection_point.point.y = y intersection_point.point.z = z - self.get_logger().info(f"Intersection Point (odom): x={x}, y={y}, z={z}") + self.get_logger().info(f"**Intersection Point (odom): x={x}, y={y}, z={z}**") self.point_publisher.publish(intersection_point) #verify that intersection point is published - self.get_logger().info('Intersection image node published.') + self.get_logger().info('**Intersection image node published.**') def main(args=None): """ From b597312db48493ac996c51889de3cf02bd557216 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 11:54:54 -0300 Subject: [PATCH 41/49] Rename float_storage to float32_values in ObstacleMemoryNode for clarity --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index 1fb0c3a..e596052 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -1,4 +1,5 @@ import rclpy +import tf2_ros import numpy as np from rclpy.node import Node from geometry_msgs.msg import PointStamped @@ -58,10 +59,11 @@ def publish_obstacle_memory(self): """ obstacle_msg = UnboundedFloat() obstacle_msg.header.stamp = self.get_clock().now().to_msg() - obstacle_msg.float_storage = [val for sublist in zip(self.x_coords, self.y_coords, self.z_coords) for val in sublist] + obstacle_msg.float32_values = [val for sublist in zip(self.x_coords, self.y_coords, self.z_coords) for val in sublist] + print(obstacle_msg.float32_values) self.memory_publisher.publish(obstacle_msg) - float_array = np.array(obstacle_msg.float_storage) + float_array = np.array(obstacle_msg.float32_values) if float_array.size % 3 == 0: array_format = float_array.reshape(-1, 3) for i, obs_coords in enumerate(array_format): From f59ad5287693dd23c119712d8c589771b11e62a3 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 11:55:09 -0300 Subject: [PATCH 42/49] Refactor transformation logic in obstacle_node.py for improved readability --- src/gpg_fproject/gpg_fproject/obstacle_node.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 680c4bf..1335950 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -105,15 +105,17 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): target_frame = "odom" try: - transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) - transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_origin = self.tf_buffer.transform( + origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_direction = self.tf_buffer.transform( + direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return - if transform_direction.vector.z == 0: - self.get_logger().warn("Ray is parallel to the XY plane.") - return + # if transform_direction.vector.z == 0: + # self.get_logger().warn("Ray is parallel to the XY plane.") + # return lamda = - transform_origin.point.z / transform_direction.vector.z x = transform_origin.point.x + transform_direction.vector.x * lamda @@ -121,8 +123,8 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): z = transform_origin.point.z + transform_direction.vector.z * lamda intersection_point = PointStamped() - intersection_point.header.frame_id = target_frame intersection_point.header.stamp = self.get_clock().now().to_msg() + intersection_point.header.frame_id = target_frame intersection_point.point.x = x intersection_point.point.y = y intersection_point.point.z = z From 7ed4de34938bad08310b1057f127f5c10d083b90 Mon Sep 17 00:00:00 2001 From: Geo99pro Date: Thu, 13 Feb 2025 11:55:39 -0300 Subject: [PATCH 43/49] Update MovePhysicalRobot to handle obstacle memory and improve movement logic --- .../gpg_fproject_controller/controller.py | 155 ++++++++++++------ 1 file changed, 102 insertions(+), 53 deletions(-) diff --git a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py index c867035..bad1b0d 100644 --- a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py +++ b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py @@ -2,23 +2,26 @@ import tf2_ros from rclpy.node import Node from nav_msgs.msg import Odometry -from math import pow, atan2, sqrt +from math import atan2, sqrt, pi from geometry_msgs.msg import TwistStamped, Pose2D, PointStamped from tf2_geometry_msgs import do_transform_point from tf2_ros import TransformException - +from std_msgs.msg import Int32 +from custom_msg_fproject.msg import UnboundedFloat +from tf_transformations import euler_from_quaternion class MovePhysicalRobot(Node): - """ Class to move the physical robot to a goal position """ def __init__(self): super().__init__('move_physical_robot') - self.publisher_ = self.create_publisher(TwistStamped, '/diff_drive_controller/cmd_vel', 10) + self.publishers_ = self.create_publisher(TwistStamped, '/diff_drive_controller/cmd_vel', 10) self.create_subscription(Odometry, '/diff_drive_controller/odom', self.pose_callback, 10) self.create_subscription(Pose2D, '/goal', self.goal_callback, 10) self.create_subscription(PointStamped, '/obstacle_position', self.obstacle_callback, 10) + self.create_subscription(UnboundedFloat, '/obstacle_memory', self.obstacle_memory_callback, 10) + #self.create_subscription(Int32, '/obstacle_memory_size', self.obstacle_memory_size_callback, 10) self.goal_pose = None self.current_pose = None @@ -27,6 +30,7 @@ def __init__(self): self.declare_parameter('linear_gain', 0.1) self.declare_parameter('angular_gain', 0.5) self.declare_parameter('obstacle_threshold', 0.5) + self.declare_parameter('repulsive_gain', 0.5) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) @@ -35,14 +39,34 @@ def goal_callback(self, msg): """ Callback function for the goal topic """ - self.goal_pose = msg + self.goal_pose = msg # Pose2D msg + self.get_logger().info(f'New goal received: {self.goal_pose.x}, {self.goal_pose.y}') def obstacle_callback(self, msg): """ Callback function for the obstacle topic """ - self.obstacle_pose = msg + self.obstacle_pose = msg # + self.get_logger().info(f'New live obstacle received: {self.obstacle_pose.point.x}, {self.obstacle_pose.point.y}') + + def obstacle_memory_callback(self, msg): + """ + Callback function for the obstacle memory topic + """ + + data = msg.float32_values # UnboundedFloat msg + self.get_logger().info(f'Obstacle memory received: {data}') + obstacles = [] + if len(data) % 3 != 0: + self.get_logger().info('Invalid obstacle memory, from a point of view of the obstacle memory size') + + else: + for i in range(0, len(data), 3): + obstacles.append((data[i], data[i+1], data[i+2])) + self.obstacle_memory = obstacles # list of tuples + self.get_logger().info(f'Obstacle memory received: {self.obstacle_memory}') + def pose_callback(self, msg): """ Callback function for the odometry topic @@ -51,71 +75,97 @@ def pose_callback(self, msg): if self.goal_pose is not None: self.move_physical_robot() - def stop_robot(self, euclidean_distance): + def stop_robot(self, euclidean_distance_to_goal): """ Stop the robot if the goal is reached """ - if euclidean_distance < 0.1: + if euclidean_distance_to_goal < 0.05: self.get_logger().info('Goal reached') velocity_msg = TwistStamped() velocity_msg.header.stamp = self.get_clock().now().to_msg() velocity_msg.twist.linear.x = 0.0 velocity_msg.twist.angular.z = 0.0 - self.publisher_.publish(velocity_msg) + self.publishers_.publish(velocity_msg) self.goal_pose = None - return def move_physical_robot(self): if self.goal_pose is None or self.current_pose is None: return + """ Move the physical robot to the goal position """ - goal_point = PointStamped() - goal_point.header.frame_id = 'odom' - goal_point.point.x = self.goal_pose.x - goal_point.point.y = self.goal_pose.y - goal_point.point.z = 0.0 - - #obstacle_point = PointStamped() - #obstacle_point.header.frame_id = 'odom' - #obstacle_point.point.x = self.obstacle_pose.point.x - #obstacle_point.point.y = self.obstacle_pose.point.y - #obstacle_point.point.z = 0.0 + current_x_position = self.current_pose.pose.pose.position.x + current_y_position = self.current_pose.pose.pose.position.y + + #convert quaternion to yaw + #https://robotics.stackexchange.com/questions/96357/ros2-python-quaternion-to-euler + orientation_q = self.current_pose.pose.pose.orientation + quanternion = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] + (roll, pitch, yaw) = euler_from_quaternion(quanternion) + + + goal_x_position = self.goal_pose.x + goal_y_position = self.goal_pose.y + + f_goal_x = goal_x_position - current_x_position + f_goal_y = goal_y_position - current_y_position + + f_rep_x = 0.0 + f_rep_y = 0.0 + + repulsive_gain = self.get_parameter('repulsive_gain').get_parameter_value().double_value + obstacle_threshold = self.get_parameter('obstacle_threshold').get_parameter_value().double_value + + if self.obstacle_pose is not None: + obs_x = self.obstacle_pose.point.x + obs_y = self.obstacle_pose.point.y + distance = sqrt((obs_x - current_x_position)**2 + (obs_y - current_y_position)**2) + if distance < obstacle_threshold and distance > 0.0: + force = repulsive_gain / distance**2 + f_rep_x += force * (current_x_position - obs_x) / distance + f_rep_y += force * (current_y_position - obs_y) / distance + + for obs in self.obstacle_memory: + obs_x, obs_y, obs_z = obs + distance = sqrt((obs_x - current_x_position)**2 + (obs_y - current_y_position)**2) + if distance < obstacle_threshold and distance > 0.0: + force = repulsive_gain / distance**2 + f_rep_x += force * (current_x_position - obs_x) / distance + f_rep_y += force * (current_y_position - obs_y) / distance + + f_total_x = f_goal_x + f_rep_x + f_total_y = f_goal_y + f_rep_y + desired_angle = atan2(f_total_y, f_total_x) + angle_diff = desired_angle - yaw + + while angle_diff > pi: + angle_diff -= 2 * pi + while angle_diff < -pi: + angle_diff += 2 * pi + + distance_to_goal = sqrt((goal_x_position - current_x_position)**2 + (goal_y_position - current_y_position)**2) + if distance_to_goal < 0.05: + self.stop_robot(distance_to_goal) + return - try: - transformed_goal = self.tf_buffer.transform(goal_point, 'base_link', timeout=rclpy.duration.Duration(seconds=1)) - euclidean_distance = sqrt(pow(transformed_goal.point.x, 2) + pow(transformed_goal.point.y, 2)) - self.stop_robot(euclidean_distance) - - angle_to_goal = atan2(transformed_goal.point.y, transformed_goal.point.x) - - base_angvel = 0 - if self.obstacle_pose and (self.get_clock().now().to_msg() - self.obstacle_pose.header.stamp) < 0.5: - transformed_obstacle = self.tf_buffer.transform(self.obstacle_pose, 'base_link', timeout=rclpy.duration.Duration(seconds=1)) - obstacle_distance = sqrt(pow(transformed_obstacle.point.x, 2) + pow(transformed_obstacle.point.y, 2)) - obstacle_threshold = self.get_parameter('obstacle_threshold').get_parameter_value().double_value - - if obstacle_distance < obstacle_threshold: - self.get_logger().info('Obstacle detected') - avoid_direction = -1 if self.obstacle_pose.point.y > 0 else 1 - angular_gain = self.get_parameter('angular_gain').get_parameter_value().double_value - base_angvel = angular_gain * avoid_direction - - linear_gain = self.get_parameter('linear_gain').get_parameter_value().double_value - angular_gain = self.get_parameter('angular_gain').get_parameter_value().double_value - - velocity_msg = TwistStamped() - velocity_msg.header.stamp = self.get_clock().now().to_msg() - velocity_msg.twist.linear.x = min(linear_gain * euclidean_distance, 0.1) - velocity_msg.twist.angular.z = angular_gain * angle_to_goal + base_angvel - - self.publisher_.publish(velocity_msg) + linear_gain = self.get_parameter('linear_gain').get_parameter_value().double_value + angular_gain = self.get_parameter('angular_gain').get_parameter_value().double_value + + linear_speed = linear_gain* sqrt(f_total_x**2 + f_total_y**2) + angular_speed = angular_gain * angle_diff - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - self.get_logger().warn(f"Transformation failed: {e}") + velocity_msg = TwistStamped() + velocity_msg.header.stamp = self.get_clock().now().to_msg() + velocity_msg.twist.linear.x = linear_speed + velocity_msg.twist.angular.z = angular_speed + self.publishers_.publish(velocity_msg) + self.get_logger().info(f'Linear speed: {linear_speed:.2f}, Angular speed: {angular_speed:.2f}, goal_distance: {distance_to_goal:.2f}') def main(args=None): + """ + Main function to run the move_physical_robot node + """ rclpy.init(args=args) move_physical_robot = MovePhysicalRobot() rclpy.spin(move_physical_robot) @@ -123,5 +173,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() - + main() \ No newline at end of file From a54e8e0c72670ea7da87510e6fd85c7fea8290dc Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:35:18 -0300 Subject: [PATCH 44/49] Add obstacle transformation handling in MovePhysicalRobot for improved accuracy --- .../gpg_fproject_controller/controller.py | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py index bad1b0d..e0996f3 100644 --- a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py +++ b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py @@ -1,4 +1,5 @@ import rclpy +import rclpy.duration import tf2_ros from rclpy.node import Node from nav_msgs.msg import Odometry @@ -9,6 +10,7 @@ from std_msgs.msg import Int32 from custom_msg_fproject.msg import UnboundedFloat from tf_transformations import euler_from_quaternion +from rclpy.time import Time class MovePhysicalRobot(Node): """ @@ -48,6 +50,14 @@ def obstacle_callback(self, msg): """ self.obstacle_pose = msg # self.get_logger().info(f'New live obstacle received: {self.obstacle_pose.point.x}, {self.obstacle_pose.point.y}') + try: + # transform the obstacle position to the base_link frame + trans = self.tf_buffer.transform(msg, 'base_link', rclpy.duration.Duration(seconds=1)) + self.obstacle_pose = trans + self.get_logger().info(f'Obstacle position received: {self.obstacle_pose.point.x}, {self.obstacle_pose.point.y}') + except TransformException as e: + self.get_logger().error(f'Obstacle position Transform error: {e}') + self.obstacle_pose = None def obstacle_memory_callback(self, msg): """ @@ -63,7 +73,20 @@ def obstacle_memory_callback(self, msg): else: for i in range(0, len(data), 3): - obstacles.append((data[i], data[i+1], data[i+2])) + try: + # transform the obstacle position to the base_link frame + obstacles_msg = PointStamped() + obstacles_msg.header.frame_id = 'odom' + obstacles_msg.point.x = data[i] + obstacles_msg.point.y = data[i+1] + obstacles_msg.point.z = data[i+2] + + trans = self.tf_buffer.transform(obstacles_msg, 'base_link', rclpy.duration.Duration(seconds=1)) + obstacles.append((trans.point.x, trans.point.y, trans.point.z)) + except TransformException as e: + self.get_logger().error(f'Each obstacle Transform error: {e}') + + #obstacles.append((data[i], data[i+1], data[i+2])) self.obstacle_memory = obstacles # list of tuples self.get_logger().info(f'Obstacle memory received: {self.obstacle_memory}') From 4a87f196c1ed948f2ea568f2e45a746e88e4232a Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:36:01 -0300 Subject: [PATCH 45/49] Refactor obstacle_msg construction in ObstacleMemoryNode for improved clarity and performance --- src/gpg_fproject/gpg_fproject/obstacle_memory.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_memory.py b/src/gpg_fproject/gpg_fproject/obstacle_memory.py index e596052..fd2cfc1 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_memory.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -59,8 +59,9 @@ def publish_obstacle_memory(self): """ obstacle_msg = UnboundedFloat() obstacle_msg.header.stamp = self.get_clock().now().to_msg() - obstacle_msg.float32_values = [val for sublist in zip(self.x_coords, self.y_coords, self.z_coords) for val in sublist] - print(obstacle_msg.float32_values) + sublists = [sublist for sublist in zip(self.x_coords, self.y_coords, self.z_coords)] + flat = [val for sublist in sublists for val in sublist] + obstacle_msg.float32_values = flat self.memory_publisher.publish(obstacle_msg) float_array = np.array(obstacle_msg.float32_values) @@ -74,14 +75,16 @@ def publish_obstacle_memory(self): PS = PointStamped() PS.header.stamp = self.get_clock().now().to_msg() PS.header.frame_id = "odom" - PS.point.x, PS.point.y, PS.point.z = obs_coords + PS.point.x = float(obs_coords[0]) + PS.point.y = float(obs_coords[1]) + PS.point.z = float(obs_coords[2]) memo_size = Int32() memo_size.data = self.memory_size try: self.obstacle_publishers[topic_name].publish(PS) self.memory_size_publisher.publish(memo_size) self.get_logger().info(f'Obstacle {i} published. Well done buddy!') - self.logger.info(f'Obstacle memory size: {self.memory_size}') + self.get_logger.info(f'Obstacle memory size: {self.memory_size}') except Exception as e: self.get_logger().error(f'Error publishing obstacle to topic {topic_name}: {e} ! Check it out!') else: From c37d31fec0dab1e8838efcc0bf043819f59e08b7 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:36:10 -0300 Subject: [PATCH 46/49] Update obstacle_node.py to change target frame from 'odom' to 'base_link' and remove unnecessary timestamp assignments for improved clarity --- .../gpg_fproject/obstacle_node.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index 1335950..be42656 100644 --- a/src/gpg_fproject/gpg_fproject/obstacle_node.py +++ b/src/gpg_fproject/gpg_fproject/obstacle_node.py @@ -10,6 +10,7 @@ from cv_bridge import CvBridge, CvBridgeError from geometry_msgs.msg import PointStamped, Vector3Stamped from tf2_geometry_msgs import do_transform_point +from rclpy.time import Time class ObstacleNode(Node): @@ -90,32 +91,32 @@ def publish_obstacle_position(self, cv_image, bottom_point, x, y): self.get_logger().info(f'Obstacle ray direction: {ray}') origin = PointStamped() - origin.header.stamp = self.get_clock().now().to_msg() + #origin.header.stamp = self.get_clock().now().to_msg() origin.header.frame_id = 'camera_link' origin.point.x = 0.0 origin.point.y = 0.0 origin.point.z = 0.0 direction = Vector3Stamped() - direction.header.stamp = self.get_clock().now().to_msg() + #direction.header.stamp = self.get_clock().now().to_msg() direction.header.frame_id = 'camera_link' direction.vector.x = ray[0] direction.vector.y = ray[1] direction.vector.z = ray[2] - target_frame = "odom" + target_frame = "base_link" try: - transform_origin = self.tf_buffer.transform( - origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) - transform_direction = self.tf_buffer.transform( - direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_origin = self.tf_buffer.transform(origin, + target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_direction = self.tf_buffer.transform(direction, + target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return - # if transform_direction.vector.z == 0: - # self.get_logger().warn("Ray is parallel to the XY plane.") - # return + if transform_direction.vector.z == 0: + self.get_logger().warn("Ray is parallel to the XY plane.") + return lamda = - transform_origin.point.z / transform_direction.vector.z x = transform_origin.point.x + transform_direction.vector.x * lamda From 73a32bbb04d3f0caaa7cdefe4ca7337c41323898 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:36:16 -0300 Subject: [PATCH 47/49] Refactor image_node.py to remove unnecessary timestamp assignments and improve transformation logic for clarity --- src/gpg_fproject/gpg_fproject/image_node.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index a166292..8fbe165 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -11,6 +11,7 @@ from tf2_geometry_msgs import do_transform_point from std_msgs.msg import Float64MultiArray, String from geometry_msgs.msg import PointStamped, Vector3Stamped +from rclpy.time import Time class ImageNode(Node): def __init__(self): @@ -165,14 +166,14 @@ def process_image(self, cv_image): self.get_logger().info(f"**Goal object Ray direction: {ray}**") origin = PointStamped() - origin.header.stamp = self.get_clock().now().to_msg() + #origin.header.stamp = self.get_clock().now().to_msg() origin.header.frame_id = "camera_link" origin.point.x = 0.0 origin.point.y = 0.0 origin.point.z = 0.0 direction = Vector3Stamped() - direction.header.stamp = self.get_clock().now().to_msg() + #direction.header.stamp = self.get_clock().now().to_msg() direction.header.frame_id = "camera_link" direction.vector.x = ray[0] direction.vector.y = ray[1] @@ -181,17 +182,15 @@ def process_image(self, cv_image): target_frame = "odom" try: - transform_origin = self.tf_buffer.transform( - origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) - transform_direction = self.tf_buffer.transform( - direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_origin = self.tf_buffer.transform(origin, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) + transform_direction = self.tf_buffer.transform(direction, target_frame, timeout=rclpy.duration.Duration(seconds=1.0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: self.get_logger().warn(f"Transformation failed: {e}") return - #if transform_direction.vector.z == 0: - # self.get_logger().warn("Ray is parallel to the XY plane.") - # return + if transform_direction.vector.z == 0: + self.get_logger().warn("Ray is parallel to the XY plane.") + return lamda = - transform_origin.point.z / transform_direction.vector.z x = transform_origin.point.x + transform_direction.vector.x * lamda From 9fc2fc75df1f106fe9727f9b7d67ece3dbe55b39 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:59:14 -0300 Subject: [PATCH 48/49] Update .gitignore to include additional file patterns for better exclusion management --- .gitignore | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3d87a20..9290363 100644 --- a/.gitignore +++ b/.gitignore @@ -20,4 +20,8 @@ mask.png result.png worlds_new2.sdf~ fproject_world.sdf~ -fproject_world.sdf.bak \ No newline at end of file +fproject_world.sdf.bak +rosbag2_2024_12_16-20_32_03 +*.png +.gitignore +.zip \ No newline at end of file From 0ed790ae650a065acb4db4460e0e87178b18af05 Mon Sep 17 00:00:00 2001 From: geo99pro Date: Thu, 13 Feb 2025 17:59:53 -0300 Subject: [PATCH 49/49] Update .gitignore to include zip files for better exclusion management --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 9290363..2522317 100644 --- a/.gitignore +++ b/.gitignore @@ -24,4 +24,4 @@ fproject_world.sdf.bak rosbag2_2024_12_16-20_32_03 *.png .gitignore -.zip \ No newline at end of file +*.zip \ No newline at end of file