FollowMe heading is wrong when target velocity is unknown

[istvanbeszteri]

When the drone is in Follow Me mode and the target velocity is unknown, the drone is not facing the target and is always positioned to the north of the target.

Steps to reproduce the behavior:

1. Take off
2. Change the flight mode to Follow Me
3. Send the following mavlink message:
   MAVLink_follow_target_message(
   timestamp = current_milli_time() - start_time,
   est_capabilities = 0,
   lat = latitude
   lon = longitude
   alt = altitude,
   vel = [0.0, 0.0, 0.0],
   acc = [0.0, 0.0, 0.0],
   attitude_q = [1.0, 0.0, 0.0, 0.0],
   rates = [0.0, 0.0, 0.0],
   position_cov = [0.0, 0.0, 0.0],
   custom_state = 0
   )

Expected behavior
The drone flies towards the target while facing the target.

I believe the problem is the following:
In follow_target.cpp in the on_active() method, the yaw update code is nested into an if statement that depends on the target velocity. If the target velocity is unknown, yaw is not updated at all.
Probably the same is true for the relative position calculation.

[istvanbeszteri]

I wrongly thought the problem is related to unknown target velocity. However the problem is related to the following if statement evaluation:
if (target_velocity_valid() && updated)

The "updated" variable is initialized to false at the beginning of the method and it is not changed anywhere else in the code.

[bresch]

It got broken here: e4ad994#diff-d30d556c349debf1b65dbed92ecbeab7906946c3a9644e7353f71c9dea4089d4L109-R105

[potaito]

I wrongly thought the problem is related to unknown target velocity. However the problem is related to the following if statement evaluation:
if (target_velocity_valid() && updated)

The "updated" variable is initialized to false at the beginning of the method and it is not changed anywhere else in the code.

Yes, and that's actually not the only problem. I'm working on a complete fresh start of follow-me. In the meantime @istvanbeszteri, could you test this PR #17531?

And btw, here is an important hidden tunable for the follow-me mode:

PX4-Autopilot/src/modules/navigator/follow_target.h

Line 67 in 62dc926

static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;

, which I would increase from 5m to 50m, which enables smoother velocity tracking in the additional range.

[dylantzx]

Hello @istvanbeszteri , I think I'm facing the same issue in which the following drone does not trail behind the target naturally, even with NAV_FT_FS = 1. It also doesn't face the drone and keeps point north. Are there any steps that you've done to improve on this? Or would I have to wait for the fresh start of follow-me that @potaito has mentioned?

Also, I wanted to clarify what target_velocity is referring to? I am currently simulating the FollowMe in Gazebo. In this context, would target_velocity be referring to the linear x,y,z of a Twist message from the target?

twist: 
  linear: 
    x: -0.0318861830551
    y: -0.0319973775274
    z: -0.0839316785293

Does it affect the heading in any way? Or is it possible to just send the Lat, Lon, and Altitude to as a Follow_Target MAVLink message?

[istvanbeszteri]

@dylantzx , The drone position was not solved. It seems the drone always positions itself to the north of the target. With this we probably need to wait for the new implementation.

The facing issue was fixed. I checked with PR #17531 and it worked. I think later the fix was merged to the master branch, but I did not check that myself. I am still using the version I updated with the PR.
If you refer to "vel" in the MAVLink_follow_target_message as target_velocity, I think that is not used in the follow me implementation. I think the target velocity is calculated from the position changes. I basically updated the lat, lon and alt values only in the MAVLink_follow_target_message message.

[potaito]

@dylantzx I have been working on a new version for the follow-me, yes. But it might take 2-3 more months before I can share everything. I'll clarify later.

When I initially debugged the existing follow_target implementation, I found that the TARGET_ACCEPTANCE_RADIUS of 5 meters is a bit too low. I increased it to 50 instead:

PX4-Autopilot/src/modules/navigator/follow_target.h

Line 67 in 9f87702

static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;

Follow_target basically has two modes: One where it's outside of this acceptance radius and then it's just trying to enter it by controlling the position. And a second mode, which is active while inside the acceptance radius, where it will then also try to track the velocity of the target. I could imagine that in your case it never enters the acceptance radius and is therefore not tracking with all of its power.

Oh and I just noticed while looking at the code what the other issue is:
There is another hard-coded acceptance radius on this line

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Line 196 in 9f87702

if ((_target_distance).length() > 1.0F) {

This means the yaw angle is only being commanded while the drone is closer than 1m. I would manually change that value to 15m or something.

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Lines 196 to 212 in 9f87702

	if ((_target_distance).length() > 1.0F) {
	// yaw rate smoothing
	// this really needs to control the yaw rate directly in the attitude pid controller
	// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
	_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
	_navigator->get_global_position()->lon,
	_current_target_motion.lat,
	_current_target_motion.lon);
	_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
	} else {
	_yaw_angle = _yaw_rate = NAN;
	}

Hope this helps.

[potaito]

@dylantzx , The drone position was not solved. It seems the drone always positions itself to the north of the target. With this we probably need to wait for the new implementation.

No no, just increase the hard-coded threshold here on this line from 1m to something higher:

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Line 196 in 9f87702

