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 14 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}
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
111 changes: 64 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,84 @@ 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),
/* 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)
{
for (int idx = 0; idx < sensor_orientations.size(); idx++) {
if (sensor_orientations[idx].first == sensor_orientation)
return idx;
}

return -1;
}
32 changes: 20 additions & 12 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 @@ -43,13 +43,13 @@ class DistanceSensorItem {
{ }

// params
bool is_subscriber; //!< this item is a subscriber, else is a publisher
bool send_tf; //!< defines if a transform is sent or not
uint8_t sensor_id; //!< id of the sensor
bool is_subscriber; //!< this item is a subscriber, else is a publisher
bool send_tf; //!< defines if a transform is sent or not
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
int covariance; //!< in centimeters, current specification
int orientation; //!< check orientation of sensor if != -1
int covariance; //!< in centimeters, current specification
std::string frame_id; //!< frame id for send

// topic handle
Expand All @@ -64,7 +64,7 @@ class DistanceSensorItem {

private:
std::vector<float> data; //!< array allocation for measurements
size_t data_index; //!< array index
size_t data_index; //!< array index

/**
* Calculate measurements variance to send to the FCU.
Expand All @@ -75,7 +75,8 @@ class DistanceSensorItem {
data.push_back(range);
else {
data[data_index] = range; // it starts rewriting the values from 1st element
if (++data_index > 49) data_index = 0; // restarts the index when achieves the last element
if (++data_index > 49)
data_index = 0; // restarts the index when achieves the last element
}

float average, variance, sum = 0, sum_ = 0;
Expand All @@ -88,7 +89,7 @@ class DistanceSensorItem {

/* Compute the variance */
for (auto d : data)
sum_ += pow((d - average), 2);
sum_ += (d - average) * (d - average);

variance = sum_ / data.size();

Expand Down Expand Up @@ -193,15 +194,17 @@ class DistanceSensorPlugin : public MavRosPlugin {

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!

if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) {
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 (%f)!",
sensor->topic_name.c_str(),
UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation)).c_str(),
sensor->orientation);
}

auto range = boost::make_shared<sensor_msgs::Range>();

range->header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms);

range->min_range = dist_sen.min_distance * 1E-2; // in meters
range->min_range = dist_sen.min_distance * 1E-2; // in meters
range->max_range = dist_sen.max_distance * 1E-2;
range->field_of_view = sensor->field_of_view;

Expand Down Expand Up @@ -248,6 +251,7 @@ void DistanceSensorItem::range_cb(const sensor_msgs::Range::ConstPtr &msg)

if (covariance > 0) covariance_ = covariance;
else covariance_ = uint8_t(calculate_variance(msg->range) * 1E2); // in cm
/** @todo Propose changing covarince from uint8_t to float */
ROS_DEBUG_NAMED("distance_sensor", "DS: %d: sensor variance: %f", sensor_id, calculate_variance(msg->range) * 1E2);

// current mapping, may change later
Expand Down Expand Up @@ -323,6 +327,10 @@ DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *ow
ROS_ERROR_NAMED("distance_sensor", "DS: %s: `orientation` not set!", topic_name.c_str());
p.reset(); return p; // nullptr
}
else if (p->orientation == -1) {
ROS_ERROR_NAMED("distance_sensor", "DS: %s: defined orientation is not valid!", topic_name.c_str());
p.reset(); return p; // nullptr
}

// optional
pnh.param("covariance", p->covariance, 0);
Expand All @@ -336,6 +344,6 @@ DistanceSensorItem::Ptr DistanceSensorItem::create_item(DistanceSensorPlugin *ow

return p;
}
}; // namespace mavplugin
}; // namespace mavplugin

PLUGINLIB_EXPORT_CLASS(mavplugin::DistanceSensorPlugin, mavplugin::MavRosPlugin)