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 name #342

Closed
wants to merge 18 commits into from
Closed

Orientation enum name #342

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
d5f736a
uas_sensor_orientation: fill OrientationPair with names
TSC21 Jul 16, 2015
5f06c83
uas_sensor_orientation: added function to retrieve orientation alias …
TSC21 Jul 16, 2015
3e7f786
uas_sensor_orientation: add orientation_from_str;
TSC21 Jul 16, 2015
9b8ece4
distance_sensor: uncrustify
TSC21 Jul 16, 2015
9162a50
px4_config: adapt to distance_sensor params to new features
TSC21 Jul 16, 2015
0462cd9
uas_sensor_orientation: ident correction
TSC21 Jul 16, 2015
08f4e8f
px4_config: list currently available orientations
TSC21 Jul 16, 2015
b44a461
uas_sensor_orientation: removed angles.h include
TSC21 Jul 16, 2015
8fedc4e
uas_sensor_orientation: correct UAS::orientation_from_str() code
TSC21 Jul 17, 2015
73cea49
distance_sensor: restructure orientation matching and verification
TSC21 Jul 17, 2015
1dbaf73
px4_config: removed "" from orientation names
TSC21 Jul 17, 2015
3779217
uas_sensor_orientation: sensor_orientations: tab-to-whitespace ident
TSC21 Jul 17, 2015
ad10689
distance_sensor: comment/ident adjust
TSC21 Jul 17, 2015
ad7a7b4
distance_sensor: reset orientation handling to default
TSC21 Jul 17, 2015
5a7478d
distance_sensor: (re-)construct orientation handling
TSC21 Jul 17, 2015
872fdce
distance_sensor: convert 'position' from Vector3 to Eigen::Vector3d
TSC21 Jul 17, 2015
b9275aa
px4_config: add available sensor orientations reference wiki page
TSC21 Jul 17, 2015
9f92221
distance_sensor: make Travis happy: change param to getParam
TSC21 Jul 17, 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
12 changes: 11 additions & 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 @@ -330,11 +330,21 @@ class UAS {
static std::string str_system_status(enum MAV_STATE st);

/**
* @brief Function to match the received orientation received by DISTANCE_SENSOR msg
* @brief Function to match the received orientation received by MAVLink msg
* and the rotation of the sensor relative to the FCU.
*/
static Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);

/**
* @brief Retrieve alias of the orientation received by MAVLink msg.
*/
static std::string str_sensor_orientation(MAV_SENSOR_ORIENTATION orientation);

