Skip to content

Commit

Permalink
Merge branch 'master' into ros2
Browse files Browse the repository at this point in the history 
* master:
  1.13.0
  update changelog
  py-lib: fix compatibility with py3 for Noetic
  re-generate all coglets
  test: add checks for ROTATION_CUSTOM
  lib: Fix rotation search for CUSTOM
  Removed CamelCase for class members.  Publish to "report"
  More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
  Fixed callback name to match `handle_{MESSAGE_NAME.lower()}` convention
  Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html
  Fixed topic names to match more closely what other plugins use.  Fixed a typo.
  Add plugin for reporting terrain height estimate from FCU
  1.12.2
  update changelog
  Set time/publish_sim_time to false by default
  plugin: setpoint_raw: move getParam to initializer
  extras: trajectory: backport #1667
  • Loading branch information
@vooon
vooon committed Jan 16, 2022
2 parents 02af8ca + ed762be commit 685491a
Show file tree
Hide file tree
Showing 15 changed files with 276 additions and 108 deletions.
6 changes: 6 additions & 0 deletions libmavconn/CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,12 @@ Changelog for package libmavconn
* disable all packages but messages
* Contributors: Mikael Arguedas, Vladimir Ermakov

1.13.0 (2022-01-13)
-------------------

1.12.2 (2021-12-12)
-------------------

1.12.1 (2021-11-29)
-------------------
* mavconn: fix connection issue introduced by `#1658 <https://github.com/mavlink/mavros/issues/1658>`_
Expand Down
23 changes: 23 additions & 0 deletions mavros/CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -422,6 +422,29 @@ Changelog for package mavros
* disable all packages but messages
* Contributors: Alexey Rogachevskiy, Mikael Arguedas, Thomas, Vladimir Ermakov

1.13.0 (2022-01-13)
-------------------
* Merge pull request `#1690 <https://github.com/mavlink/mavros/issues/1690>`_ from mavlink/fix-enum_sensor_orientation
Fix enum sensor_orientation
* py-lib: fix compatibility with py3 for Noetic
* test: add checks for ROTATION_CUSTOM
* lib: Fix rotation search for CUSTOM
Fix `#1688 <https://github.com/mavlink/mavros/issues/1688>`_.
* Contributors: Vladimir Ermakov

1.12.2 (2021-12-12)
-------------------
* Merge pull request `#1672 <https://github.com/mavlink/mavros/issues/1672>`_ from okalachev/patch-1
Set time/publish_sim_time to false by default
* Set time/publish_sim_time to false by default
* Merge pull request `#1669 <https://github.com/mavlink/mavros/issues/1669>`_ from Hs293Go/master
plugin: setpoint_raw: move getParam to initializer
* plugin: setpoint_raw: move getParam to initializer
Repeatedly getting the thrust_scaling parameter in a callback that can
be invoked from a fast control loop may fail spuriously and trigger a
fatal error
* Contributors: Oleg Kalachev, Vladimir Ermakov, hs293go

1.12.1 (2021-11-29)
-------------------
* mavconn: fix connection issue introduced by `#1658 <https://github.com/mavlink/mavros/issues/1658>`_
Expand Down
119 changes: 62 additions & 57 deletions mavros/src/lib/enum_sensor_orientation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <string>
#include <utility>
#include <unordered_map>

