Add topics for orientation control setpoint.

[mhkabir]

Can you please add support for position control setpoint too :)

[vooon]

Could you list messages what you want?

[mhkabir]

The positon control messages will be implemented in Mavlink Rev. 4 Might take some time... I'll update you then.

Anyway, do you plan to do the orientation control plugin in recent future? Otherwise i will do it for you :D Will you implement a custom message type? Tell me your plan ;)

[vooon]

Now I do not have UAV, only APM (waiting Bixler 2) and later pixhawk (when 3DR will again send a parcel to Russia).

While this plugin is delayed, I will gladly accept your contribution.

ROS API: I guided by the principle: the use of standard type wherever possible. For example, I think geometry_msgs/Pose (or PoseStamped) is suitable for position control message.

[TSC21]

@vooon I sent you an e-mail with a possible code that integrates this with a vision plugin. If you can do or point out corrections to make it work it would be good. I think the code is self explanatory of what is needed.

[vooon]

@TSC21 Recently received only from Nuno. Send again please.

Update: i think i must cleanup first.

[TSC21]

@TSC21 Recently received only from Nuno. Send again please.

Update: i think i must cleanup first.

I'm Nuno ;) sorry I forgot to present my self. Yeah definetly a cleanup must be done, since most of the code is based on the other plugins and it was difficult to me to figure the purpose of some parts of the code. I'm up to help you figure what's the purpose of some things I added to the plugin, so that topics like /local_position_NED and /local_position_ENU (which is a convertion from NED coming from px4 relative to a /local_origin) may be acessible by a motion planner running on development pc. A /tf between this (which we can consider the /base_link frame) and /map or /local_origin will be ideal. Or maybe send it as geometry_msgs/PoseWithCovariance.

[vooon]

1. rviz things must be in separate node
2. later cleaned plugin may be added to libmavros_plugins.so

Now i'm trying Glenn's code (for understanding). Playing with rviz (and did not work, mav_basic node). Think it require TF...

[TSC21]

Yeah maybe it's better to put rviz things in a separate node. But then the plugin must be able to send /local_pos_NED or equivalent /local_pos_ENU (as a /tf or a geometry_msgs/Point) and the pose of the mav (based on it's attitude -> send this as a PoseWithCovarianceStamped). The /tf should be between /base_link (or /mav) and a base_frame of our choice (p.e. /odom->/base_link, /map->/base_link).

Also receive a /tf or a geometry_msg/Pose or PoseWithCoriance. Any of it should come from a planner to send through SET_LOCAL_POSITION_SETPOINT msg and one coming from a visual odometry/ pose estimation/ SLAM system to send through VISION_POSITION_ESTIMATE msg. In the case of a /tf, it should be through a lookup transform between /base_link and /map (or /local_origin). The planning frame should be sync with the local frame of the mav.

What's not working? You need mav_tracker.launch to be running to give you the tf between /local_origin and /ar_marker_8 and /ar_marker_8 and /camera (which can be /camera_link if using openni driver with a kinect type camera). Also there's a slight change I think that has to be made in px4_viz.cpp related to a tf that is being made between the marker and px4 (or camera and px4 not really certain). It has to be mav not px4 I think. But better to ask Glenn directly ;) I'm available to help also.

--- Mensagem Original ---

De: "Vladimir Ermakov" notifications@github.com Enviado: 11 de Julho de 2014 21:03 Para: "vooon/mavros" mavros@noreply.github.com Cc: "TSC21" n.marques21@hotmail.com Assunto: Re: [mavros] Add topics for orientation control setpoint. (#33)

1. rviz things must be in separate node
2. later cleaned plugin may be added to libmavros_plugins.so

Now i'm trying Glenn's code (for understanding). Playing with rviz (and did not work, mav_basic node). Think it require TF...

---

Reply to this email directly or view it on GitHub: https://github.com/vooon/mavros/issues/33#issuecomment-48776468

[ggregory8]

Hi guys,

I was hoping to get my node running in mavros today but have run out of time. Thanks for supporting ROS hydro with boost 1.46 :)

I agree that the different functionality should be split into multiple plugins:

• One to read position and pose information from autopilot to add visualisation to RViz
• One to read the pose/position topic from respective vision estimate package and then covert to the correct coordinate frame and then send to autopilot via VISION_POSITION_ESTIMATE. This would need to be dynamic and have parameters to allow configuration for different vision estimate packages and frames used. I.e ar_track_alvar provides a pose or TF of the marker relative to the camera, but what we want to actually send to the autopilot will be the MAV relative to the local_origin.
• Others to handle mission planning etc.

mav_basic was a node that simply added visualisation (four circles and an arrow) to visualize the quad in RViz at the /mav frame. I had static transforms to define /mav and /local_origin positions relative to /camera and /ar_marker_8 respectively.

