Implementing End-to-End Robot Vision Service with a Fisheye Camera and ROS on TurtleBot3

The project builds an end‑to‑end visual‑servoing system for a TurtleBot3 Burger that relies solely on a fisheye camera and ROS to drive the robot.

Goals

Move the robot from its current location to a target location defined by two ArUco markers (position and orientation).

Avoid obstacles marked in red using an A‑star planner.

Park precisely so that the robot’s final pose matches the target pose.

Camera Calibration

Using the camera_calibration package, the camera is calibrated to obtain radial and tangential distortion coefficients and the intrinsic camera matrix ( fx, fy, Cx, Cy ). The distortion model is illustrated in the source images, and the five distortion coefficients are extracted for later undistortion.

Image Acquisition and Undistortion

The node subscribes to the ROS topic /camera/image_raw, converts each frame to a NumPy array, crops the region of interest, and undistorts the image with the previously obtained camera matrix and distortion coefficients.

ArUco Marker Detection and Pose Estimation

OpenCV’s cv2.detectMarkers identifies the two ArUco tags (current and target). Pose is estimated with cv2.estimatePoseSingleMarkers, yielding a translation vector [x, y, z] and a rotation vector [x, y, z] for each marker. These vectors are published on a custom ROS topic current_position using a message that contains geometry_msgs/Vector3 rotational and geometry_msgs/Vector3 translational .

Controller: From Vectors to Homogeneous Transform

The rotation vector is converted to a 3×3 rotation matrix via cv2.Rodrigues. Together with the translation vector, a 4×4 homogeneous matrix is formed. The current homogeneous matrix is inverted and multiplied by the target homogeneous matrix to obtain a combined matrix t. From t, the planar offsets dx = t[0][3] and dy = t[1][3] are extracted.

alpha = math.atan2(dy, dx)
rho = math.sqrt(dx**2 + dy**2)

The robot first publishes angular velocity on the cmd_vel topic until the heading aligns (alpha), then publishes linear velocity until the distance (rho) approaches zero, using a proportional controller with separate gains for angular and linear speed.

Obstacle Detection

The image is divided into cells sized to the robot. Each cell is converted to HSV; if any pixel falls within the red hue range, the cell is marked as an obstacle (1), otherwise 0. The resulting binary array feeds the A‑star planner.

A‑star Path Planning

The planner treats the obstacle map as a graph. The cost function f = g + h combines the accumulated path cost g and the heuristic estimate h (Euclidean distance to the goal). The algorithm returns a sequence of intermediate waypoints that avoid obstacles.

Intermediate Pose Estimation

Waypoint pixel coordinates are converted back to world coordinates, and the same ArUco pose‑estimation routine is applied to obtain the robot’s pose at each intermediate point.

Visualization

The shortest path is drawn on the image: blue for free space, light‑blue for obstacles, orange circles for the path, and bold white letters “S” and “G” for start and goal. The final map image is shown in the source.

Executing the Path

During each on_receive_image callback, the controller updates self.current_homogeneous_matrix. For each waypoint, it computes the target homogeneous matrix, fixes the heading, moves forward, and proceeds to the next waypoint when the error falls below a threshold.

Parking (Final Alignment)

At the goal, the rotation matrix is converted to Euler angles; the Z‑axis angle (beta) is extracted and used to publish a final angular velocity command so the robot’s orientation matches the target pose.

Running the System

roscore

roslaunch ueye_cam rgb8.launch

roslaunch turtlebot3_bringup turtlebot3_robot.launch

roslaunch visual_servoing visual_servoing.launch

These commands start the ROS core, camera driver, TurtleBot3 bring‑up, and the visual‑servoing launch file respectively.