Drone doesn't move by using the class DronePoseTracker

[wentao-ju]

Hi,

Thanks for the amazing work!

When I tried to use the DronePoseTracker class from gym_unrealcv/envs/tracking/baseline.py, I encountered an issue where the tracker did not move forward to follow the target. Instead, it oscillated left and right in place, even though the tracker’s camera remained directed at the target.
I printed the velocity values of the tracker at each step, and they were either 1 or -1, indicating that the tracker was expected to move. However, it remained stationary except for the oscillation.

class DronePoseTracker(object):
    def __init__(self, expected_distance = 250, expected_angle = 0):

        self.velocity_high = 1
        self.velocity_low = -1
        self.angle_high = 1
        self.angle_low = -1
        self.expected_distance = expected_distance
        self.expected_angle = expected_angle
        from simple_pid import PID
        self.angle_pid = PID(1, 0.01, 0, setpoint=1)
        self.velocity_pid = PID(5, 0.1, 0.05, setpoint=1)

    def act(self, pose, target_pose):
        delt_yaw = misc.get_direction(pose, target_pose) # get the angle between current pose and goal in x-y plane
        angle = np.clip(self.angle_pid(self.expected_angle-delt_yaw), self.angle_low, self.angle_high)
        current_distance = np.linalg.norm(np.array(pose[:3]) - np.array(target_pose[:3]))
        delt_distance = current_distance - self.expected_distance
        velocity_pid_output = self.velocity_pid(-delt_distance)
        print('Target pose:', target_pose)
        print('Tracker pose:', pose)
        print("current_distance", current_distance)
        print("delt_distance", delt_distance)
        print("velocity_pid_output", velocity_pid_output)
        velocity = np.clip(velocity_pid_output, self.velocity_low, self.velocity_high)
        print(f'Tracker --- angle: {angle}, velocity: {velocity}')
        return [velocity, 0, 0, angle]

I refer to the Nav2GoalAgent class to create a simple drone target class to test the tracker.

class DroneTargetController(object):
    def __init__(self, goal_area, random_th=0, max_len=200):
        self.step_counter = 0
        self.keep_steps = 0

        self.discrete = False
        self.velocity_high = 1
        self.velocity_low = -1
        self.angle_high = 1
        self.angle_low = -1

        self.goal_area = goal_area
        self.random_th = random_th

        self.max_len = max_len
        self.reset()

    def act(self, pose, ref_goal=None):
        self.step_counter += 1

        if self.pose_last == None:
            self.pose_last = pose
            d_moved = 10
        else:
            d_moved = np.linalg.norm(np.array(self.pose_last) - np.array(pose))
            self.pose_last = pose

        self.d_move_ave = self.d_move_ave * 0.7 + d_moved * 0.3

        if self.check_reach(self.goal, pose) or self.d_move_ave < 3 or self.step_counter > self.max_len:
            self.d_move_ave = 5
            if ref_goal is None or np.random.random() > self.random_th:
                self.goal = self.generate_goal(self.goal_area)
                self.velocity = 0.01
            else:
                self.goal = ref_goal
            self.step_counter = 0

        delt_yaw = misc.get_direction(pose, self.goal)
        angle = np.clip(delt_yaw, self.angle_low, self.angle_high)
        velocity = self.velocity * (1 + 0.2 * np.random.random())
        return [velocity,0,0,angle]

    def reset(self):
        self.step_counter = 0
        self.keep_steps = 0
        self.angle_noise_step = 0
        self.goal_id = 0
        self.d_move_ave = 5
        self.goal = self.generate_goal(self.goal_area)
        self.velocity = np.random.randint(self.velocity_low, self.velocity_high)
        self.pose_last = None

    def generate_goal(self, goal_area):
        if goal_area == None:
            goal_area = self.goal_area

        x = np.random.randint(goal_area[0], goal_area[1])
        y = np.random.randint(goal_area[2], goal_area[3])
        z = np.random.randint(goal_area[4], goal_area[5])
        goal = np.array([x, y, z])

        return goal

    def check_reach(self, goal, now):
        error = np.array(now[:3]) - np.array(goal[:3])
        distance = np.linalg.norm(error)
        return distance < 50

Print outputs for the first 5 steps

Target pose: [-330.435, -1548.525, 140.131, 0.0, 123.936, 0.0]
Tracker pose: [-470.0, -1341.109, 140.131, 0.002, -179.773, 0.002]
current_distance 249.99957256163472
delt_distance -0.00042743836527847634
velocity_pid_output 4.997862808173608
Tracker --- angle: 1.0, velocity: 1.0

Target pose: [-330.435, -1548.525, 140.131, 0.0, 132.545, 0.0]
Tracker pose: [-470.0, -1341.109, 140.131, -0.094, -179.77, 0.001]
current_distance 249.99957256163472
delt_distance -0.00042743836527847634
velocity_pid_output 5.005659474154129
Tracker --- angle: 1.0, velocity: 1.0

Target pose: [-330.435, -1548.525, 140.131, 0.0, 149.295, 0.0]
Tracker pose: [-470.0, -1341.109, 140.131, -2.066, -179.058, 0.001]
current_distance 249.99957256163472
delt_distance -0.00042743836527847634
velocity_pid_output 5.011856824036435
Tracker --- angle: 1.0, velocity: 1.0

Target pose: [-330.435, -1548.525, 140.131, 0.0, 159.119, 0.0]
Tracker pose: [-470.0, -1341.109, 140.131, -2.333, -178.005, 0.001]
current_distance 249.99957256163472
delt_distance -0.00042743836527847634
velocity_pid_output 5.018154131174563
Tracker --- angle: 1.0, velocity: 1.0

Target pose: [-330.435, -1548.525, 140.131, 0.0, 166.36, 0.0]
Tracker pose: [-470.0, -1341.109, 140.131, -3.321, -176.867, 0.001]
current_distance 249.99957256163472
delt_distance -0.00042743836527847634
velocity_pid_output 5.022852122214475
Tracker --- angle: 1.0, velocity: 1.0

Let me know if you need further details. Thank you!