Skip to content

Commit

Permalink
Remove follow_direction support from follow_me
Browse files Browse the repository at this point in the history
- Along with Proto submodule update
  • Loading branch information
junwoo091400 committed May 20, 2022
1 parent 24ec398 commit dc656c3
Show file tree
Hide file tree
Showing 8 changed files with 86 additions and 396 deletions.
2 changes: 1 addition & 1 deletion proto
6 changes: 3 additions & 3 deletions src/integration_tests/follow_me.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,9 @@
#include <thread>
#include <chrono>
#include <array>

#include "integration_test_helper.h"

#include "mavsdk.h"
#include "plugins/telemetry/telemetry.h"
#include "plugins/action/action.h"
Expand Down Expand Up @@ -34,7 +36,6 @@ TEST_F(SitlTest, PX4FollowMeOneLocation)
auto telemetry = std::make_shared<Telemetry>(system);
auto follow_me = std::make_shared<FollowMe>(system);
auto action = std::make_shared<Action>(system);

LogInfo() << "Waiting for system to be ready";
ASSERT_TRUE(poll_condition_with_timeout(
[telemetry]() {
Expand Down Expand Up @@ -154,8 +155,7 @@ TEST_F(SitlTest, PX4FollowMeMultiLocationWithConfig)
config.min_height_m = 12.f; // increase min height
config.follow_distance_m = 20.f; // set distance b/w system and target during FollowMe mode
config.responsiveness = 0.2f; // set to higher responsiveness
config.follow_direction =
FollowMe::Config::FollowDirection::Front; // System follows target from FRONT side
config.follow_angle_deg = 0.0; // System follows target from FRONT side

// Apply configuration
FollowMe::Result config_result = follow_me->set_config(config);
Expand Down
29 changes: 0 additions & 29 deletions src/mavsdk/plugins/follow_me/follow_me.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,33 +56,6 @@ FollowMe::Result FollowMe::stop() const
return _impl->stop();
}

std::ostream&
operator<<(std::ostream& str, FollowMe::Config::FollowDirection const& follow_direction)
{
switch (follow_direction) {
case FollowMe::Config::FollowDirection::None:
return str << "None";
case FollowMe::Config::FollowDirection::Behind:
return str << "Behind";
case FollowMe::Config::FollowDirection::Front:
return str << "Front";
case FollowMe::Config::FollowDirection::FrontRight:
return str << "Front Right";
case FollowMe::Config::FollowDirection::FrontLeft:
return str << "Front Left";
case FollowMe::Config::FollowDirection::Right:
return str << "Right";
case FollowMe::Config::FollowDirection::Left:
return str << "Left";
case FollowMe::Config::FollowDirection::BehindRight:
return str << "Behind Right";
case FollowMe::Config::FollowDirection::BehindLeft:
return str << "Behind Left";
default:
return str << "Unknown";
}
}

std::ostream&
operator<<(std::ostream& str, FollowMe::Config::FollowAltitudeMode const& follow_altitude_mode)
{
Expand All @@ -103,7 +76,6 @@ bool operator==(const FollowMe::Config& lhs, const FollowMe::Config& rhs)
rhs.min_height_m == lhs.min_height_m) &&
((std::isnan(rhs.follow_distance_m) && std::isnan(lhs.follow_distance_m)) ||
rhs.follow_distance_m == lhs.follow_distance_m) &&
(rhs.follow_direction == lhs.follow_direction) &&
((std::isnan(rhs.responsiveness) && std::isnan(lhs.responsiveness)) ||
rhs.responsiveness == lhs.responsiveness) &&
(rhs.altitude_mode == lhs.altitude_mode) &&
Expand All @@ -119,7 +91,6 @@ std::ostream& operator<<(std::ostream& str, FollowMe::Config const& config)
str << "config:" << '\n' << "{\n";
str << " min_height_m: " << config.min_height_m << '\n';
str << " follow_distance_m: " << config.follow_distance_m << '\n';
str << " follow_direction: " << config.follow_direction << '\n';
str << " responsiveness: " << config.responsiveness << '\n';
str << " altitude_mode: " << config.altitude_mode << '\n';
str << " max_tangential_vel_m_s: " << config.max_tangential_vel_m_s << '\n';
Expand Down
1 change: 0 additions & 1 deletion src/mavsdk/plugins/follow_me/follow_me_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,6 @@ FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config)
success = false;
}
}


if (_config.responsiveness != responsiveness) {
if (_parent->set_param_float("FLW_TGT_RS", responsiveness) ==
Expand Down
35 changes: 0 additions & 35 deletions src/mavsdk/plugins/follow_me/include/plugins/follow_me/follow_me.h
Original file line number Diff line number Diff line change
Expand Up @@ -61,37 +61,6 @@ class FollowMe : public PluginBase {
* @brief Configuration type.
*/
struct Config {
/**
* @brief * [!DEPRECIATED!]
* Direction relative to the target that the vehicle should follow.
* Measured in reference to target's course (where it's moving towards)
*
* It is only used for setting the follow angle indirectly, for legacy reasons
* (since PX4 Follow Me used to use FollowDirection enums for setting, but now
* uses the follow_angle_deg float value).
*
* DO NOT USE if possible!
*/
enum class FollowDirection {
None, /**< @brief Follow direction invalid (defaults to Behind). */
Behind, /**< @brief Follow from behind. */
Front, /**< @brief Follow from front. */
FrontRight, /**< @brief Follow from front right. */
FrontLeft, /**< @brief Follow from front left. */
Right, /**< @brief Follow from right. */
Left, /**< @brief Follow from left. */
BehindRight, /**< @brief Follow from behind right. */
BehindLeft, /**< @brief Follow from behind left. */
};

/**
* @brief Stream operator to print information about a `FollowMe::FollowDirection`.
*
* @return A reference to the stream.
*/
friend std::ostream&
operator<<(std::ostream& str, FollowMe::Config::FollowDirection const& follow_direction);

/**
* @brief Altitude mode to configure which altitude the follow me will assume the target to
* be at.
Expand All @@ -116,10 +85,6 @@ class FollowMe : public PluginBase {
8.0}; /**< @brief [m] Follow height in meters (recommended minimum 8 meters) */
float follow_distance_m{8.0}; /**< @brief [m] Follow distance to target in meters
(recommended minimum 4 meter) */
FollowDirection
follow_direction{}; /**< @brief [!DEPRECIATED!] Direction to follow target at. Exists as
a legacy to old PX4 Follow-Me, but now only used for setting the
follow_angle_deg. This value never gets sent to the PX4 side! */
float responsiveness{0.1}; /**< @brief How responsive the vehicle is to the motion of the
target, Lower value = More responsive (range 0.0 to 1.0) */
FollowAltitudeMode altitude_mode{}; /**< @brief Follow Altitude control mode */
Expand Down
Loading

0 comments on commit dc656c3

Please sign in to comment.