Timeout quicker for vision_position plugin

[ggregory8]

@vooon
I think we need to find a way to stop sending the vision estimate if the tf is not updated (when using tf_listener). For example there is about an 8s delay from when I stop the vision estimate (ar_track_alvar) and when the vision_position plugin stops sending the vision_estimate to PX4.

Any ideas on this? Is there a way to check if a transform is older than a time, say 1s? Or alternatively we could also check the pose topic (/ar_pose_marker in the case of ar_track_alvar package) and only do the tf lookup if we get a valid estimate on this topic?

[vooon]

Don't know how do this now. And now i'am busy with asio cleanup.
Seems at weekend i get my pixhawk, so next issue - wp & command.

[ggregory8]

I'm going to do some research into if there is a check to see whether a new TF is available (I thought .waitForTransform() did this, and then only do .lookUpTransform(). If there is no way to do this then might have to add callback for /ar_pose_marker topic and then do .lookUpTransform() here.

This will solve a couple of issues; sync the vision estimate message to PX4 with the estimate from ROS package (ar_track_alvar in my case), also it will stop sending estimates to PX4 as soon as the vision estimate stops. Then we get faster timeout response in PX4.

This situation is only relevant to tf_listener function.

[TSC21]

@ggregory8 specifying code to ar_track_alvar in the plugin will remove it's general purpose. Finding a solution to the TF always sending a transform will be the best thing to do. The thing I came up with will be possibly reduce the lookup waiting time.

[TSC21]

Another thing we can do is creating a transform_old which takes the previous values of the transform, a transform_now, which takes the values of ros::Time(0) transform. Then we compare both by it's position and orientation and only send the vision_estimate if they are different.

Note: a comparison between a tf that, for example, was listen a second ago, can be done by also considering the time stamp instead of a just the last tf. Say for example we create a auxiliar tf each second and compare them. (As a MAV is a dynamic vehicle (not stable at all), a tf comparison between the last tf and the actual one will always be different , which in this case benefit us since the tf won't always be sent and, in that case, we clean up the always sending part).

[ggregory8]

I do like this last idea. Whilst still slightly hacky it will do what we want until we find another fix! I'll test it locally and let you know how it goes. Probably tomorrow job now.

On 23 Jul 2014, at 6:02 pm, TSC21 notifications@github.com wrote:

Another thing we can do is creating a transform_old which takes the previous values of the transform, a transform_now, which takes the values of ros::Time(0) transform. Then we compare both by it's position and orientation and only send the vision_estimate if they are different.

—
Reply to this email directly or view it on GitHub.

[TSC21]

I can also take a look at this ;) I think the priority should be on the PX4 side. What's left to be arranged so your PR can be accepted?

[ggregory8]

We're just just discussing some items about fusing the multiple position estimates into the INAV filter.

There really isn't any change between the existing branch and the PR though so it shouldn't told you back. Hopefully we can get it sorted soon.

On 23 Jul 2014, at 6:22 pm, TSC21 notifications@github.com wrote:

I can also take a look at this ;) I think the priority should be on the PX4 side. What's left to be arranged so your PR can be accepted?

—
Reply to this email directly or view it on GitHub.

[ggregory8]

If implemented your suggestion @TSC21 and its working great! The vision estimate will only be sent if the transform timestamp is new. This give a much fast timeout in PX4.

`
void tf_listener(void) {
tf::TransformListener listener(pos_nh);
tf::StampedTransform transform;
tf::StampedTransform transform_last; //GG
ros::Rate rate(30.0); //GG
while (pos_nh.ok()) {
// Wait up to 3s for transform
listener.waitForTransform(frame_id, child_frame_id, ros::Time(0), ros::Duration(0.10));
try{
listener.lookupTransform(frame_id, child_frame_id, ros::Time(0), transform);
// GG Check to see if the transform is new
if (transform.stamp_ != transform_last.stamp_)
{
send_vision_transform(static_casttf::Transform(transform), transform.stamp_);
}

                            transform_last = transform;
        }
        catch (tf::TransformException ex){
            ROS_ERROR_NAMED("position", "VisionTF: %s", ex.what());
        }
        rate.sleep();
    }
}

`
P.S For some reason I can't get the code tags to work here!

[vooon]

Add 4 spaces (or 1 tab) for code blocks.

[vooon]

@ggregory8 Just added a patch.

[TSC21]

You can close this @vooon. I confirmed it's working!

[MaMadDl]

i think there's a bug here
i was trying to send vision_pose to px4 with topic and not tf and i got the following error
[DEBUG] Vision: Same transform as last one, dropped.
i traced it to this line of code
if (last_transform_stamp == stamp) { ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped."); return; }
and i tried a quick fix by removing it and recompiling and vision_pose now goes through the mavros

[TSC21]

@MaMadDl if you read the comments on this issue carefully, you will see this is not a bug - it was inserted there for a purpose.

[TSC21]

Update your message timestamp instead of removing those lines (Check and update the msg header). Also you will required the timestamp for proper estimation on the FCU side.