how to mavros + tf

[szebedy]

Can somebody please give a quick introduction on how the /mavros/setpoint_position/tf feature works? I will post a working tutorial/example code here once I figure it out.

I have a vision based control application, where a drone is flying around, finds a visual marker, extracts the coordinates of the visual marker in body frame coordinates and finally flies to the marker with a position setpoint in local NED frame coordinates.

I need to transform the coordinates of the visual marker from body frame to local NED frame based on the position of the drone in local NED coordinates (/mavros/setpoint_position/local).

I little help would be very much appreciated since the documentation isn't too verbose.

MAVROS version and platform

Mavros: 0.23.3 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ ] ArduPilot [ x ] PX4

Version: 1.7.3

[TSC21]

/mavros/setpoint_position/tf serves for lookup between two frames (https://github.com/mavlink/mavros/blob/master/mavros/launch/px4_config.yaml#L104-L105). That would be used as the position you want to send the drone to, and not the position of the drone itself. If you want to send an estimate of the vehicle position WRT to the marker, you need to actually know the position of the marker WRT to the local origin, and send this estimate using vision_pose_estimate plugin.

[szebedy]

@TSC21 Thank you so much for the clarification. I was able to transform the position of the marker in local origin frame, and fly there using the mavros/setpoint_position/local topic.

However, unfortunately I still can't make use of the tf modules provided by mavros. I assumed that since the mavros/local_position/tf/send parameter is true by default, I would see a map(local_origin)->drone transformation in tf. Also, I thought if I set the mavros/setpoint_position/tf/listen parameter to true, and there exists a map(local_origin)->target_position path in tf, the drone would automatically use it as position setpoint and fly there. However neither of my assumptions turned out to be true. Below you can find a code snippet, maybe I just forgot something.

Otherwise can you please explain how the mavros/local_position/tf module is meant to be used?

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <math.h>

#define FLIGHT_ALTITUDE 1.0f

#define ROS_RATE 20.0

ros::Subscriber state_sub;
ros::Subscriber marker_pos_sub;
ros::Subscriber local_pos_sub;

ros::Publisher local_pos_pub;
ros::Publisher setpoint_pub;

ros::ServiceClient arming_client;
ros::ServiceClient land_client;
ros::ServiceClient set_mode_client;

...

geometry_msgs::PoseArray marker_position;
void marker_position_cb(const geometry_msgs::PoseArray::ConstPtr& msg){
    marker_position = *msg;

    static tf2_ros::TransformBroadcaster br;

    //Transformation from drone to visual marker coordinates
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "drone";
    transformStamped.child_frame_id = "target_position";

    ...

    br.sendTransform(transformStamped);
}

geometry_msgs::PoseStamped local_position;
void local_position_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_position = *msg;

    static tf2_ros::TransformBroadcaster br;

    //Transformation from map to drone coordinates
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "map";
    transformStamped.child_frame_id = "drone";

    ...

    br.sendTransform(transformStamped);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");

    ros::NodeHandle nh;

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(ROS_RATE);

    state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    marker_pos_sub = nh.subscribe<geometry_msgs::PoseArray>("whycon/poses", 10, marker_position_cb);
    local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, local_position_cb);

    local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    setpoint_pub = nh.advertise<mavros_msgs::PositionTarget>("mavros/setpoint_raw/local", 10);

    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    land_client = nh.serviceClient<mavros_msgs::CommandTOL>("mavros/cmd/land");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
        ROS_INFO("connecting to FCT...");
    }

    offboardMode();

    takeOff();

    approachMarker(nh);

    land();

    return 0;
}

void offboardMode(){
  ...
}

void takeOff(){
  ...
}

