Add topics for orientation control setpoint.

[vooon]

No description provided.

[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:
#33 (comment)

[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?

[TSC21]

@LorenzMeier already sent you an e-mail with the link for downloading the VM ;)
Let me know if you need anything else! Thanks!

Guys, if anyone else wants this image just send me a PM. I have been with some work on the odroid side, trying to get a wifi dual-band dongle working with the Linaro 12.11 image (I'll be probably using the Fully Loaded image, with Unity desktop - I was able to get it with a 3.8 kernel and now just have to test if the vision pkgs + mavros are properly working on it; Maybe then I will also create a custom Odroid image to share with you and in the Offboard PX4 tutorials).

[LorenzMeier]

Here is the state Glenn had success with:
PX4/PX4-Autopilot#1221

[vooon]

Warn: @TSC21 send to me another setpoint* plugins, so in generalization i will change setpoint_position name space.

[TSC21]

@vooon vision plugin just works if I add the prefix / to vision in the case of the poses.

So it stays like this:
vision_sub = sp_nh.subscribe("/vision", 10, &VisionPositionPlugin::vision_cov_cb, this);
vision_sub = sp_nh.subscribe("/vision", 10, &VisionPositionPlugin::vision_cb, this);

[vooon]

@TSC21 Moving vision to / is bad thing, ensure that you use /mavros/position/vision in other side.
Note ~ expands as node name (/mavros for defaults).

[TSC21]

So in the vision pose estimator pkg I must remap, for example, /pose to /mavros/position/vision. Is that what you mean?

[mhkabir]

Yep, that's what you should do.

On Wed, Aug 6, 2014 at 8:42 PM, TSC21 notifications@github.com wrote:

So in the vision pose estimator pkg I must remap, for example, /pose to
/mavros/position/vision. Is that what you mean?

—
Reply to this email directly or view it on GitHub
#33 (comment).

[TSC21]

Welcome @mhkabir :D Should be done then!
Anyway, when the upgrade to tf2 is done, any prefix / must be eliminated.

[TSC21]

Currently message update in bound with #94.

[vooon]

Note: message name updated for mavlink sub-rev4 (#97).

[mhkabir]

@vooon This works fine, as tested by users.

[vooon]

@mhkabir Yeah, but @TSC21 some time ago wanted to keep issue opened for discussion.
Maybe now we can close?

[TSC21]

Can close now. Doing some restruct on my linux machine now. Will be back at the project soon ;)