/**
* @brief Retrieve sensor orientation number from alias name.
*/
static int orientation_from_str(std::string 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.

Change std::string sensor_orientation to const std::string &sensor_orientation.

Also this function should be capable convert numeric value of enum (e.g. strtol() then check & return).
That needed for backward compatibility.

Copy link
Member Author

Choose a reason for hiding this comment

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

Change std::string sensor_orientation to const std::string &sensor_orientation.

Why? And why not also Eigen::Quaterniond sensor_orientation_matching?

Also this function should be capable convert numeric value of enum (e.g. strtol() then check & return).

Convert to what? MAV_SENSOR_ORIENTATION orientation is supposed to be a number, which is used as an index to return: orientation name in case of UAS::str_sensor_orientation(); and orientation values in case of UAS::sensor_orientation_matching(). And orientation_from_str() was created so we can return the orientation number/index if we give the orientation name. So I think we have all the functionalities we need.

Copy link
Member

Choose a reason for hiding this comment

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

Why?

Because this reduces object copying. What you want change in UAS::sensor_orientation_matching()?

strtol()

Because there may be lots of configs with number representation. Why not just simply try to fall back to old behavior?

Copy link
Member Author

Choose a reason for hiding this comment

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

Because this reduces object copying. What you want change in UAS::sensor_orientation_matching()?

This I undestand, but:

Because there may be lots of configs with number representation. Why not just simply try to fall back to old behavior?

... I can't. Can you code an example please?


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

/**
Expand Down
50 changes: 46 additions & 4 deletions mavros/launch/px4_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,25 +114,67 @@ distance_sensor:
hrlv_ez4_pub:
id: 0
frame_id: "hrlv_ez4_sonar"
orientation: 8 # RPY:{180.0, 0.0, 0.0}
orientation: "ROLL_180" # RPY:{180.0, 0.0, 0.0}
Copy link
Member

Choose a reason for hiding this comment

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

" not necessary, and perhaps better to delete.
Example:

pub:
  orientation: ROLL_180

field_of_view: 0.0 # XXX TODO
send_tf: true
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
lidarlite_pub:
id: 1
frame_id: "lidarlite_laser"
orientation: 8
orientation: "ROLL_180"
field_of_view: 0.0 # XXX TODO
send_tf: true
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
sonar_1_sub:
subscriber: true
id: 2
orientation: 8
orientation: "ROLL_180"
laser_1_sub:
subscriber: true
id: 3
orientation: 8
orientation: "ROLL_180"

## Currently available orientations:
# "NONE"
# "YAW_45"
# "YAW_90"
# "YAW_135"
# "YAW_180"
# "YAW_225"
# "YAW_270"
# "YAW_315"
# "ROLL_180"
# "ROLL_180_YAW_45"
# "ROLL_180_YAW_90"
# "ROLL_180_YAW_135"
# "PITCH_180"
# "ROLL_180_YAW_225"
# "ROLL_180_YAW_270"
# "ROLL_180_YAW_315"
# "ROLL_90"
# "ROLL_90_YAW_45"
# "ROLL_90_YAW_90"
# "ROLL_90_YAW_135"
# "ROLL_270"
# "ROLL_270_YAW_45"
# "ROLL_270_YAW_90"
# "ROLL_270_YAW_135"
# "PITCH_90"
# "PITCH_270"
# "PITCH_180_YAW_90"
# "PITCH_180_YAW_270"
# "ROLL_90_PITCH_90"
# "ROLL_180_PITCH_90"
# "ROLL_270_PITCH_90"
# "ROLL_90_PITCH_180"
# "ROLL_270_PITCH_180"
# "ROLL_90_PITCH_270"
# "ROLL_180_PITCH_270"
# "ROLL_270_PITCH_270"
# "ROLL_90_PITCH_180_YAW_90"
# "ROLL_90_YAW_270"
# "ROLL_315_PITCH_315_YAW_315"
##
Copy link
Member

Choose a reason for hiding this comment

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

Tooooo long. That should be in ros wiki (and link to that doc).

Copy link
Member Author

Choose a reason for hiding this comment

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

So add it :)

Copy link
Member

Choose a reason for hiding this comment

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

Why me? You also can edit. WIKI

Copy link
Member Author

Choose a reason for hiding this comment

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

Where do I add it? There's no place for enums there right now and this is not specific for distance_sensor only.

Copy link
Member

Choose a reason for hiding this comment

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

Maybe: http://wiki.ros.org/mavros/OrientationEnum

Copy link
Member Author

Choose a reason for hiding this comment

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

I think it should have a general Enumerations page like http://wiki.ros.org/mavros/Enumerations. And then we add them all there (there's also others like MAV_FRAME).

Copy link
Member

Choose a reason for hiding this comment

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

Ok.


# image_pub
image:
Expand Down
114 changes: 67 additions & 47 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,7 +14,6 @@
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#include <array>
#include <angles/angles.h>
#include <mavros/mavros_uas.h>

#define DEG_TO_RAD (M_PI / 180.0f)
Expand All @@ -26,66 +25,87 @@ typedef std::pair<const std::string, const Eigen::Quaterniond> OrientationPair;

// internal data initializer
static const OrientationPair make_orientation(const std::string &name,
const double roll,
const double pitch,
const double yaw)
const double roll,
const double pitch,
const double yaw)
{
const Eigen::Quaterniond rot = UAS::quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw) * DEG_TO_RAD);
return std::make_pair(name, rot);
}

/** @todo Combine the bellow with string representation */

// Note: usage of double { makes YouCompleteMe happy
static const std::array<const OrientationPair, 39> sensor_orientations = {{
/* 0 */ make_orientation("", 0.0, 0.0, 0.0),
/* 1 */ make_orientation("", 0.0, 0.0, 45.0),
/* 2 */ make_orientation("", 0.0, 0.0, 90.0),
/* 3 */ make_orientation("", 0.0, 0.0, 135.0),
/* 4 */ make_orientation("", 0.0, 0.0, 180.0),
/* 5 */ make_orientation("", 0.0, 0.0, 225.0),
/* 6 */ make_orientation("", 0.0, 0.0, 270.0),
/* 7 */ make_orientation("", 0.0, 0.0, 315.0),
/* 8 */ make_orientation("", 180.0, 0.0, 0.0),
/* 9 */ make_orientation("", 180.0, 0.0, 45.0),
/* 10 */ make_orientation("", 180.0, 0.0, 90.0),
/* 11 */ make_orientation("", 180.0, 0.0, 135.0),
/* 12 */ make_orientation("", 0.0, 180.0, 0.0),
/* 13 */ make_orientation("", 180.0, 0.0, 225.0),
/* 14 */ make_orientation("", 180.0, 0.0, 270.0),
/* 15 */ make_orientation("", 180.0, 0.0, 315.0),
/* 16 */ make_orientation("", 90.0, 0.0, 0.0),
/* 17 */ make_orientation("", 90.0, 0.0, 45.0),
/* 18 */ make_orientation("", 90.0, 0.0, 90.0),
/* 19 */ make_orientation("", 90.0, 0.0, 135.0),
/* 20 */ make_orientation("", 270.0, 0.0, 0.0),
/* 21 */ make_orientation("", 270.0, 0.0, 45.0),
/* 22 */ make_orientation("", 270.0, 0.0, 90.0),
/* 23 */ make_orientation("", 270.0, 0.0, 135.0),
/* 24 */ make_orientation("", 0.0, 90.0, 0.0),
/* 25 */ make_orientation("", 0.0, 270.0, 0.0),
/* 26 */ make_orientation("", 0.0, 180.0, 90.0),
/* 27 */ make_orientation("", 0.0, 180.0, 270.0),
/* 28 */ make_orientation("", 90.0, 90.0, 0.0),
/* 29 */ make_orientation("", 180.0, 90.0, 0.0),
/* 30 */ make_orientation("", 270.0, 90.0, 0.0),
/* 31 */ make_orientation("", 90.0, 180.0, 0.0),
/* 32 */ make_orientation("", 270.0, 180.0, 0.0),
/* 33 */ make_orientation("", 90.0, 270.0, 0.0),
/* 34 */ make_orientation("", 180.0, 270.0, 0.0),
/* 35 */ make_orientation("", 270.0, 270.0, 0.0),
/* 36 */ make_orientation("", 90.0, 180.0, 90.0),
/* 37 */ make_orientation("", 90.0, 0.0, 270.0),
/* 38 */ make_orientation("", 315.0, 315.0, 315.0)
/* 0 */ make_orientation("NONE", 0.0, 0.0, 0.0),
/* 1 */ make_orientation("YAW_45", 0.0, 0.0, 45.0),
/* 2 */ make_orientation("YAW_90", 0.0, 0.0, 90.0),
/* 3 */ make_orientation("YAW_135", 0.0, 0.0, 135.0),
/* 4 */ make_orientation("YAW_180", 0.0, 0.0, 180.0),
/* 5 */ make_orientation("YAW_225", 0.0, 0.0, 225.0),
/* 6 */ make_orientation("YAW_270", 0.0, 0.0, 270.0),
/* 7 */ make_orientation("YAW_315", 0.0, 0.0, 315.0),
/* 8 */ make_orientation("ROLL_180", 180.0, 0.0, 0.0),
/* 9 */ make_orientation("ROLL_180_YAW_45", 180.0, 0.0, 45.0),
/* 10 */ make_orientation("ROLL_180_YAW_90", 180.0, 0.0, 90.0),
/* 11 */ make_orientation("ROLL_180_YAW_135", 180.0, 0.0, 135.0),
/* 12 */ make_orientation("PITCH_180", 0.0, 180.0, 0.0),
/* 13 */ make_orientation("ROLL_180_YAW_225", 180.0, 0.0, 225.0),
/* 14 */ make_orientation("ROLL_180_YAW_270", 180.0, 0.0, 270.0),
/* 15 */ make_orientation("ROLL_180_YAW_315", 180.0, 0.0, 315.0),
/* 16 */ make_orientation("ROLL_90", 90.0, 0.0, 0.0),
/* 17 */ make_orientation("ROLL_90_YAW_45", 90.0, 0.0, 45.0),
/* 18 */ make_orientation("ROLL_90_YAW_90", 90.0, 0.0, 90.0),
/* 19 */ make_orientation("ROLL_90_YAW_135", 90.0, 0.0, 135.0),
Copy link
Member

Choose a reason for hiding this comment

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

Here better to use spaces, else formatting will depend on editor.
Same exception about matrix init code.

/* 20 */ make_orientation("ROLL_270", 270.0, 0.0, 0.0),
/* 21 */ make_orientation("ROLL_270_YAW_45", 270.0, 0.0, 45.0),
/* 22 */ make_orientation("ROLL_270_YAW_90", 270.0, 0.0, 90.0),
/* 23 */ make_orientation("ROLL_270_YAW_135", 270.0, 0.0, 135.0),
/* 24 */ make_orientation("PITCH_90", 0.0, 90.0, 0.0),
/* 25 */ make_orientation("PITCH_270", 0.0, 270.0, 0.0),
/* 26 */ make_orientation("PITCH_180_YAW_90", 0.0, 180.0, 90.0),
/* 27 */ make_orientation("PITCH_180_YAW_270", 0.0, 180.0, 270.0),
/* 28 */ make_orientation("ROLL_90_PITCH_90", 90.0, 90.0, 0.0),
/* 29 */ make_orientation("ROLL_180_PITCH_90", 180.0, 90.0, 0.0),
/* 30 */ make_orientation("ROLL_270_PITCH_90", 270.0, 90.0, 0.0),
/* 31 */ make_orientation("ROLL_90_PITCH_180", 90.0, 180.0, 0.0),
/* 32 */ make_orientation("ROLL_270_PITCH_180", 270.0, 180.0, 0.0),
/* 33 */ make_orientation("ROLL_90_PITCH_270", 90.0, 270.0, 0.0),
/* 34 */ make_orientation("ROLL_180_PITCH_270", 180.0, 270.0, 0.0),
/* 35 */ make_orientation("ROLL_270_PITCH_270", 270.0, 270.0, 0.0),
/* 36 */ make_orientation("ROLL_90_PITCH_180_YAW_90", 90.0, 180.0, 90.0),
/* 37 */ make_orientation("ROLL_90_YAW_270", 90.0, 0.0, 270.0),
/* 38 */ make_orientation("ROLL_315_PITCH_315_YAW_315", 315.0, 315.0, 315.0)
}};

std::string UAS::str_sensor_orientation(MAV_SENSOR_ORIENTATION orientation)
{
size_t idx = size_t(orientation);
if (idx >= sensor_orientations.size()) {
ROS_WARN_NAMED("uas", "SENSOR: wrong orientation index: %zu", idx);
return std::to_string(idx);
}

return sensor_orientations[idx].first;
}

Eigen::Quaterniond UAS::sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation)
{
size_t idx = size_t(orientation);
if (idx >= sensor_orientations.size()) {
ROS_WARN_NAMED("uas", "SENSOR: wrong orintation index: %zu", idx);
ROS_WARN_NAMED("uas", "SENSOR: wrong orientation index: %zu", idx);
return Eigen::Quaterniond::Identity();
}

return sensor_orientations[idx].second;
}

