I need to make an orbit around object with facing the Object and with constant radius
So i used the circle mode and use Zed/pose topic from zed Ros2 wrapper as a position reference
The problem is when i run the circle mode it gives about half circle Cw and suddenly stops and gives left yaw (ccw) and then continues left diagonal motion and sometimes it moves unexpectedly
and this is my script to gives the pose to mavros
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import math
from transforms3d.euler import euler2quat # استخدمنا transforms3d بدل tf_transformations
# ======== CONFIG ========
CIRCLE_CENTER_X = 0.0 # مركز الدايرة X (متر)
CIRCLE_CENTER_Y = 0.0 # مركز الدايرة Y (متر)
# ========================
class ZedToMavros(Node):
def __init__(self):
super().__init__('zed_to_mavros_pose')
# Subscription to ZED pose
self.sub = self.create_subscription(
PoseStamped,
'/zed/zed_node/pose',
self.callback,
10
)
# Publishers to MAVROS
self.pub = self.create_publisher(
PoseStamped,
'/mavros/vision_pose/pose',
10
)
self.odom_pub = self.create_publisher(
Odometry,
'/mavros/odometry/in',
10
)
self.get_logger().info("ZED → MAVROS bridge started (vision_pose + odometry/in)")
def callback(self, msg):
# ZED pose
x = msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z
# ===== تعليق حساب yaw للتجربة =====
# yaw = math.atan2(CIRCLE_CENTER_Y - y, CIRCLE_CENTER_X - x)
# q = euler2quat(0, 0, yaw) # (roll, pitch, yaw) → (w, x, y, z)
# استخدام orientation من ZED مباشرة
q = [
msg.pose.orientation.w,
msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z
]
# ==== PoseStamped ====
pose_msg = PoseStamped()
pose_msg.header = msg.header
pose_msg.header.frame_id = "map"
pose_msg.pose.position.x = x
pose_msg.pose.position.y = y
pose_msg.pose.position.z = z
# ترتيب صحيح [w, x, y, z]
pose_msg.pose.orientation.w = q[0]
pose_msg.pose.orientation.x = q[1]
pose_msg.pose.orientation.y = q[2]
pose_msg.pose.orientation.z = q[3]
self.pub.publish(pose_msg)
# ==== Odometry ====
odom_msg = Odometry()
odom_msg.header = msg.header
odom_msg.header.frame_id = "map"
odom_msg.child_frame_id = "base_link"
odom_msg.pose.pose = pose_msg.pose
# تصفير السرعات
odom_msg.twist.twist.linear.x = 0.0
odom_msg.twist.twist.linear.y = 0.0
odom_msg.twist.twist.linear.z = 0.0
odom_msg.twist.twist.angular.x = 0.0
odom_msg.twist.twist.angular.y = 0.0
odom_msg.twist.twist.angular.z = 0.0
self.odom_pub.publish(odom_msg)
def main():
rclpy.init()
node = ZedToMavros()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
I believe this is quite rarely used, but in case it’s relevant the source is here. How are you trying to configure the mode and enable it?
I’m not familiar with this, and am not sure whether others in the community here are likely to be either. It may help to explain how that works, or link to its documentation or a definition?
Have you checked (e.g. by printing/logging) what kinds of angles you’re sending to the vehicle? Perhaps there’s a wrapping transition point (e.g. from 0 to 360°, or -180 to +180°) that’s causing problems?
Regarding the ZED pose setup: I’m using the ZED2i camera with the official ZED ROS2 wrapper. The wrapper publishes positional tracking data on the /zed/zed_node/pose topic as a PoseStamped message. I then bridge this to MAVROS via the /mavros/vision_pose/pose_cov topic so ArduSub can use it as an external position source instead of GPS (since we’re underwater). The relevant EKF3 parameters I set are:
EK3_SRC1_POSXY = 6 (ExternalNav)
EK3_SRC1_YAW = 6 (ExternalNav)
EK3_SRC1_POSZ = 1 (Baro, using a Bar30 pressure sensor)
VISO_TYPE = 1
GPS_TYPE = 0
Regarding the yaw wrapping issue: yes, I did handle the -180/+180 wraparound using a smooth_yaw() function that tracks the previous yaw and normalizes the delta. However I later removed the yaw override entirely and now pass the raw ZED orientation directly to MAVROS, so ArduSub handles yaw on its own during Circle mode.
Current behavior: the ROV completes roughly half a circle then loses direction and starts drifting. The ZED pose frequency stays stable at ~33Hz with no drops, and the position is stable when the ROV is stationary (noise < 0.002m). We’re testing in a featureless swimming pool, so I suspect ZED may be drifting due to lack of visual features — but I’m not 100% sure if that’s the root cause or if there’s something wrong with how Circle mode uses the external position reference.
This could definitely cause issues - especially if it thinks it sees the same thing as it saw previously, and decides it’s back at that pose.
Have you tried checking which positions are being sent to the autopilot, and whether there’s a jump at the time you’re having issues? If the input is smooth and the output is not then it’s likely an autopilot issue. If the input is strongly correlated with your behaviour then it’s likely the input at fault.
I did some testing to check exactly that. Here’s what I found:
ZED pose input (published to /mavros/vision_pose/pose_cov):
Frequency: stable at ~33Hz with no drops during the entire run, including the moment the circle breaks
Position noise when stationary: < 0.002m on all axes
No visible jumps in the raw pose data while the ROV is moving
MAVROS local position output (/mavros/local_position/pose):
Also stable when stationary (x values oscillating within ±0.001m)
I haven’t yet been able to log it continuously during the full circle run to catch the exact moment of failure
So based on this, the input appears smooth — which suggests the issue may be on the autopilot side. However I can’t fully rule out a subtle drift in the ZED pose during rotation that only appears mid-circle (since the pool is featureless, ZED might drift specifically when rotating rather than translating).
My next step is to log both the ZED pose and the local_position simultaneously during a circle attempt and compare them at the exact moment of failure. Is there a recommended way to log MAVLink position data alongside ROS topics for this kind of correlation? Or is there a specific ArduSub parameter I should check that might cause Circle mode to abort or switch behavior mid-run?
Hi @AbdelrahmanAshraf -
I’d recommend checking out the autopilot .BIN log, which should illustrate the vehicles position and incoming data directly. Checking the parameters associated with circle mode would be good!
If the pools is completely featureless (e.g. spherical) I’m surprised it can estimate position at all, and wouldn’t be surprised if there are points where it thinks it recognises a previous position and jumps “back” to it. Similar thoughts apply for any environment with closely matched symmetry.
Not that I’m aware of, though you should be able to at least get the MAVLink data via a telemetry log (.tlog file) if you have a software generating one, or, failing that, fetch a DataFlash (.bin) log from the autopilot (noting the non-MAVLink log messages).
PID tuning affects how quickly and strongly the vehicle responds to changing inputs or targets, and may oscillate if the gains are too high, but shouldn’t ever cause sudden jumps in a situation where the position estimate and target are expected to only be changing smoothly.
To be more accurate in describing the ROV attitude
When i set to Circle mode with Mavros first it moves in 1/4 or 1/2 circle CW then suddenly moves CCW little bit and after that moves in Zigzag diagonal motion
