How to find MAVROS topic definitions and use their variables in C++

Hi, I am trying to develop an offboard controller in C++ that uses local position data to issue new commands, with pseudo code such as “if local position x > 5 move to local position y = 5”.

I am using latest source of PX4, trying to develop using SITL with Gazebo no ROS noetic. The issue that I am having is finding the appropriate definitions in the many libraries between ROS, PX4, and MAVROS.

In the MAVROS offboard control example: MAVROS Offboard control example (C++) | PX4 User Guide, the code uses the topic “/mavros/setpoint_position/local” to tell the drone to fly upwards 2 meters. You can play around with these lines in C++ to get the drone to fly to wherever you want, but the code itself is incredibly confusing. To tell the drone to fly upwards we use the line:

pose.pose.position.z = 2;

But I have searched everywhere and cannot find where the definition for this line comes from. A lot of people indicate that using this reference: geometry_msgs/PoseStamped Documentation is explanatory, but all I get from it is an ominous “Header header, pose pose.”

So how does a new developer learn that you must use the code pose.pose.position.z to direct the drone to a coordinate without this example? If you investigate any of the header files you see nothing about position.z.

Which brings me to my main question: I would like to be able to use the published topic “/mavros/local_position/pose” to get active position data from PX4. I can view in a ROS terminal the current local position via this topic, but I cannot find anywhere that indicates how to call the position float variables from inside the topic to inside the source code. I have examined the local_position.cpp file, but cannot find anything relevant.

To be clear, this isn’t a question about roscpp or ROS in general, I would just like to know how to find the relevant definitions of a MAVROS topic so that it can be used in development. I should be able to grab position data in the same way that I can send out commands to go to a position. Clearly pose.pose.position.x came from somewhere.

Hi @bkearney ,

Well, that is standard ROS messaging format, PoseStamped is a message with two fields, one is header, of type std_msgs/Header (yes, messages are often composed by other messages) which is used to set the timestamp of the message, the other is pose, of type geometry_msgs/Pose, which contains the actual data: position and orientation. Lastly, position has three float fields, x, y, z. You want to set the position fields therefore you use pose.pose.position.x, pose.pose.position.y, pose.pose.position.z.

Right here

setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this); creates the /mavros/setpoint_position/local subscriber and then

void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
	{
		Eigen::Affine3d tr;
		tf::poseMsgToEigen(req->pose, tr);

		send_position_target(req->header.stamp, tr);
	}

is the callback that is called to send the data to PX4. First the message (pose field) is converted to a Eigen::Affine3d object, which is just another way to represent position and orientation, then send_position_target send the message using the timestamp contained in the header.

You have a ROS topic, you just have to subscribe to it in you code.

This IS a general ROS question: you know that the position of the drone is shared through the topic /mavros/local_position/pose, you know that that topic is of type geometry_msgs/PoseStamped. You don’t need anything else. geometry_msgs/PoseStamped has pose, pose has position, position has your three coordinates in ROS local frame (why local? because the topic name says so). Nevertheless, the message is published by this file

and you can see how the message is composed.