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

Orientation enum refactoring #341

Closed
wants to merge 9 commits into from
Closed

Orientation enum refactoring #341

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
77de35d
uas_sensor_orientation: return the resulting orientation in rads
TSC21 Jul 16, 2015
fb215ae
uas_sensor_orientation: use Eigen::Vector3d instead of Vector3
TSC21 Jul 16, 2015
88ba73d
distance_sensor: corrected the transform direction (fcu -> sensor)
TSC21 Jul 16, 2015
4fd1ab5
uas_sensor_orientation: uncrustify
TSC21 Jul 16, 2015
5de5c82
distance_sensor: use synchronized_header instead of synchronized_stamp
TSC21 Jul 16, 2015
6ce849f
uas_sensor_orientation: return quaternion instead of RPY
TSC21 Jul 16, 2015
5003940
uas_sensor_orientation: remove static prefix from sensor_orientation …
TSC21 Jul 16, 2015
3c3bac7
uas_sensor_orientation: return identity quaternion if no orientation …
TSC21 Jul 16, 2015
39e4389
uas_sensor_orientation: typo correction
TSC21 Jul 16, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion mavros/include/mavros/mavros_uas.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -333,7 +333,7 @@ class UAS {
* @brief Function to match the received orientation received by DISTANCE_SENSOR msg
* and the rotation of the sensor relative to the FCU.
*/
static tf::Vector3 sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);
static Eigen::Vector3d sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);

/* -*- frame conversion utilities -*- */

Expand Down
93 changes: 47 additions & 46 deletions mavros/src/lib/uas_sensor_orientation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,63 +14,64 @@
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#include <array>
#include <angles/angles.h>
#include <mavros/mavros_uas.h>

using namespace mavros;

// XXX do we need combine that with string representation?
/** @todo Combine the bellow with string representation */