[vooon]

Finally forced to work rviz_imu_plugin (needed static_transform_publisher for /fcu). And it looks like I have not correctly convert coordinate frame.

Roll on X, Pitch on Y, Yaw on Z (match).

Strange: imu_filter_madgwick connected to mag & data_raw from mavros don't post anything on /imu/data. Update: disable mag and it works, but then it goes mad (and laggy: ~2 fps).

[vooon]

Update: orientation is correct (i forget that ENU: forward X, while on NED: forward Y).

[vooon]

Conversions are fine: http://www.av8n.com/physics/coords.htm (axes icon on APM confusing me). But i don't understand why acceleration vector points up.

[TSC21]

Hi @vooon,

What accel vector is that? Linear Z accel? The marker you are creating is using what attitude data?

[vooon]

This vector created by rviz_imu_plugin, Yes it is Z.

@TSC21, @ggregory8 can you test local_position plugin?

Update: doc http://wiki.ros.org/mavros#local_position

[ggregory8]

Once I get mavros running properly (see my latest issue) ill test this tomorrow.

[TSC21]

What are the test conditions? I'm with PX4 stack, master firmware version.

Echoing /mavros/position/local:

header: seq: 137 stamp: secs: 1405182181 nsecs: 504166956 frame_id: fcu pose: position: x: -10.8664245605 y: 1.39578604698 z: 7.72344923019 orientation: x: 3.47663345355e-310 y: 1.36240281664e-317 z: 9.88131291682e-324 w: 1.0

Position is drifting as hell relative to the local_origin, which I think is normal since we are just using IMU based attitude to positioning the vehicle right?

But why does the orientation doesn't change?

But it seems to be working. We need now to establish a plugin to send a vision_position_estimate to the fcu, so it can give us a more plausible pose.

[vooon]

Try view in rviz (take screenshot). I think orientation computed from wrong source, but because i can not test i use your code. Checked out Glenn's code, he use RPY data. Seems that was bug.

[TSC21]

So what orientation source were you using? Here is the screenshot:

Just for reference, I didn't move the quad and is still on the same position since I connected the battery, so yeah it's drifting. I'll checkout your new commit.

[TSC21]

Tested: not working. Got the following when echoing the /position/local topic:

header: seq: 101 stamp: secs: 1405187177 nsecs: 379489215 frame_id: fcu pose: position: x: 8.28247642517 y: -2.2185151577 z: 2.90728497505 orientation: x: nan y: nan z: nan w: nan

No orientation source then... What did you change in the code?

[vooon]

Strange. Orientation computed from ATTITUDE RPY angles (imu_pub), stores in UAS. Check imu/data.

[TSC21]

Here it is:

header: seq: 105 stamp: secs: 1405187681 nsecs: 474430007 frame_id: fcu orientation: x: 0.00578563750961 y: -0.00189045150751 z: 0.981920095113 w: 0.189197989961 orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] angular_velocity: x: -0.00115376268514 y: -0.00281615415588 z: 0.000289604155114 angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07] linear_acceleration: x: -0.0226740501821 y: 0.0292340740561 z: 9.76953697205 linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

Seems good no?

[vooon]

Imu data seems valid, pose must have same values...

[TSC21]

Yeah it's working now! Didn't no why it wasn't before:

header: seq: 1907 stamp: secs: 1405187779 nsecs: 955288985 frame_id: fcu pose: position: x: 5.43731975555 y: 0.502503752708 z: 1.68717598915 orientation: x: 0.00577745355177 y: -0.0018961479381 z: 0.981934983468 w: 0.189120897561

Still getting extreme drifts, as normal.

We need to get some visual pose reference to the board now, through VISION_POSITION_ESTIMATE msg. Can you get a external_pose.cpp or something like that to receive external pose estimations to get them to the PX4 code, or are you planning to integrate that on the local_position.cpp?

Allow it to receive a PoseWithCovariance or PoseStamped and a tf between 2 frames from user choice (parameters: base_frame and fixed_frame).

I'll be doing the tests for you since you don't have the hardware yet.

[TSC21]

Here are the pose estimation sources that are interesting to receive messages from:

http://wiki.ros.org/ar_track_alvar

• set static_transform from /marker to /local_origin (local_origin as the launching point of the MAV)
• get transform from /marker to /camera (/camera_link to kinect type cameras - given by the pkg)
• set static_transform from /camera_link to /fcu (depends on camera position and orientation relative to the FCU - can be a parameter defined by user on cmd line when rosrun)
• get transform from /marker to /fcu
• send position and orientation of /fcu given transform from /marker to /fcu

http://felixendres.github.io/rgbdslam_v2

