diff --git a/.gitignore b/.gitignore index 3d87a20..2522317 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 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() diff --git a/src/custom_msg_fproject/msg/UnboundedFloat.msg b/src/custom_msg_fproject/msg/UnboundedFloat.msg new file mode 100644 index 0000000..3e87d43 --- /dev/null +++ b/src/custom_msg_fproject/msg/UnboundedFloat.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +float32[] float32_values \ No newline at end of file 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 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 + + 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 diff --git a/src/gpg_fproject/gpg_fproject/image_node.py b/src/gpg_fproject/gpg_fproject/image_node.py index 49ad28f..8fbe165 100644 --- a/src/gpg_fproject/gpg_fproject/image_node.py +++ b/src/gpg_fproject/gpg_fproject/image_node.py @@ -1,17 +1,17 @@ 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 -from gpg_fproject.utils import get_hsv_value_based_on_click +from rclpy.time import Time class ImageNode(Node): def __init__(self): @@ -25,10 +25,12 @@ def __init__(self): self.form_subscription = self.create_subscription(String, '/object_form', self.form_callback, 10) 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.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) self.current_servo_position = 0.0 + self.camera_info = None self.user_color = None self.user_form = None @@ -43,13 +45,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): """ @@ -58,15 +60,19 @@ 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): """ - 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]) @@ -89,17 +95,17 @@ 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('Object not detected') + self.get_logger().info('**No object detected**') return 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: @@ -110,13 +116,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 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) @@ -141,16 +147,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) @@ -160,30 +163,30 @@ 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() origin.header.frame_id = "camera_link" - origin.header.stamp = rclpy.time.Time().to_msg() + 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.frame_id = "camera_link" - direction.header.stamp = rclpy.time.Time().to_msg() direction.vector.x = ray[0] direction.vector.y = ray[1] direction.vector.z = ray[2] - target_frame = 'odom' + 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)) - 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 + 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.") @@ -192,19 +195,19 @@ def process_image(self, cv_image): 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.stamp = self.get_clock().now().to_msg() intersection_point.header.frame_id = target_frame - intersection_point.header.stamp = rclpy.time.Time().to_msg() 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.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): """ 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..fd2cfc1 --- /dev/null +++ b/src/gpg_fproject/gpg_fproject/obstacle_memory.py @@ -0,0 +1,102 @@ +import rclpy +import tf2_ros +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): + 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 + 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 + """ + 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) + 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() + + 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_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): + """ + Publish the obstacle memory + """ + obstacle_msg = UnboundedFloat() + obstacle_msg.header.stamp = self.get_clock().now().to_msg() + 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) + if float_array.size % 3 == 0: + 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 = "odom" + 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.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: + self.get_logger().error('Obstacle memory storage has incorrect shape!') + return + +def main(args=None): + rclpy.init(args=args) + obstacle_memory_node = ObstacleMemoryNode() + rclpy.spin(obstacle_memory_node) + obstacle_memory_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/gpg_fproject/gpg_fproject/obstacle_node.py b/src/gpg_fproject/gpg_fproject/obstacle_node.py index da7a434..be42656 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 @@ -7,9 +8,9 @@ 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 - +from rclpy.time import Time class ObstacleNode(Node): @@ -19,9 +20,9 @@ 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() self.camera_model = image_geometry.PinholeCameraModel() self.camera_info = None @@ -47,7 +48,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,56 +62,54 @@ 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) + 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 - max_contour = max(contours, key=cv2.contourArea) - if contours: - M = cv2.moments(max_contour) + for contour in contours: + M = cv2.moments(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]) - 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') + 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.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=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 @@ -119,22 +118,22 @@ 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 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 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'Intersection obstacle node published.') def main(args=None): 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 diff --git a/src/gpg_fproject/launch/gpg_fproject_launch.py b/src/gpg_fproject/launch/gpg_fproject_launch.py index 4be587b..9d34c5c 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', + 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, diff --git a/src/gpg_fproject/setup.py b/src/gpg_fproject/setup.py index 68c14d7..1d07eb2 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']), ], @@ -29,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 = gpg_fproject.obstacle_memory:main' ], }, ) diff --git a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py index c867035..e0996f3 100644 --- a/src/gpg_fproject_controller/gpg_fproject_controller/controller.py +++ b/src/gpg_fproject_controller/gpg_fproject_controller/controller.py @@ -1,24 +1,29 @@ import rclpy +import rclpy.duration 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 +from rclpy.time import Time 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 +32,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 +41,55 @@ 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}') + 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): + """ + 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): + 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}') + def pose_callback(self, msg): """ Callback function for the odometry topic @@ -51,71 +98,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 +196,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() - + main() \ No newline at end of file