static const std::array<const tf::Vector3, 39> sensor_orientation = {
/* 0 */ tf::Vector3(0.0, 0.0, 0.0),
/* 1 */ tf::Vector3(0.0, 0.0, 45.0),
/* 2 */ tf::Vector3(0.0, 0.0, 90.0),
/* 3 */ tf::Vector3(0.0, 0.0, 135.0),
/* 4 */ tf::Vector3(0.0, 0.0, 180.0),
/* 5 */ tf::Vector3(0.0, 0.0, 225.0),
/* 6 */ tf::Vector3(0.0, 0.0, 270.0),
/* 7 */ tf::Vector3(0.0, 0.0, 315.0),
/* 8 */ tf::Vector3(180.0, 0.0, 0.0),
/* 9 */ tf::Vector3(180.0, 0.0, 45.0),
/* 10 */ tf::Vector3(180.0, 0.0, 90.0),
/* 11 */ tf::Vector3(180.0, 0.0, 135.0),
/* 12 */ tf::Vector3(0.0, 180.0, 0.0),
/* 13 */ tf::Vector3(180.0, 0.0, 225.0),
/* 14 */ tf::Vector3(180.0, 0.0, 270.0),
/* 15 */ tf::Vector3(180.0, 0.0, 315.0),
/* 16 */ tf::Vector3(90.0, 0.0, 0.0),
/* 17 */ tf::Vector3(90.0, 0.0, 45.0),
/* 18 */ tf::Vector3(90.0, 0.0, 90.0),
/* 19 */ tf::Vector3(90.0, 0.0, 135.0),
/* 20 */ tf::Vector3(270.0, 0.0, 0.0),
/* 21 */ tf::Vector3(270.0, 0.0, 45.0),
/* 22 */ tf::Vector3(270.0, 0.0, 90.0),
/* 23 */ tf::Vector3(270.0, 0.0, 135.0),
/* 24 */ tf::Vector3(0.0, 90.0, 0.0),
/* 25 */ tf::Vector3(0.0, 270.0, 0.0),
/* 26 */ tf::Vector3(0.0, 180.0, 90.0),
/* 27 */ tf::Vector3(0.0, 180.0, 270.0),
/* 28 */ tf::Vector3(90.0, 90.0, 0.0),
/* 29 */ tf::Vector3(180.0, 90.0, 0.0),
/* 30 */ tf::Vector3(270.0, 90.0, 0.0),
/* 31 */ tf::Vector3(90.0, 180.0, 0.0),
/* 32 */ tf::Vector3(270.0, 180.0, 0.0),
/* 33 */ tf::Vector3(90.0, 270.0, 0.0),
/* 34 */ tf::Vector3(180.0, 270.0, 0.0),
/* 35 */ tf::Vector3(270.0, 270.0, 0.0),
/* 36 */ tf::Vector3(90.0, 180.0, 90.0),
/* 37 */ tf::Vector3(90.0, 0.0, 270.0),
/* 38 */ tf::Vector3(315.0, 315.0, 315.0)
static const std::array<const Eigen::Vector3d, 39> sensor_orientation = {
Copy link
Member

Choose a reason for hiding this comment

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

Really that used once and Eigen::Quaterniond is more usable form of rotation.
So i want redo this at all.
My steps:

  1. make static initializer function, maybe constexpr (if eigen can handle it)
    this function will be used only in that module, so it should be static.
  2. replace all calls to that func

Copy link
Member Author

Choose a reason for hiding this comment

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

Really that used once and Eigen::Quaterniond is more usable form of rotation.

Well I can't really see the point, cause if you want you can easily convert it to quaternions.

Copy link
Member

Choose a reason for hiding this comment

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

static Eigen::Quaterniond make_orientation(const double x, const double y, const double z)
{
    Eigen::Vector3d v = Eigen::Vector3d(x, y, z) * DEG_TO_RAD;
    return UAS::quaternion_from_rpy(v);
}

Copy link
Member Author

Choose a reason for hiding this comment

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

static Eigen::Quaterniond make_orientation(const double x, const double y, const double z)
{
    Eigen::Vector3d v = Eigen::Vector3d(x, y, z) * DEG_TO_RAD;
    return UAS::quaternion_from_rpy(v);
}

Are we supposed to add that to mavros_uas.h?

Copy link
Member

Choose a reason for hiding this comment

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

WHY!? static function in uas_sensor_orientation.cpp!

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!? static function in uas_sensor_orientation.cpp!

? What are you referring to?

/* 0 */ Eigen::Vector3d(0.0, 0.0, 0.0),
/* 1 */ Eigen::Vector3d(0.0, 0.0, 45.0),
/* 2 */ Eigen::Vector3d(0.0, 0.0, 90.0),
/* 3 */ Eigen::Vector3d(0.0, 0.0, 135.0),
/* 4 */ Eigen::Vector3d(0.0, 0.0, 180.0),
/* 5 */ Eigen::Vector3d(0.0, 0.0, 225.0),
/* 6 */ Eigen::Vector3d(0.0, 0.0, 270.0),
/* 7 */ Eigen::Vector3d(0.0, 0.0, 315.0),
/* 8 */ Eigen::Vector3d(180.0, 0.0, 0.0),
/* 9 */ Eigen::Vector3d(180.0, 0.0, 45.0),
/* 10 */ Eigen::Vector3d(180.0, 0.0, 90.0),
/* 11 */ Eigen::Vector3d(180.0, 0.0, 135.0),
/* 12 */ Eigen::Vector3d(0.0, 180.0, 0.0),
/* 13 */ Eigen::Vector3d(180.0, 0.0, 225.0),
/* 14 */ Eigen::Vector3d(180.0, 0.0, 270.0),
/* 15 */ Eigen::Vector3d(180.0, 0.0, 315.0),
/* 16 */ Eigen::Vector3d(90.0, 0.0, 0.0),
/* 17 */ Eigen::Vector3d(90.0, 0.0, 45.0),
/* 18 */ Eigen::Vector3d(90.0, 0.0, 90.0),
/* 19 */ Eigen::Vector3d(90.0, 0.0, 135.0),
/* 20 */ Eigen::Vector3d(270.0, 0.0, 0.0),
/* 21 */ Eigen::Vector3d(270.0, 0.0, 45.0),
/* 22 */ Eigen::Vector3d(270.0, 0.0, 90.0),
/* 23 */ Eigen::Vector3d(270.0, 0.0, 135.0),
/* 24 */ Eigen::Vector3d(0.0, 90.0, 0.0),
/* 25 */ Eigen::Vector3d(0.0, 270.0, 0.0),
/* 26 */ Eigen::Vector3d(0.0, 180.0, 90.0),
/* 27 */ Eigen::Vector3d(0.0, 180.0, 270.0),
/* 28 */ Eigen::Vector3d(90.0, 90.0, 0.0),
/* 29 */ Eigen::Vector3d(180.0, 90.0, 0.0),
/* 30 */ Eigen::Vector3d(270.0, 90.0, 0.0),
/* 31 */ Eigen::Vector3d(90.0, 180.0, 0.0),
/* 32 */ Eigen::Vector3d(270.0, 180.0, 0.0),
/* 33 */ Eigen::Vector3d(90.0, 270.0, 0.0),
/* 34 */ Eigen::Vector3d(180.0, 270.0, 0.0),
/* 35 */ Eigen::Vector3d(270.0, 270.0, 0.0),
/* 36 */ Eigen::Vector3d(90.0, 180.0, 90.0),
/* 37 */ Eigen::Vector3d(90.0, 0.0, 270.0),
/* 38 */ Eigen::Vector3d(315.0, 315.0, 315.0)
};

tf::Vector3 UAS::sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation)
Eigen::Vector3d UAS::sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation)
{
// XXX should it return radians?

size_t idx = size_t(orientation);
if (idx >= sensor_orientation.size()) {
ROS_WARN_NAMED("uas", "SENSOR: wrong orintation index: %zu", idx);
return tf::Vector3();
return Eigen::Vector3d();
}

return sensor_orientation[idx];
return Eigen::Vector3d(angles::from_degrees(sensor_orientation[idx].x()),
Copy link
Member Author

Choose a reason for hiding this comment

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

A thing I can do here is:

return
Eigen::Vector3d(sensor_orientation[idx].x(), sensor_orientation[idx].y(), sensor_orientation[idx].z()) * DEG_TO_RAD;

It's not so extensive and then I don't need angles.h in this case.

Copy link
Member Author

Choose a reason for hiding this comment

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

Or since you want quaternions so bad, I can also do:

return UAS::quaternion_from_rpy(Eigen::Vector3d(sensor_orientation[idx].x(),
                                                sensor_orientation[idx].y(),
                                                sensor_orientation[idx].z()) * DEG_TO_RAD);

Better?

angles::from_degrees(sensor_orientation[idx].y()),
angles::from_degrees(sensor_orientation[idx].z()));
};
8 changes: 2 additions & 6 deletions mavros_extras/src/plugins/distance_sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -222,17 +222,13 @@ class DistanceSensorPlugin : public MavRosPlugin {

if (sensor->send_tf) {
/* variables init */
// XXX #319
auto rpy = UAS::sensor_orientation_matching(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation));
// how it can work, if rpy in degrees, not radians?
auto q = tf::createQuaternionFromRPY(rpy.x(), rpy.y(), rpy.z());
Copy link
Member

Choose a reason for hiding this comment

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

And angles still in degrees, while function i think expects radians.

Copy link
Member Author

Choose a reason for hiding this comment

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

https://github.com/mavlink/mavros/pull/341/files#diff-9a7712c102878d6fe49118acf4ba8462R74

Copy link
Member

Choose a reason for hiding this comment

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

Ugly. Unneeded calculations at every call.

Copy link
Member Author

Choose a reason for hiding this comment

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

Well it will do the same thing if it converts to quaternions...

Copy link
Member Author

Choose a reason for hiding this comment

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

And it just does this once for each msg received.

Copy link
Member Author

Choose a reason for hiding this comment

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

For example Eigen::Vector3d v = Eigen::Vector3d(x, y, z) * DEG_TO_RAD; also does calculations on each call (and are exactly the same as the from_degrees() function). So I don't really see your point. You are forcing a problem where it doesn't exist.

Copy link
Member

Choose a reason for hiding this comment

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

make_orientation() needed to fill const array, so no extra calculations.

Copy link
Member

Choose a reason for hiding this comment

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

And quaternion are used in ros for all orientation/rotation things. I don't see any place where you need euler angles now. But i see where quaternion is usable.
Also to do rotation you steel need make quaternion and then apply it to Affine3d or other Q.

So no reason why store this data uncalculated and do calc at each call. That data is a constant, so just do precalculation.


geometry_msgs::TransformStamped transform;

// @TSC21 revisit that please!
// In TF1 code transform: sensor -> fcu is that true?
transform.header = range->header;
transform.child_frame_id = "fcu";
transform.header = uas->synchronized_header("fcu", dist_sen.time_boot_ms);
transform.child_frame_id = sensor->frame_id;

/* rotation and position set */
tf::quaternionTFToMsg(q, transform.transform.rotation);
Expand Down