void approachMarker(ros::NodeHandle & nh){
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(ROS_RATE);

    nh.setParam("/mavros/local_position/tf/frame_id", "map");
    nh.setParam("/mavros/local_position/tf/child_frame_id", "drone");
    nh.setParam("/mavros/local_position/tf/send", true);

    nh.setParam("/mavros/setpoint_position/tf/frame_id", "map");
    nh.setParam("/mavros/setpoint_position/tf/child_frame_id", "target_position");
    nh.setParam("/mavros/setpoint_position/tf/listen", true);

    for(int j = 0; ros::ok() && j < 500; ++j){
    ...
        ros::spinOnce();
        rate.sleep();
    ...

    nh.setParam("mavros/setpoint_position/tf/listen", false);
    ROS_INFO("Marker approached!");

    return;
}

void land(){
  ...
}

[szebedy]

I tried the following measures to resolve the issue, but none of them helped.

1. Manually setting the send parameter to true with nh.setParam("mavros/local_position/tf/send", true); But I still can't see the transformation using rosrun rqt_tf_tree rqt_tf_tree

2. Switching from tf to tf2 with the previously posted implementation, broadcasting the necessary frame transformations. But the nh.setParam("mavros/setpoint_position/tf/listen", true); still does not take effect.

Also the documentation should to be updated, since the default ~local_position/tf/frame_id and is "map" and not "local_origin". Also, the default ~local_position/tf/send appears to be false by default. (https://github.com/mavlink/mavros/blob/95f2b40dd919a22f88bb05b916608e49f09503a4/mavros/src/plugins/local_position.cpp#L39)

[TSC21]

@szebedy what is the result of rosrun tf view_frames and then evince frames.pdf?

[szebedy]

@TSC21 frames.pdf

The map->drone->target_position are there because I broadcast them in the callback functions. Otherwise I only have the rest.

[szebedy]

@TSC21 This is what I'm left with if I comment out the br.sendTransform( ... ) functions in the position callback functions, but do have a nh.setParam("mavros/local_position/tf/send", true) somewhere in the beginning.

[TSC21]

I assumed that since the mavros/local_position/tf/send parameter is true by default, I would see a map(local_origin)->drone transformation in tf.

No. The published tf is from map to base_link (here), unless you changed the naming. Also, it is set to false by default. Changing this to true should let local_position plugin publish the tf from tf_frame_id (map by default) to tf_child_frame_id (base_link by default).

[szebedy]

@TSC21 That's exactly what I expected, but even after setting mavros/local_position/tf/send to true, I still can't see the tf_frame_id to tf_child_frame_id transformation in tf. Has this function been tested?

[TSC21]

mavros is a global namespace. Have you tried /mavros/local_position/tf/send instead?

[TSC21]

Also, the local_position tf will only be published if you actually receive LOCAL_POSITION_NED msgs from the FCU (https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/local_position.cpp#L83). Are you sure you are getting those?

[TSC21]

    for(int j = 0; ros::ok() && j < 500; ++j){
    ...
        nh.setParam("mavros/setpoint_position/tf/listen", true);
        ros::spinOnce();
        rate.sleep();
    ...

What are you using this for? This doesn't make much sense to me (setting params each time on a for loop). You usually use a while loop for this and you don't have to set the params each time, only once.

[szebedy]

@TSC21 Thank you for looking into the code.

I tried /mavros/local_position/tf/send instead of mavros/local_position/tf/send, but I still can't see the transforms with rosrun tf view_frames or rosrun rqt_tf_tree rqt_tf_tree.

I do get into the callback function from this subscription: local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, local_position_cb) I can even broadcast the transforms in the callback function with a tf2 broadcaster. That means I do get the LOCAL_POSITION_NED msgs from the FCU right?

Regarding the while loop you're right, I omitted too much, but I only set it to true there if the visual marker has been found. Otherwise it just stays true. But it does not work even if I just set it true in the beginning.

[szebedy]

I updated the code in my second commend based on what we discussed here. Unfortunately I still have the two problems (/mavros/local_position/tf/send and /mavros/setpoint_position/tf/listen do not work)

[TSC21]

@szebedy have you tried just by default run MAVROS and check if you can get the transforms by setting tf/send to true? Exclude your code and check if it works please.

[TSC21]

Also, try using getParam() to check the value of the params.

[szebedy]

@TSC21 getParam() always returns the expected value. Whatever I set the parameters (like /mavros/local_position/tf/send or /mavros/local_position/tf/child_frame_id) to, I always get them back.

Since I do need at least a flight controller (physical or simulated) so that I get the position updates, I tried mavros on the following platforms:

1. On an Intel Aero RTF Drone with px4 on the flight controller launching with roslaunch mavros px4.launch fcu_url:=tcp://127.0.0.1:5760

2. With a simulated flight controller in gazebo and launching with roslaunch px4 mavros_posix_sitl.launch

How exactly do you mean running MAVROS by default? I also tried stripping the code to the bare minimum for mavros tf to work, but I still got nothing showing up in tf.

[szebedy]

@TSC21 Are you sure this is not a bug on the mavros side?

1. Some conversions like local_origin -> local_origin_ned do show up in tf, therefore tf is working.
2. I get the /mavros/local_position/pose updates, therefore I assume the handle_local_position_ned function is called. (https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/local_position.cpp#L83)
3. The /mavros/local_position/tf parameters are set to the correct value (verified with getParam()).

I don't know what else I could do from the calling side.

[jannsta1]

Maybe the confusion here is that if /mavros/global/position/tf/send is true then the _mapglobal tf frame is sent and the _maplocal tf frame is not sent, even if /mavros/local/position/tf/send is set to true.

I'm not sure what the intended behaviour is but this is just something I observed while trying to figure out how to use tf frames with mavros (some general documentation on this would be welcome ;)).

[jannsta1]

Oh wait - I think the problem I was having was because I modified the tf -> frame_id parameter a while ago such that they are different for local_position and global_position tf messages. Although the issue described here could be due to the global_position and local_position tf frames writing to the same frame_id which perhaps causes confusion if /mavros/global_position/tf/send and /mavros/local_position/tf/send parameters are both set to true.

[nshahrestani]

Just in case anyone else stumbles upon this thread like I did, I found that setting /mavros/local_position/tf/send to 'true' via the parameter server did not result in the TF being published, but modifying the same value in the corresponding yaml config file did result in expected behavior.