Weird behavior with 'hover()'

[OdedHorowits]

Hi

I took the movement functions that the teleopt module uses, i.e. speed.py. With that speed module I can give the drone an order to go to a direction using velocity vector, and it keeps moving until I tell it to stop. How do I tell it to stop? two options, both problematic.

1. I can set the velocity vector to zeros. It stops, after a drift caused by its movement. But it stops.
2. I can call the 'hover()' function. That stops the drone immediately which is what I need.

But if I use the second option, when performing another movement on another direction, I still get leftovers from the former movement. For example, if I go 'forwards' (1,0,0), then call hover, and then go 'right' (0,1,0), the drone would go forwards, stops, then go forwards again for half a second and only then fixes its movement and go right. That is not desirable at all...

Is that a bug?

[pariaspe]

It looks like a bug, @RPS98 any idea on what might be happening?

[OdedHorowits]

If you want I can add code and screen recording

[pariaspe]

If you want I can add code and screen recording

Sure, that will be helpful to reproduce the error.

[OdedHorowits]

So here is my DroneInterface:

from as2_python_api.drone_interface_base import DroneInterfaceBase # /opt/ros/humble/lib/python3.10/site-packages/as2_python_api/drone_interface_base.py
from as2_python_api.drone_interface_teleop import DroneInterfaceTeleop

class MyDroneInterface(DroneInterfaceBase):

    class Directions:
        FORWARD=[1,0,0]
        BACKWARD=[-1,0,0]
        LEFT=[0,1,0]
        RIGHT=[0,-1,0]
        UP=[0,0,1]
        DOWN=[0,0,-1]
        ZERO=[0,0,0]

    def __init__(self, drone_id: str = "drone0", verbose: bool = False,
                 use_sim_time: bool = True) -> None:
        super().__init__(drone_id=drone_id, verbose=verbose, use_sim_time=use_sim_time)
        self.motion = DroneInterfaceTeleop()
        self.pose_frame_id='earth'
        self.twist_frame_id='earth'

    def hover(self):
        """Hover."""
        self.motion.motion_ref_handler.hover()

    def move_at_speed_in_plane(self, velocities, height, yaw_angle=0.0, verbose=False):
        """Move at speed."""
        if verbose:
            cprint(f'DEBUG::move_at_speed_in_plane()\tvelocities: {velocities}\theight: {height}', 'light_green')
        self.motion.motion_ref_handler.speed_in_a_plane.send_speed_in_a_plane_command_with_yaw_angle(
                        twist=velocities,
                        height=height,
                        twist_frame_id=self.twist_frame_id,
                        pose_frame_id=self.pose_frame_id,
                        yaw_angle=yaw_angle
                        )

Then I have a wrapper for using the interface:

from MyDroneInterface import MyDroneInterface

class bridge_test:
    def __init__(self):
        rclpy.init()
        self.drone_interface = MyDroneInterface("drone0", verbose=False, use_sim_time=True)
        self.directions_dict = {
            'FORWARD': self.drone_interface.Directions.FORWARD,
            'BACKWARD': self.drone_interface.Directions.BACKWARD,
            'LEFT': self.drone_interface.Directions.LEFT,
            'RIGHT': self.drone_interface.Directions.RIGHT,
            'UP': self.drone_interface.Directions.UP,
            'DOWN': self.drone_interface.Directions.DOWN,
            'ZERO': self.drone_interface.Directions.ZERO
        }

    def hover(self):
        self.drone_interface.hover()

    def go_to_direction(self, direction, speed=1.0, duration=None, verbose=False):
        self.drone_interface.move_at_speed_in_plane(
            velocities=[speed*d for d in self.directions_dict[direction]],
            height=self.height,
            verbose=verbose
            )

        if duration is not None:
            time.sleep(duration)
            ## Option A - set velocity vector to zero
            self.drone_interface.move_at_speed_in_plane(
                velocities=self.directions_dict['ZERO'],
                height=self.height
            )
            ## Option B - use hover()
            self.hover()

Results of using hover():

https://github.com/aerostack2/aerostack2.github.io/assets/15073370/56eba0ad-061c-4a61-9829-caa93a76ca2e

Results of using zero velocity:

https://github.com/aerostack2/aerostack2.github.io/assets/15073370/5295478c-0439-42a7-9eb8-5ca9931057a0

(You can see at the first movement the leftovers of the former video that uses hover...)

[pariaspe]

Not sure what it is happening. Also, why are you inheriting from DroneInterfaceBase instead of inheriting directly from DroneInterfaceTeleop? It might not be the issue here but you are duplicating every ros interface in DroneInterfaceBase class

[OdedHorowits]

You are completely right. The reason is that I started to use the DroneInterfaceTeleop module functions only after I build the interface... Anyway I switched it now to inherit from DroneInterfaceTeleop. Thanks for that. It seems that the 'hover' phenomena is still exist but much more tolerable, as can been seen. Still, it is a weird behaviour.

https://github.com/aerostack2/aerostack2.github.io/assets/15073370/9cf98deb-bb5e-408a-b297-8fd5e93b0d44

[RPS98]

Hi @OdedHorowits,

This issue should have been fixed in https://github.com/aerostack2/aerostack2/pull/584. Can you test it?

[OdedHorowits]

I would change the src code according to the change in that issue, re-compile and report.

[OdedHorowits]

So I cant check it since I installed the package using the binaries... I will try to builld it from src