Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Transformed attitude to inertial frame for Pose #195

Closed
wants to merge 10 commits into from
Closed

Transformed attitude to inertial frame for Pose #195

wants to merge 10 commits into from

Conversation

mhkabir
Copy link
Member

@mhkabir mhkabir commented Jan 24, 2015

This is for #193
Needs to be tested. Testers welcome. Anyone with a PX4Flow or other accurate position estimator (like VICON) can easily test this.

I'll attach my Rviz config file for the visualization plugin testing.

mhkabir
mhkabir reviewed Jan 24, 2015
View reviewed changes
mavros/src/plugins/local_position.cpp
@@ -100,7 +100,16 @@ class LocalPositionPlugin : public MavRosPlugin {
*/
tf::Transform transform;
transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));
transform.setRotation(uas->get_attitude_orientation());

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add a small comment here to explain what you do

mhkabir
mhkabir reviewed Jan 25, 2015
View reviewed changes
mavros_extras/src/plugins/visualization.cpp
q_body.header.stamp = uas->synchronise_stamp(pos_ned.time_boot_ms);
geometry_msgs::QuaternionStamped q_inertial;
tf::TransformListener listener;
listener.transformQuaternion(child_frame_id, ros::Time::now(), q_body, fixed_frame_id, q_inertial);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why use ros::time::now()?

vooon
vooon reviewed Jan 25, 2015
View reviewed changes
mavros/src/plugins/local_position.cpp
@@ -75,10 +80,11 @@ class LocalPositionPlugin : public MavRosPlugin {

ros::NodeHandle pos_nh;
ros::Publisher local_position;
tf::TransformBroadcaster tf_broadcaster;
boost::shared_ptr<tf::TransformBroadcaster> tf_broadcaster;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is required to make it pointers?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@tonybaltovski You can revert this please. And make listener normal too, without pointer.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not required.

@vooon
Copy link
Member

vooon commented Jan 25, 2015

@mhkabir Just note: as i understand after publish Message::Ptr object should not be used again.
Pointer passed to messaging system (but only nodelet can be copyless) and freed by it.

@mhkabir
Copy link
Member Author

mhkabir commented Jan 26, 2015

@tonybaltovski Many thanks. Thanks for the formatting fix on the visualisation plugin. My bad, haha.
@voon I'll address that soon after this is merged in.

@vooon
Copy link
Member

vooon commented Jan 26, 2015

@mhkabir @tonybaltovski Now looks good to merge. But need test.

@mhkabir Don't see rviz config attachment.

@mhkabir
Copy link
Member Author

mhkabir commented Feb 2, 2015

@vooon @tonybaltovski

Hmm, so this won't even startup :

[ INFO] [1422859571.894910337]: Plugin LocalPosition [alias local_position] loaded and initialized
terminate called after throwing an instance of 'tf2::LookupException'
  what():  "local_origin" passed to lookupTransform argument target_frame does not exist.

Looks like we are looking for the tf even before its published the first time. Workarounds?

@mhkabir
Copy link
Member Author

mhkabir commented Feb 2, 2015

So for the to body -> world, I think we'll need to do it with a matrix. Generalise it in libmavros.

@vooon
Copy link
Member

vooon commented Feb 2, 2015

Seems that tf transformation for local_origin frame don't exist, maybe static transform can fix that?

@mhkabir
Copy link
Member Author

mhkabir commented Feb 2, 2015

But local_origin transform is generated by the plugin itself (from LOCAL_POSITION_NED)

tonybaltovski
tonybaltovski reviewed Feb 2, 2015
View reviewed changes
mavros/src/plugins/local_position.cpp
q.header.frame_id = child_frame_id;
q.header.stamp = uas->synchronise_stamp(pos_ned.time_boot_ms);
tf_listener.transformQuaternion(frame_id, q, q);

tf::Transform transform;
transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Shouldn't this be transform.setOrigin(tf::Vector3(pos_ned.x, -pos_ned.y, -pos_ned.z));

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Local NED is ground frame and this conversion is according to #49

@tonybaltovski
Copy link
Contributor

Isn't the attitude relative to the body frame assuming it has no rotation? The current tf is world -> fcu and then we would need fcu -> fcu_att?

@mhkabir
Copy link
Member Author

mhkabir commented Feb 3, 2015

I think that's it.

@TSC21 TSC21 mentioned this pull request Apr 9, 2015
Closed
@TSC21
Copy link
Member

TSC21 commented Jun 11, 2015

I think this may be solved with #208 .

@mhkabir
Copy link
Member Author

mhkabir commented Jun 12, 2015

Using TF, this isn't working. Closing for now.

@mhkabir mhkabir closed this Jun 12, 2015
nmichael pushed a commit to rislab/mavros that referenced this pull request Mar 19, 2016
@LorenzMeier
Merge pull request mavlink#195 from mavlink/packed_structs
3bee95f 
Force all structs to be packed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
testing required
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
4 participants
@mhkabir @vooon @tonybaltovski @TSC21