if ((_target_distance).length() > 1.0F) {

Honestly I don't see why this if-else exists in the first place.

[dylantzx]

@potaito Hello, thank you so much for the explanation. However, I thought the code actually meant the other way around, where if the _target_distance is <= 1.0, then it would lock the yaw and if larger than 1.0, then it would try to adjust the yaw?

This means the yaw angle is only being commanded while the drone is closer than 1m. I would manually change that value to 15m or something.

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Lines 196 to 212 in 9f87702

	if ((_target_distance).length() > 1.0F) {
	// yaw rate smoothing
	// this really needs to control the yaw rate directly in the attitude pid controller
	// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
	_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
	_navigator->get_global_position()->lon,
	_current_target_motion.lat,
	_current_target_motion.lon);
	_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
	} else {
	_yaw_angle = _yaw_rate = NAN;
	}

In addition to increasing the target_distance margin to 15m, I have also increased the acceptance radius to 50m as you've advised to, and the NAV_FT_DST parameter to be smaller than 50m such that the drone can track with all of its power. However, my drone's heading is still weird.

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Line 196 in 9f87702

if ((_target_distance).length() > 1.0F) {

PX4-Autopilot/src/modules/navigator/follow_target.h

Line 67 in 9f87702

static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;

On the other hand, if I were to comment out this line, although it causes the _target_distance to not be updated and stay at 0, it somehow allows the following drone to always face the target

PX4-Autopilot/src/modules/navigator/follow_target.cpp

Line 138 in 9f87702

map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),

[dylantzx]

@istvanbeszteri Yes, I'm also not quite sure what the vel is used for. I tried printing out the target_velocity and it stays at 0 throughout. Feeding it the velocity values of the target doesn't seem to make much of a difference as well.

If you refer to "vel" in the MAVLink_follow_target_message as target_velocity, I think that is not used in the follow me implementation. I think the target velocity is calculated from the position changes. I basically updated the lat, lon and alt values only in the MAVLink_follow_target_message message.

[potaito]

@potaito Hello, thank you so much for the explanation. However, I thought the code actually meant the other way around, where if the _target_distance is <= 1.0, then it would lock the yaw and if larger than 1.0, then it would try to adjust the yaw?

Ah you are right of course. Yes. Sorry!
You don't want to control yaw while the drone is very close or almost on top of the target, since the yaw can then make big jumps. Theoretically the yaw is not even defined for the case when the drone is exactly above the target, i.e. has the identical coordinates.

[potaito]

@istvanbeszteri Yes, I'm also not quite sure what the vel is used for. I tried printing out the target_velocity and it stays at 0 throughout. Feeding it the velocity values of the target doesn't seem to make much of a difference as well.

If you refer to "vel" in the MAVLink_follow_target_message as target_velocity, I think that is not used in the follow me implementation. I think the target velocity is calculated from the position changes. I basically updated the lat, lon and alt values only in the MAVLink_follow_target_message message.

How are you sending the FOLLOW_TARGET message to the drone? Until recently MAVSDK was not passing the GPS velocity, but only the position. That's why I'm asking.

And PX4 is now logging the follow_target message. So no need to add printfs, you can just take a look at the log and see what the follow_target message is saying :)

[dylantzx]

@potaito I am currently using MAVROS to subscribe to the target's GPS position and velocity, then send it as a MAVLink FOLLOW_TARGET message to the PX4. However, I am not fully sure if I am doing it correctly. Would be great if you could advice! :) Here is the code:

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <std_msgs/Char.h>
#include <mavros_msgs/GPSRAW.h>
#include <geometry_msgs/TwistStamped.h>

 namespace mavros {
 namespace extra_plugins{

 class FollowTargetPlugin : public plugin::PluginBase {
 public:
     FollowTargetPlugin() : PluginBase(),
         nh("/uav1/mavros")

    { };

     void initialize(UAS &uas_)
     {
         PluginBase::initialize(uas_);
         follow_target_sub = nh.subscribe("gpsstatus/gps1/raw", 10, &FollowTargetPlugin::follow_target_cb, this);
         velocity_sub = nh.subscribe("local_position/velocity_body", 10, &FollowTargetPlugin::velocity_cb, this);
     };

     Subscriptions get_subscriptions()
     {
         return {/* RX disabled */ };
     }

 private:
    ros::NodeHandle nh;
    ros::Subscriber follow_target_sub;
    ros::Subscriber velocity_sub;
    int32_t global_lat;
    int32_t global_lon; 
    float global_alt; 

    void follow_target_cb(const mavros_msgs::GPSRAW::ConstPtr &msg)
    {
        global_lat = msg->lat;
        global_lon = msg->lon;
        global_alt = msg->alt;
    }

    void velocity_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
    {
        mavlink::common::msg::FOLLOW_TARGET ft = {};
        ft.lat = global_lat;
        ft.lon = global_lon;
        ft.alt = global_alt;
        ft.vel[0] = msg->twist.linear.x;
        ft.vel[1] = msg->twist.linear.y;
        ft.vel[2] = msg->twist.linear.z;
        UAS_FCU(m_uas)->send_message_ignore_drop(ft);
    }

 };
 }   // namespace extra_plugins
 }   // namespace mavros

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::FollowTargetPlugin, mavros::plugin::PluginBase)

How would I go about doing this? Would this be referring to the log downloads under Analyze tools in QGC?

And PX4 is now logging the follow_target message. So no need to add printfs, you can just take a look at the log and see what the follow_target message is saying :)

[potaito]

@dylantzx

How would I go about doing this? Would this be referring to the log downloads under Analyze tools in QGC?

Exactly

[junwoo091400]

Solved by #18026