• set static_transform from /map to /local_origin (local_origin as the launching point of the MAV)
• get transform from /map to /camera_link (given by rgbdslam)
• set static_transform from /camera_link to /fcu (depends on camera position and orientation relative to the FCU - can be a parameter defined by user on cmd line when rosrun)
• get transform from /map to /fcu
• send position and orientation of /fcu given transform from /map to /fcu

http://wiki.ros.org/ccny_rgbd

• subscribe /pose to receive geometry_msgs/PoseStamped from /fcu current pose according to the odometer (has to have a relation with /local_origin) or,
• set static_transform from /odom to /local_origin (local_origin as the launching point of the MAV)
• get transform from /odom to /camera_link (given by ccny_rgbd)
• set static_transform from /camera_link to /fcu (depends on camera position and orientation relative to the FCU - can be a parameter defined by user on cmd line when rosrun)
• get transform from /odom to /fcu
• send position and orientation of /fcu given transform from /odom to /fcu

http://wiki.ros.org/fovis

• subscribe /pose to receive geometry_msgs/PoseStamped from /fcu current pose according to the odometer (has to have a relation with /local_origin) or,
• get transform from /odom to /camera_link (given by fovis)
• set static_transform from /camera_link to /fcu
• get transform from /odom to /fcu
• send position and orientation of /fcu given transform from /odom to /fcu

http://wiki.ros.org/viso2_ros

https://vision.in.tum.de/data/software/dvo

• subscribe /rgbd/pose to receive geometry_msgs/PoseStamped from /fcu current pose according to the odometer (has to have a relation with /local_origin)

[vooon]

I think SETPOINT's and VISION_POSITION_ESTIMATE send can be in local_position.cpp (related to position task). But i did not decide API for this messages (seometry_msgs/PoseStamped?).

[TSC21]