#include "mavros/utils.hpp"
#include "mavros/frame_tf.hpp"
Expand Down Expand Up @@ -75,101 +76,105 @@ static const OrientationPair make_orientation(
// cog.msg(f"Parse Error: {ex}, desc: {desc}")
// return cls()
//
// cog.outl(f"static const std::array<const OrientationPair, {len(enum)}> sensor_orientations{{{{")
// cog.outl(
// f"static const std::unordered_map<typename std::underlying_type<{ename}>::type,\n"
// f" const OrientationPair> sensor_orientations{{{{")
// for k, e in enum:
// name_short = e.name[len(pfx2):]
// vec = Vector3.parse_rpy(e.description)
// cog.outl(
// f"""/* {k:>2} */ make_orientation"""
// f"""("{name_short}", {vec.Roll}, {vec.Pitch}, {vec.Yaw}),""")
// cog.outl(f""" {{{k}, make_orientation("{name_short}", {vec.Roll}, {vec.Pitch}, {vec.Yaw})}},""")
//
// cog.outl("}};")
// ]]]
static const std::array<const OrientationPair, 42> sensor_orientations{{
/* 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_90_PITCH_68_YAW_293", 90.0, 68.0, 293.0),
/* 39 */ make_orientation("PITCH_315", 0.0, 315.0, 0.0),
/* 40 */ make_orientation("ROLL_90_PITCH_315", 90.0, 315.0, 0.0),
/* 100 */ make_orientation("CUSTOM", 0.0, 0.0, 0.0),
static const std::unordered_map<typename std::underlying_type<MAV_SENSOR_ORIENTATION>::type,
const OrientationPair> sensor_orientations{{
{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_90_PITCH_68_YAW_293", 90.0, 68.0, 293.0)},
{39, make_orientation("PITCH_315", 0.0, 315.0, 0.0)},
{40, make_orientation("ROLL_90_PITCH_315", 90.0, 315.0, 0.0)},
{100, make_orientation("CUSTOM", 0.0, 0.0, 0.0)},
}};
// [[[end]]] (checksum: 2ed537a9279bb6a3992df43e225bbeb7)

// [[[end]]] (checksum: ddb7b21315b0f0636d264ad2d6b3856e)

std::string to_string(MAV_SENSOR_ORIENTATION orientation)
{
const auto idx = enum_value(orientation);
if (idx >= sensor_orientations.size()) {
auto it = sensor_orientations.find(idx);

if (it == sensor_orientations.end()) {
RCLCPP_ERROR(logger, "SENSOR: wrong orientation index: %d", idx);
return std::to_string(idx);
}

return sensor_orientations[idx].first;
return it->second.first;
}

Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation)
{
const auto idx = enum_value(orientation);
if (idx >= sensor_orientations.size()) {
auto it = sensor_orientations.find(idx);

if (it == sensor_orientations.end()) {
RCLCPP_ERROR(logger, "SENSOR: wrong orientation index: %d", idx);
return Eigen::Quaterniond::Identity();
}

return sensor_orientations[idx].second;
return it->second.second;
}

int sensor_orientation_from_str(const std::string & sensor_orientation)
{
// XXX bsearch

// 1. try to find by name
for (size_t idx = 0; idx < sensor_orientations.size(); idx++) {
if (sensor_orientations[idx].first == sensor_orientation) {
return idx;
for (const auto & kv : sensor_orientations) {
if (kv.second.first == sensor_orientation) {
return kv.first;
}
}

// 2. try convert integer
// fallback for old configs that uses numeric orientation.
try {
int idx = std::stoi(sensor_orientation, 0, 0);
if (0 > idx || size_t(idx) > sensor_orientations.size()) {
if (0 > idx || sensor_orientations.find(idx) == sensor_orientations.end()) {
RCLCPP_ERROR(logger, "SENSOR: orientation index out of bound: %d", idx);
return -1;
} else {
Expand Down
27 changes: 9 additions & 18 deletions mavros/src/lib/enum_to_string.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ std::string to_string(MAV_AUTOPILOT e)
// ]]]

//! MAV_TYPE values
static const std::array<const std::string, 38> mav_type_strings{{
static const std::array<const std::string, 36> mav_type_strings{{
/* 0 */ "Generic micro air vehicle",
/* 1 */ "Fixed wing aircraft",
/* 2 */ "Quadrotor",
Expand Down Expand Up @@ -212,8 +212,6 @@ static const std::array<const std::string, 38> mav_type_strings{{
/* 33 */ "Servo",
/* 34 */ "Open Drone ID. See https:",
/* 35 */ "Decarotor",
/* 36 */ "Battery",
/* 37 */ "Parachute",
}};


Expand All @@ -226,15 +224,15 @@ std::string to_string(MAV_TYPE e)

return mav_type_strings[idx];
}
// [[[end]]] (checksum: 376064ea1578250d182ab62a85b06a41)
// [[[end]]] (checksum: 9fabd2cfbcc8bd349dbd7f69f2f81351)

// [[[cog:
// ename = 'MAV_TYPE'
// enum_value_is_name_outl(ename, funcname='enum_to_name', suffix='_names')
// ]]]

//! MAV_TYPE values
static const std::array<const std::string, 38> mav_type_names{{
static const std::array<const std::string, 36> mav_type_names{{
/* 0 */ "GENERIC",
/* 1 */ "FIXED_WING",
/* 2 */ "QUADROTOR",
Expand Down Expand Up @@ -271,8 +269,6 @@ static const std::array<const std::string, 38> mav_type_names{{
/* 33 */ "SERVO",
/* 34 */ "ODID",
/* 35 */ "DECAROTOR",
/* 36 */ "BATTERY",
/* 37 */ "PARACHUTE",
}};

std::string enum_to_name(MAV_TYPE e)
Expand All @@ -284,7 +280,7 @@ std::string enum_to_name(MAV_TYPE e)

return mav_type_names[idx];
}
// [[[end]]] (checksum: 5e58c6b1f7868c26f570b97fa4389feb)
// [[[end]]] (checksum: ab74f38c87f80d889bca765375689bdd)

// [[[cog:
// ename = 'MAV_STATE'
Expand Down Expand Up @@ -566,8 +562,8 @@ std::string to_string(MAV_FRAME e)
// enum = get_enum(ename)
//
// cog.outl(
// f"static const std::unordered_map<size_t, const std::string> "
// f"{suffix.lower()}_strings{{{{")
// f"static const std::unordered_map<typename std::underlying_type<{ename}>::type,\n"
// f" const std::string> {suffix.lower()}_strings{{{{")
// for k, e in enum:
// name_short = e.name[len(suffix) + 1:]
// entry = f"""{{{k}, "{name_short}"}},"""
Expand All @@ -578,7 +574,8 @@ std::string to_string(MAV_FRAME e)
//
// cog.outl("}};")
// ]]]
static const std::unordered_map<size_t, const std::string> mav_comp_id_strings{{
static const std::unordered_map<typename std::underlying_type<MAV_COMPONENT>::type,
const std::string> mav_comp_id_strings{{
{0, "ALL"},
{1, "AUTOPILOT1"},
{25, "USER1"},
Expand Down Expand Up @@ -683,19 +680,13 @@ static const std::unordered_map<size_t, const std::string> mav_comp_id_strings{{
{158, "PERIPHERAL"},
{159, "QX1_GIMBAL"},
{160, "FLARM"},
{161, "PARACHUTE"},
{171, "GIMBAL2"},
{172, "GIMBAL3"},
{173, "GIMBAL4"},
{174, "GIMBAL5"},
{175, "GIMBAL6"},
{180, "BATTERY"},
{181, "BATTERY2"},
{190, "MISSIONPLANNER"},
{191, "ONBOARD_COMPUTER"},
{192, "ONBOARD_COMPUTER2"},
{193, "ONBOARD_COMPUTER3"},
{194, "ONBOARD_COMPUTER4"},
{195, "PATHPLANNER"},
{196, "OBSTACLE_AVOIDANCE"},
{197, "VISUAL_INERTIAL_ODOMETRY"},
Expand All @@ -713,7 +704,7 @@ static const std::unordered_map<size_t, const std::string> mav_comp_id_strings{{
{242, "TUNNEL_NODE"},
{250, "SYSTEM_CONTROL"},
}};
// [[[end]]] (checksum: bdc7916b48d45154a0b164047b45edf7)
// [[[end]]] (checksum: 9c8184e019003b807b4d2498e6f1d81f)

std::string to_string(MAV_COMPONENT e)
{
Expand Down
20 changes: 19 additions & 1 deletion mavros/test/test_sensor_orientation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,14 @@ TEST(UTILS, sensor_orientation_matching__roll_180_yaw_90)
EXPECT_QUATERNION(expected, out);
}

TEST(UTILS, sensor_orientation_matching__custom)
{
auto expected = ftf::quaternion_from_rpy(0.0, 0.0, 0.0);
auto out = utils::sensor_orientation_matching(SO::ROTATION_CUSTOM);

EXPECT_QUATERNION(expected, out);
}

TEST(UTILS, to_string__none)
{
EXPECT_EQ("NONE", utils::to_string(SO::ROTATION_NONE));
Expand All @@ -70,14 +78,19 @@ TEST(UTILS, to_string__roll_180_yaw_90)
EXPECT_EQ("ROLL_180_YAW_90", utils::to_string(SO::ROTATION_ROLL_180_YAW_90));
}

TEST(UTILS, to_string__custom)
{
EXPECT_EQ("CUSTOM", utils::to_string(SO::ROTATION_CUSTOM));
}

TEST(UTILS, sensor_orientation_from_str__none)
{
EXPECT_EQ(enum_value(SO::ROTATION_NONE), utils::sensor_orientation_from_str("NONE"));
}

TEST(UTILS, sensor_orientation_from_str__unknown)
{
EXPECT_LT(utils::sensor_orientation_from_str("completely wrong identificator"), 0);
EXPECT_LT(utils::sensor_orientation_from_str("completely wrong identifier"), 0);
}

TEST(UTILS, sensor_orientation_from_str__number)
Expand Down Expand Up @@ -110,6 +123,11 @@ TEST(UTILS, sensor_orientation_from_str__last_element_roll_90_yaw_270)
utils::sensor_orientation_from_str("ROLL_90_YAW_270"));
}

TEST(UTILS, sensor_orientation_from_str__custom)
{
EXPECT_EQ(enum_value(SO::ROTATION_CUSTOM), utils::sensor_orientation_from_str("CUSTOM"));
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
Expand Down
16 changes: 16 additions & 0 deletions mavros_extras/CHANGELOG.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,6 +225,22 @@ Changelog for package mavros_extras
* extras: adsb: begin porting to ros2
* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, David Jablonski, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Peter010103, Ricardo Marques, Russell, Vladimir Ermakov

1.13.0 (2022-01-13)
-------------------
* Merge pull request `#1677 <https://github.com/mavlink/mavros/issues/1677>`_ from AndersonRayner/add_terrain
Add plugin for reporting terrain height estimate from the FCU
* Removed CamelCase for class members. Publish to "report"
* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
* Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention
* Fixed topic names to match more closely what other plugins use. Fixed a typo.
* Add plugin for reporting terrain height estimate from FCU
* Contributors: Vladimir Ermakov, matt

1.12.2 (2021-12-12)
-------------------
* extras: trajectory: backport `#1667 <https://github.com/mavlink/mavros/issues/1667>`_
* Contributors: Vladimir Ermakov

1.12.1 (2021-11-29)
-------------------
* Merge pull request `#1660 <https://github.com/mavlink/mavros/issues/1660>`_ from scoutdi/fix-warnings
Expand Down
1 change: 1 addition & 0 deletions mavros_extras/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ add_library(mavros_extras_plugins SHARED
src/plugins/play_tune.cpp
src/plugins/px4flow.cpp
src/plugins/rangefinder.cpp
src/plugins/terrain.cpp
src/plugins/trajectory.cpp
src/plugins/tunnel.cpp
src/plugins/vfr_hud.cpp
Expand Down
Loading

0 comments on commit 685491a

Please sign in to comment.