# Circle mode with Zed camera

**URL:** <https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595>\
**Category:** ArduSub\
**Tags:** ardusub, positioning\
**Created:** [March 9, 2026, 2:47pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595 "2026-03-09T14:47:52Z")\
**Posts on this page:** 10\
**Page:** 1

<div class="post-metadata">

**Author:** ![AbdelrahmanAshraf](https://avatars.discourse-cdn.com/v4/letter/a/a587f6/32.png) [@AbdelrahmanAshraf](https://discuss.bluerobotics.com/u/AbdelrahmanAshraf)\
**Post date:** [March 9, 2026, 2:47pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/1 "2026-03-09T14:47:52Z")

</div>

Hello

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

```python
#!/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()

```

---

<div class="post-metadata">

**Author:** ![EliotBR](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/eliotbr/32/6937_2.png) [@EliotBR](https://discuss.bluerobotics.com/u/EliotBR)\
**Post date:** [March 9, 2026, 5:27pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/2 "2026-03-09T17:27:15Z")

</div>

Hi @AbdelrahmanAshraf, welcome to the forum 🙂

> [@AbdelrahmanAshraf](#):
>
> i used the circle mode

I believe this is quite rarely used, but in case it’s relevant the source is [here](https://github.com/ArduPilot/ardupilot/blob/Sub-4.5/ArduSub/mode_circle.cpp). How are you trying to configure the mode and enable it?

> [@AbdelrahmanAshraf](#):
>
> use Zed/pose topic from zed Ros2 wrapper as a position reference

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?

> [@AbdelrahmanAshraf](#):
>
> 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

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?

---

<div class="post-metadata">

**Author:** ![AbdelrahmanAshraf](https://avatars.discourse-cdn.com/v4/letter/a/a587f6/32.png) [@AbdelrahmanAshraf](https://discuss.bluerobotics.com/u/AbdelrahmanAshraf)\
**Post date:** [March 9, 2026, 5:37pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/3 "2026-03-09T17:37:08Z")

</div>

Hi, thanks for the response!

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.

Any insight would be greatly appreciated!

---

<div class="post-metadata">

**Author:** ![EliotBR](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/eliotbr/32/6937_2.png) [@EliotBR](https://discuss.bluerobotics.com/u/EliotBR)\
**Post date:** [March 9, 2026, 5:44pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/4 "2026-03-09T17:44:19Z")

</div>

> [@AbdelrahmanAshraf](#):
>
> We’re testing in a featureless swimming pool, so I suspect ZED may be drifting due to lack of visual features

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.

> [@AbdelrahmanAshraf](#):
>
> 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.

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.

---

<div class="post-metadata">

**Author:** ![AbdelrahmanAshraf](https://avatars.discourse-cdn.com/v4/letter/a/a587f6/32.png) [@AbdelrahmanAshraf](https://discuss.bluerobotics.com/u/AbdelrahmanAshraf)\
**Post date:** [March 9, 2026, 5:49pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/5 "2026-03-09T17:49:30Z")

</div>

Thanks for the follow-up!

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?

Thanks again!

---

<div class="post-metadata">

**Author:** ![tony-white](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/tony-white/32/18385_2.png) [@tony-white](https://discuss.bluerobotics.com/u/tony-white)\
**Post date:** [March 9, 2026, 5:58pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/6 "2026-03-09T17:58:14Z")

</div>

Hi @AbdelrahmanAshraf -  
I’d recommend checking out the autopilot .BIN [log](https://blueos.cloud/docs/latest/usage/advanced/#log-browser), which should illustrate the vehicles position and incoming data directly. Checking the parameters associated with [circle mode](https://ardupilot.org/copter/docs/circle-mode.html) would be good!

---

<div class="post-metadata">

**Author:** ![EliotBR](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/eliotbr/32/6937_2.png) [@EliotBR](https://discuss.bluerobotics.com/u/EliotBR)\
**Post date:** [March 9, 2026, 5:59pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/7 "2026-03-09T17:59:05Z")

</div>

> [@AbdelrahmanAshraf](#):
>
> 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).

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.

> [@AbdelrahmanAshraf](#):
>
> Is there a recommended way to log MAVLink position data alongside ROS topics for this kind of correlation?

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](https://ardupilot.org/sub/docs/logmessages.html)).

---

<div class="post-metadata">

**Author:** ![AbdelrahmanAshraf](https://avatars.discourse-cdn.com/v4/letter/a/a587f6/32.png) [@AbdelrahmanAshraf](https://discuss.bluerobotics.com/u/AbdelrahmanAshraf)\
**Post date:** [March 9, 2026, 8:49pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/8 "2026-03-09T20:49:58Z")

</div>

Before Sending BIN

I think I haven’t tuned the PID params perfectly

Could be this the issue or should i check the pose first?

---

<div class="post-metadata">

**Author:** ![EliotBR](https://sea1.discourse-cdn.com/flex019/user_avatar/discuss.bluerobotics.com/eliotbr/32/6937_2.png) [@EliotBR](https://discuss.bluerobotics.com/u/EliotBR)\
**Post date:** [March 9, 2026, 9:01pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/9 "2026-03-09T21:01:14Z")

</div>

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.

---

<div class="post-metadata">

**Author:** ![AbdelrahmanAshraf](https://avatars.discourse-cdn.com/v4/letter/a/a587f6/32.png) [@AbdelrahmanAshraf](https://discuss.bluerobotics.com/u/AbdelrahmanAshraf)\
**Post date:** [March 9, 2026, 9:09pm UTC](https://discuss.bluerobotics.com/t/circle-mode-with-zed-camera/22595/10 "2026-03-09T21:09:48Z")

</div>

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