SET_LOCAL_POSITION_SETPOINT (https://pixhawk.ethz.ch/mavlink/#SET_LOCAL_POSITION_SETPOINT) should read a geometry_msgs/PoseStamped but it will only get X, Y, Z and Yaw.

VISION_POSITION_ESTIMATE (https://pixhawk.ethz.ch/mavlink/#VISION_POSITION_ESTIMATE) should be able to both read geometry_msgs/PoseStamped or use a TransformListener::lookupTransform to check for a StampedTransform between the /fcu frame given /odom or /map, as I told you in the comment above (most of these pkgs give a /tf instead of a pose, as you can see). Some of the above steps are equal to different pkgs so just have to add the same code for different pkgs.

Then you can use tf::poseTFToMsg and convert it to a PoseStamped msg, or just get the data directly from the StampedTransform.

[TSC21]

@vooon any update one this?

[vooon]

I hope I can finish tomorrow.

[TSC21]

Great! :D any help I can give? There's another message I think must be added: SAFETY_SET_ALLOWED_AREA (https://pixhawk.ethz.ch/mavlink/#SAFETY_SET_ALLOWED_AREA).

For most planners there's a planning area that must be considered, and in this case limiting the movement of the quad to a certain planning area is a enhancement that may be considered essential in terms of security for example.

In this case parameters can be added to give the coordinates of two opposite corners of a cube (have to check if it can be a quadrangular prism more than a cube -> rooms don't have to be cubic)

[vooon]

Think better find message for that thing (two Point + custom info field) or define new one.

[TSC21]

In that case it must be a message with a header like this: (like geometry_msgs/PointStamped)

This represents two Points with reference coordinate frame and timestamp

Header header Point point_corner1 Point point_corner2

and both points as: float64 x float64 y float64 z

[mhkabir]

A point of importance here : We are implementing plugins t o send messages, but please be aware that most messages are not implemented in the PX4 stack, and not in master.

[TSC21]

Yes that's why I'm asking in the px4users group for that implementation.

[mhkabir]

My suggestions for plugins (what I've coded) :

Pose plugin - sends fused pose estimates to PX4 Orientation plugin - sends YPRT setpoints to PX4 Position plugin - sends XYZ position setpoints to PX4

Keep it simple, and then we can expand from there. Integration of too many plugins into mavros makes no sense. For specialized plugins, just roll your own and keep it separated from mavros source ;)

[TSC21]

The pose plugin get's tf's and posestamped mgs?

Not talking about a specialized plugin when talking about the allowed_area. Just something that I think must be integrated with the Position plugin.

[mhkabir]

Yes, the pose plugin receives TFs and posestamped (two topics : /pose_input_ENU and /pose_input_NED for modular design) or could set a parameter and one input topic.

Allowed_area would actually not improve safety in out present unstable situation. Better to get flying and we can integrate enhancements.

Also, ferrying back local position data from the PX4 makes no sense unless one is using a distributed EKF setup. In that case, the EKF data could be transported back. One should ideally forward all sensor data to a HL computer and do the position fusion there. Pathplanner, etc. should use that estimate. I will personally be disabling positon fusion on the PX4. I'm forwarding all data to the Odroid, including optical flow and using an EKF there for fusion.

Kabir

[TSC21]

Kabir are you getting a proper fusion between optical flow and attitude data coming from PX4? Don't see where's the problem of using the PX4 EKF, since I think it already merges the IMU data, flow data and estimation coming offboard. Not saying that it is important, but allowed_area still is a enhancement, not a essential feature.

[mhkabir]

PX4 isn't using EKF on master, as of now I think. Also, flow fusion, etc. is not done on the EKF as of now.

There is no problem of using the PX4 EKF, but fusing IMU data into visual gives better estimates.

[mhkabir]

I haven't tested flow in my fusion yet as I need to compute noise values and add some support in the framework, but it should be done soon. Presently using vision, IMU, baro, mag fused. Will be adding something for sonar, rangefinder and pointclouds ( kinect, stereo, etc.) soon I think.

[TSC21]

So what you are saying is that, for now, we should rely on a pose fusion running on a offboard computer since PX4 EKF doesn't do IMU+visual_pose+flow fusion, right? Is that confirmed? Anyway, what's your result till now?

[mhkabir]

PX4's fusion should work find, but it doesn't use an EKF and there is no time-delay compensation, etc. Depends on how robust the system has to be for you.

Its does do IMU+visual_pose+flow fusion, but the robustness is questionable as I haven't flown it yet and saw lots of drift with ground tests. Bad vision rates also make the local_xyz outputs go crazy, which will probably make it difficult to fly.

I'm working on getting my vision source working for now. Some stupid memory allocation problems have been impeding my progress and I've been forced to downgrade to 12.11 on the Odroid.

[TSC21]

I'll be glad to help with those. I have already a planner configured so right now I just need a proper pose and position estimate. I have pixhawk+asus xtion+px4flow+odroid with linaro 12.11 config. No vision only, but can get visual odemetry from the SLAM algorithm I'm using. My time is tight since it's a work for my MSc thesis. How can I collab with you? By the way, @vooon nice work with the plugins so far. I will be glad to test them when they are ready.

[mhkabir]

Please mail me at mhkabir98@gmail.com. Having a second hand should make this faster ;)

BTW, how did you install the mavlink submodule on 12.11? Source?

[TSC21]

Done! Still, I think we should give a try with the PX4 estimator. Like connecting the px4flow to TELEM2 and sending the vision_position_estimator through the plugin when it's ready. Anyway, @mhkabir if you subscribe a topic like /pose and use the data in the framework you are using, will you still need to adapt the framework to receive pointclouds when there's some other algorithm that already does that? You just have to fuse poses right? Take a look at http://felixendres.github.io/rgbdslam_v2 and see if you can fuse the pose given from a tf lookup with your fusion framework.

[TSC21]

@mhkabir did you receive my e-mail?

[BEmran]

Hi, I wrote a plugin which sends a message to the VISION_POSITION_ESTIMATE using this functions:

mavlink_msg_vision_position_estimate_pack(0, 0, &msg,usec,x,y,z,0,0,0); uas->mav_link->send_message(&msg);

what I noticed is the follwing, the LOCAL_POSITION_NED follows the x and y data I sent exactly. However, the z value of the LOCAL_POSITION_NED doesn't change or follow what ever I sent. It only changes when the IMU data change (the pix4/Quadrotor moves). Does any one have any idea why ?

[mhkabir]

Z data is fused with baro in PX4's estimator, which has some drift. Moving the quad up and down changes baro measurements...

On Mon, Jul 14, 2014 at 3:05 PM, Emran notifications@github.com wrote:

Hi, I wrote a plugin which sends a message to the VISION_POSITION_ESTIMATE using this functions:

mavlink_msg_vision_position_estimate_pack(0, 0, &msg,usec,x,y,z,0,0,0); uas->mav_link->send_message(&msg);

what I noticed is the follwing, the LOCAL_POSITION_NED follows the x and y data I sent exactly. However, the z value of the LOCAL_POSITION_NED doesn't change or follow what ever I sent. It only changes when the IMU data change (the pix4/Quadrotor moves). Does any one have any idea why ?

— Reply to this email directly or view it on GitHub https://github.com/vooon/mavros/issues/33#issuecomment-48880976.

[BEmran]

Thanks for your replying Kabir. But the z-axis information coming from the vision system and sent to the VISION_POSITION_ESTIMATE is totally ignored. Won't it be better if the PX4's estimator depends on information from the vision system than the baro reading ?

[TSC21]

It would be better if it could fuse this info. And better than baro would be using the px4flow maxbotix sonar data. Do you have one to do that test?