int UAS::orientation_from_str(std::string sensor_orientation)
{
int index, idx = 0;
for (const auto &orientation : sensor_orientations) {
Copy link
Member

Choose a reason for hiding this comment

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

Here you may use regular for, not foreach.
Also not this function do not work, almost always return -1 because it should look that (pseudocode):

for (size_t idx = 0; idx < so.size(); idx++) {
 if (so[idx].first == key)
  return idx;
}
// fallback
v = strtol(key); // this should not fail, so use try {} catch

// not found at all
return -1;

Copy link
Member

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/blob/master/mavros/src/lib/uas_stringify.cpp#L185

Copy link
Member Author

Choose a reason for hiding this comment

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

@vooon why do I need

// fallback
v = strtol(key); // this should not fail, so use try {} catch

?

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 (size_t idx = 0; idx < so.size(); idx++) {
 if (so[idx].first == key)
  return idx;
}

if key is a string, the for loop does a comparison and returns the idx only if that string name exists in the array. If the loop finishes without returning the idx, then it continues and

return -1;

So I don't see why we have to try {} catch a string to long conversion... The function or returns an index or -1...

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/blob/master/mavros/src/lib/uas_stringify.cpp#L185 only makes sense because you are using a map, which requires you to convert mode.first to int. In this case, I can even use int instead of size_t and I'll have my orientation number directly.
Something like this:

int UAS::orientation_from_str(std::string sensor_orientation)
{
    for (int idx = 0; idx < sensor_orientations.size(); idx++) {
        if (sensor_orientations[idx].first == sensor_orientation)
            return idx;
    }

    return -1;
}

if (orientation.first == sensor_orientation)
index = idx;
else
index = -1;
++idx;
} idx = 0;
return index;
}
17 changes: 10 additions & 7 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 @@ -37,7 +37,7 @@ class DistanceSensorItem {
send_tf(false),
sensor_id(0),
field_of_view(0),
orientation(-1),
orientation("ROLL_180"),
covariance(0),
data_index(0)
{ }
Expand All @@ -48,7 +48,7 @@ class DistanceSensorItem {
uint8_t sensor_id; //!< id of the sensor
double field_of_view; //!< FOV of the sensor
tf::Vector3 position; //!< sensor position
Copy link
Member

Choose a reason for hiding this comment

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

That's one field that need to change type: Eigen::Vector3d.

Copy link
Member Author

Choose a reason for hiding this comment

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

OK. But please(!), instead, go to specific lines to change so to orientation handling work ok (that's the purpose of this PR).

int orientation; //!< check orientation of sensor if != -1
std::string orientation;//!< check orientation of sensor if != -1
int covariance; //!< in centimeters, current specification
std::string frame_id; //!< frame id for send

Expand Down Expand Up @@ -191,10 +191,12 @@ class DistanceSensorPlugin : public MavRosPlugin {
return;
}

if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) {
if (UAS::orientation_from_str(sensor->orientation) >= 0 && dist_sen.orientation != UAS::orientation_from_str(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.

Not again! Please don't do expensive calls where you may avoid that.
String comparison costs much more than int. Also that broke behavior with undefined 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.

...
This still is a int comparison... The only thing I can do is store the value previously before do the comparison... Didn't broke nothing

Copy link
Member

Choose a reason for hiding this comment

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

And UAS::orientation_from_str() does not do string comparison?

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 yes. But sensor->orientation is supposed to be a string, that's the purpose. What do you suggest?

Copy link
Member Author

Choose a reason for hiding this comment

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

Ok I know what to do. Will let you know soon

Copy link
Member

Choose a reason for hiding this comment

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

Leave this if as is. Wrong place.

Copy link
Member Author

Choose a reason for hiding this comment

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

I'm not understanding this. orientation param is supposed to be a string (like "LOCAL_NED"). Can you suggest a way of receiving as a string a treat it as an integer? Cause that's what I'm trying to do since ever!

ROS_ERROR_NAMED("distance_sensor",
"DS: %s: received sensor data has different orientation (%d) than in config (%d)!",
sensor->topic_name.c_str(), dist_sen.orientation, sensor->orientation);
"DS: %s: received sensor data has different orientation (%s) than in config (%s)!",
sensor->topic_name.c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation)).c_str(),
sensor->orientation.c_str());
}

auto range = boost::make_shared<sensor_msgs::Range>();
Expand Down Expand Up @@ -263,13 +265,14 @@ void DistanceSensorItem::range_cb(const sensor_msgs::Range::ConstPtr &msg)
msg->range / 1E-2,
type,
sensor_id,
orientation,
UAS::orientation_from_str(orientation),
covariance_);
}

DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *owner, std::string topic_name)
{
auto p = boost::make_shared<DistanceSensorItem>();
auto orientation_value = UAS::orientation_from_str(p->orientation);

ros::NodeHandle pnh(owner->dist_nh, topic_name);

Expand Down Expand Up @@ -302,7 +305,7 @@ DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *ow
}

// orientation check
pnh.param("orientation", p->orientation, -1);
pnh.param("orientation", orientation_value, -1);

// optional
pnh.param("send_tf", p->send_tf, false);
Expand Down