diff --git a/leo_example_follow_aruco_marker/CMakeLists.txt b/leo_example_follow_aruco_marker/CMakeLists.txt new file mode 100644 index 0000000..dcf0279 --- /dev/null +++ b/leo_example_follow_aruco_marker/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(leo_example_follow_aruco_marker) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(generate_parameter_library REQUIRED) + +generate_parameter_module(follower_parameters + leo_follow_aruco/follower_parameters.yaml +) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_python_install_package(leo_follow_aruco) + +# Install Python executables +install(PROGRAMS + scripts/aruco_follower + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/leo_example_follow_aruco_marker/config/follower.yaml b/leo_example_follow_aruco_marker/config/follower.yaml new file mode 100644 index 0000000..1bad714 --- /dev/null +++ b/leo_example_follow_aruco_marker/config/follower.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + follow_id: 0 + marker_timeout: 1.0 + + min_ang_vel: 0.1 + max_ang_vel: 1.0 + angle_min: 0.1 + angle_max: 0.7 + + min_lin_vel_forward: 0.1 + max_lin_vel_forward: 0.4 + distance_min_forward: 0.7 + distance_max_forward: 1.2 + + min_lin_vel_reverse: 0.1 + max_lin_vel_reverse: 0.2 + distance_min_reverse: 0.4 + distance_max_reverse: 0.5 \ No newline at end of file diff --git a/leo_example_follow_aruco_marker/config/tracker.yaml b/leo_example_follow_aruco_marker/config/tracker.yaml new file mode 100644 index 0000000..2d838f2 --- /dev/null +++ b/leo_example_follow_aruco_marker/config/tracker.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + cam_base_topic: camera/image_color + output_frame: 'base_link' + + marker_dict: 4X4_50 + + publish_tf: true + marker_size: 0.15 + + aruco: + adaptiveThreshWinSizeMin: 3 + adaptiveThreshWinSizeMax: 23 + adaptiveThreshWinSizeStep: 10 diff --git a/leo_example_follow_aruco_marker/launch/follow_aruco_marker.launch.xml b/leo_example_follow_aruco_marker/launch/follow_aruco_marker.launch.xml new file mode 100644 index 0000000..78c693d --- /dev/null +++ b/leo_example_follow_aruco_marker/launch/follow_aruco_marker.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/leo_example_follow_aruco_marker/leo_follow_aruco/__init__.py b/leo_example_follow_aruco_marker/leo_follow_aruco/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/leo_example_follow_aruco_marker/leo_follow_aruco/aruco_follower_node.py b/leo_example_follow_aruco_marker/leo_follow_aruco/aruco_follower_node.py new file mode 100644 index 0000000..461663a --- /dev/null +++ b/leo_example_follow_aruco_marker/leo_follow_aruco/aruco_follower_node.py @@ -0,0 +1,195 @@ +import math +from typing import Optional + +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.publisher import Publisher +from rclpy.subscription import Subscription +from rclpy.time import Time +from rclpy.timer import Timer + +from geometry_msgs.msg import Twist, Point, Pose +from nav_msgs.msg import Odometry + +from aruco_opencv_msgs.msg import ArucoDetection, MarkerPose + +from leo_example_follow_aruco_marker.follower_parameters import follower_parameters +from leo_follow_aruco.utils import pose_delta, translate + + +class ArucoMarkerFollower(Node): + """A ROS node following specified Aruco Marker.""" + + running: bool = False + marker_angle: float = 0.0 + marker_distance: float = 0.0 + last_marker_position: Optional[Point] = None + twist_cmd: Twist = Twist() + last_odom_ts: Optional[Time] = None + odom_marker_pose: Pose = Pose() + odom_current_pose: Pose = Pose() + + def __init__(self) -> None: + super().__init__("aruco_follower") + + self.param_listener = follower_parameters.ParamListener(self) + self.params = self.param_listener.get_params() + self.param_listener.set_user_callback(self.reconfigure_callback) + + self.last_marker_ts: Time = self.get_clock().now() - Duration( + seconds=self.params.marker_timeout + 1.0 + ) + + self.cmd_vel_pub: Publisher = self.create_publisher(Twist, "cmd_vel", 1) + + self.marker_pose_sub: Subscription = self.create_subscription( + ArucoDetection, + "aruco_detections", + self.marker_callback, + 1, + ) + + self.merged_odom_sub: Subscription = self.create_subscription( + Odometry, "merged_odom", self.odom_callback, 1 + ) + + self.run_timer: Timer = self.create_timer(0.1, self.run) + + def reconfigure_callback(self, parameters: follower_parameters.Params) -> None: + """Callback for the parameters change. + Updates the values of parameters. + + Args: + parameters (follower_parameters.Params): The struct with current parameters. + """ + self.params = parameters + + def run(self) -> None: + """Function controlling the rover. Sends velocity command based on + marker position and odometry calculations. + """ + if not self.params.follow_enabled: + return + + self.update_vel_cmd() + self.cmd_vel_pub.publish(self.twist_cmd) + + def update_vel_cmd(self) -> None: + """Updates current velocity command based on recent marker detections and odometry data.""" + # Check for a timeout + if ( + self.last_marker_ts + Duration(seconds=self.params.marker_timeout) + < self.get_clock().now() + ): + self.twist_cmd.linear.x = 0.0 + self.twist_cmd.angular.z = 0.0 + return + + # Get the absolute angle to the marker + angle = math.fabs(self.marker_angle) + + # Get the direction multiplier + dir = -1.0 if self.marker_angle < 0.0 else 1.0 + + # Calculate angular command + if angle < self.params.angle_min: + ang_cmd = 0.0 + else: + ang_cmd = translate( + angle, + self.params.angle_min, + self.params.angle_max, + self.params.min_ang_vel, + self.params.max_ang_vel, + ) + + # Calculate linear command + if self.marker_distance >= self.params.distance_min_forward: + lin_cmd = translate( + self.marker_distance, + self.params.distance_min_forward, + self.params.distance_max_forward, + self.params.min_lin_vel_forward, + self.params.max_lin_vel_forward, + ) + elif self.marker_distance <= self.params.distance_max_reverse: + lin_cmd = -translate( + self.marker_distance, + self.params.distance_min_reverse, + self.params.distance_max_reverse, + self.params.max_lin_vel_reverse, + self.params.min_lin_vel_reverse, + ) + else: + lin_cmd = 0.0 + + self.twist_cmd.angular.z = dir * ang_cmd + self.twist_cmd.linear.x = lin_cmd + + def update_marker_angle_and_distance(self) -> None: + """Updates current distance and angle from the rover to the marker.""" + if self.last_marker_position: + current_pos_x, current_pos_y, current_yaw = pose_delta( + self.odom_marker_pose, self.odom_current_pose + ) + + position_x: float = self.last_marker_position.x - current_pos_x + position_y: float = self.last_marker_position.y - current_pos_y + + self.marker_angle = math.atan(position_y / position_x) - current_yaw + self.marker_distance = math.sqrt(position_x**2 + position_y**2) + + def marker_callback(self, msg: ArucoDetection) -> None: + """Callback for marker detection subscriber. Searches for the right marker + and updates the calculation needed variables. + + Args: + msg (ArucoDetection): Message received on the topic. + """ + marker: MarkerPose + for marker in msg.markers: + if marker.marker_id != self.params.follow_id: + continue + if Time.from_msg(msg.header.stamp) < self.last_marker_ts: + self.get_logger().warning( + "Got marker position with an older timestamp.", + throttle_duration_sec=3.0, + ) + continue + + self.last_marker_ts = Time.from_msg(msg.header.stamp) + self.last_marker_position = marker.pose.position + self.odom_marker_pose = self.odom_current_pose + + self.update_marker_angle_and_distance() + return + + def odom_callback(self, msg: Odometry) -> None: + """Callback for odometry subscriber. Updates stored poses needed for + distance and angle calculations. + + Args: + msg (Odometry): Message received on the topic. + """ + if self.last_odom_ts: + start_ts: Time = max(self.last_odom_ts, self.last_marker_ts) + + end_ts: Time = Time.from_msg(msg.header.stamp) + if end_ts < start_ts: + self.get_logger().warning( + "Reveived odometry has timestamp older than last marker position." + ) + + self.odom_current_pose = msg.pose + + self.update_marker_angle_and_distance() + + self.last_odom_ts = Time.from_msg(msg.header.stamp) + + def cleanup(self) -> None: + """Cleans ROS entities.""" + self.params.follow_enabled = False + self.destroy_timer(self.run_timer) + self.destroy_subscription(self.marker_pose_sub) + self.destroy_subscription(self.merged_odom_sub) + self.destroy_publisher(self.cmd_vel_pub) diff --git a/leo_example_follow_aruco_marker/leo_follow_aruco/follower_parameters.yaml b/leo_example_follow_aruco_marker/leo_follow_aruco/follower_parameters.yaml new file mode 100644 index 0000000..3bdcc20 --- /dev/null +++ b/leo_example_follow_aruco_marker/leo_follow_aruco/follower_parameters.yaml @@ -0,0 +1,89 @@ +follower_parameters: + follow_id: + type: int + default_value: 0 + description: "The ID of the marker to follow." + validation: + gt_eq<>: [0] + marker_timeout: + type: double + default_value: 1.0 + description: "Timeout for the marker detection (in seconds). When the specified time elapses with no detection the robot stops." + validation: + bounds<>: [0.1, 5.0] + min_ang_vel: + type: double + default_value: 0.1 + description: "Minimal angular velocity (in radians per second) the rover is going to rotate with." + validation: + bounds<>: [0.1, 1.0] + max_ang_vel: + type: double + default_value: 1.0 + description: "Maximal angular velocity (in radians per second) the rover is going to rotate with." + validation: + bounds<>: [1.0, 3.0] + angle_min: + type: double + default_value: 0.1 + description: "Minimal angle (in radians) to the marker, for the rover to start rotating." + validation: + bounds<>: [0.1, 1.0] + angle_max: + type: double + default_value: 0.7 + description: "Angle (in radians) to the marker at which rover will rotate with max speed." + validation: + bounds<>: [0.1, 1.0] + min_lin_vel_forward: + type: double + default_value: 0.1 + description: "Minimal linear velocity (in meters per second) the rover is going to move with driving forward." + validation: + bounds<>: [0.1, 0.5] + max_lin_vel_forward: + type: double + default_value: 0.4 + description: "Maximal linear velocity (in meters per second) the rover is going to move with driving forward." + validation: + bounds<>: [0.1, 0.5] + distance_min_forward: + type: double + default_value: 0.7 + description: "Minimal distance (in meters) to the marker, for the rover to start moving forward." + validation: + bounds<>: [0.5, 1.0] + distance_max_forward: + type: double + default_value: 1.2 + description: "Distance (in meters) to the marker, at which the rover will move forward with max speed." + validation: + bounds<>: [1.0, 2.0] + min_lin_vel_reverse: + type: double + default_value: 0.1 + description: "Minimal linear velocity (in meters per second) the rover is going to move with driving backward." + validation: + bounds<>: [0.1, 0.5] + max_lin_vel_reverse: + type: double + default_value: 0.2 + description: "Maximal linear velocity (in meters per second) the rover is going to move with driving backward." + validation: + bounds<>: [0.1, 0.5] + distance_min_reverse: + type: double + default_value: 0.4 + description: "Distance (in meters) to the marker, at which the rover will move backward with max speed." + validation: + bounds<>: [0.1, 1.0] + distance_max_reverse: + type: double + default_value: 0.5 + description: "Minimal distance (in meters) to the marker, for the rover to start moving backward." + validation: + bounds<>: [0.1, 2.0] + follow_enabled: + type: bool + default_value: true + description: "Whether to follow the marker or not." \ No newline at end of file diff --git a/leo_example_follow_aruco_marker/leo_follow_aruco/utils.py b/leo_example_follow_aruco_marker/leo_follow_aruco/utils.py new file mode 100644 index 0000000..e5d26d0 --- /dev/null +++ b/leo_example_follow_aruco_marker/leo_follow_aruco/utils.py @@ -0,0 +1,66 @@ +import math + +from geometry_msgs.msg import Pose +from tf_transformations import euler_from_quaternion + + +def translate( + value: float, leftMin: float, leftMax: float, rightMin: float, rightMax: float +) -> float: + """Proportionally projects given value from one interval onto antoher. + + Args: + value (float): Value to be projected. + leftMin (float): Minimal value from the source interval. + leftMax (float): Maximal value from the source interval. + rightMin (float): Minimal value from the target interval. + rightMax (float): Maximal value from the target interbal. + + Returns: + float: Proportionally projected value in the target interval. + """ + value = min(max(value, leftMin), leftMax) + + # Figure out how 'wide' each interval is. + leftSpan = leftMax - leftMin + rightSpan = rightMax - rightMin + + # Get the proportional placement of the value in the source interval. + valueScaled = float(value - leftMin) / float(leftSpan) + + # Cast the proportion on the target interval. + return rightMin + (valueScaled * rightSpan) + + +def pose_delta(pose1: Pose, pose2: Pose) -> tuple[float, float, float]: + """Calculates movement in linear x,y axes and yaw rotation between two odometry poses. + + Args: + pose1 (Pose): Old pose. + pose2 (Pose): Current pose. + + Returns: + tuple[float, float, float]: Movement in x axis, y axis, and yaw rotation. + """ + dx = pose2.position.x - pose1.position.x + dy = pose2.position.y - pose1.position.y + + q1 = [ + pose1.orientation.x, + pose1.orientation.y, + pose1.orientation.z, + pose1.orientation.w, + ] + q2 = [ + pose2.orientation.x, + pose2.orientation.y, + pose2.orientation.z, + pose2.orientation.w, + ] + + _, _, yaw1 = euler_from_quaternion(q1) + _, _, yaw2 = euler_from_quaternion(q2) + dyaw = yaw2 - yaw1 + dyaw = math.atan2(math.sin(dyaw), math.cos(dyaw)) + + return dx, dy, dyaw diff --git a/leo_example_follow_aruco_marker/package.xml b/leo_example_follow_aruco_marker/package.xml new file mode 100644 index 0000000..81b3353 --- /dev/null +++ b/leo_example_follow_aruco_marker/package.xml @@ -0,0 +1,34 @@ + + + + leo_example_follow_aruco_marker + 1.0.0 + + Follow Aruco Marker Example for Leo Rover. + + + Fictionlab + + MIT + + https://github.com/LeoRover/leo_examples-ros2/issues + https://github.com/LeoRover/leo_examples-ros2 + + Aleksander Szymański + + ament_cmake + ament_cmake_python + + generate_parameter_library + + aruco_opencv + aruco_opencv_msgs + geometry_msgs + nav_msgs + rclpy + tf_transformations + + + ament_cmake + + \ No newline at end of file diff --git a/leo_example_follow_aruco_marker/scripts/aruco_follower b/leo_example_follow_aruco_marker/scripts/aruco_follower new file mode 100755 index 0000000..fb0beeb --- /dev/null +++ b/leo_example_follow_aruco_marker/scripts/aruco_follower @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 + +import rclpy + +from leo_follow_aruco.aruco_follower_node import ArucoMarkerFollower + +rclpy.init() + +node = ArucoMarkerFollower() + +try: + rclpy.spin(node) +except KeyboardInterrupt as e: + node.cleanup() + node.destroy_node() + rclpy.try_shutdown()