Program Listing for File mission_protocol_base.hpp
↰ Return to documentation for file (/tmp/ws/src/mavros/mavros/include/mavros/mission_protocol_base.hpp
)
/*
* Copyright 2014,2015,2016,2017,2018,2021 Vladimir Ermakov.
* Copyright 2021 Charlie Burge.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#pragma once
#ifndef MAVROS__MISSION_PROTOCOL_BASE_HPP_
#define MAVROS__MISSION_PROTOCOL_BASE_HPP_
#include <chrono>
#include <sstream>
#include <iomanip>
#include <string>
#include <vector>
#include <condition_variable>
#include "rcpputils/asserts.hpp"
#include "mavros/mavros_uas.hpp"
#include "mavros/plugin.hpp"
#include "mavros/plugin_filter.hpp"
#include "mavros_msgs/msg/waypoint_list.hpp"
#include "mavros_msgs/srv/waypoint_clear.hpp"
#include "mavros_msgs/srv/waypoint_pull.hpp"
#include "mavros_msgs/srv/waypoint_push.hpp"
namespace mavros
{
namespace plugin
{
using namespace std::placeholders; // NOLINT
using namespace std::chrono_literals; // NOLINT
using utils::enum_value;
using mavlink::common::MAV_CMD;
using mavlink::common::MAV_FRAME;
using mavlink::common::MAV_PROTOCOL_CAPABILITY;
using mavlink::common::msg::MISSION_ITEM;
using mavlink::common::msg::MISSION_ITEM_INT;
using MRES = mavlink::common::MAV_MISSION_RESULT;
using MTYPE = mavlink::common::MAV_MISSION_TYPE;
using MFilter = plugin::filter::SystemAndOk;
// [[[cog:
//
// from pymavlink.dialects.v20 import common
// e = common.enums['MAV_FRAME']
// all_names = [ee.name[len('MAV_FRAME_'):] for ee in e.values()]
// all_names.pop() # remove ENUM_END
// global_names = [v for v in all_names if v.startswith('GLOBAL')]
// local_names = [v for v in all_names if v.startswith(('LOCAL', 'BODY', 'MOCAP',
// 'VISION', 'ESTIM'))]
// other_names = ['MISSION']
//
// waypoint_item_msg = [(v, v) if isinstance(v, str) else v for v in (
// 'frame',
// 'command',
// ('is_current', 'current'),
// 'autocontinue',
// 'param1',
// 'param2',
// 'param3',
// 'param4',
// )]
// waypoint_coords = [
// ('x_lat', 'x'),
// ('y_long', 'y'),
// ('z_alt', 'z'),
// ]
// mission_item_msg = [
// ('seq', 'seq'),
// ('mission_type', 'mission_type'),
// ]
// ]]]
// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
class MissionItem : public mavros_msgs::msg::Waypoint
{
public:
uint16_t seq;
uint8_t mission_type;
static constexpr double encode_factor(const uint8_t frame)
{
switch (frame) {
// [[[cog:
// for names, factor in [(global_names, 10000000), (local_names, 10000), (other_names, 1)]:
// for name in names:
// cog.outl(f"case enum_value(MAV_FRAME::{name}):")
// cog.outl(f" return {factor:1.1f};")
//
// cog.outl("default:\n return 1.0;")
// ]]]
case enum_value(MAV_FRAME::GLOBAL):
case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT):
case enum_value(MAV_FRAME::GLOBAL_INT):
case enum_value(MAV_FRAME::GLOBAL_RELATIVE_ALT_INT):
case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT):
case enum_value(MAV_FRAME::GLOBAL_TERRAIN_ALT_INT):
return 10000000.0;
case enum_value(MAV_FRAME::LOCAL_NED):
case enum_value(MAV_FRAME::LOCAL_ENU):
case enum_value(MAV_FRAME::LOCAL_OFFSET_NED):
case enum_value(MAV_FRAME::BODY_NED):
case enum_value(MAV_FRAME::BODY_OFFSET_NED):
case enum_value(MAV_FRAME::BODY_FRD):
case enum_value(MAV_FRAME::LOCAL_FRD):
case enum_value(MAV_FRAME::LOCAL_FLU):
return 10000.0;
case enum_value(MAV_FRAME::MISSION):
return 1.0;
default:
return 1.0;
// [[[end]]] (checksum: 62e411e9acae54305a129fdf57f5b732)
}
}
explicit MissionItem(const MISSION_ITEM & wpi)
: mavros_msgs::msg::Waypoint()
{
// [[[cog:
// fields = mission_item_msg + waypoint_item_msg + waypoint_coords
// for i, (a, b) in enumerate(fields):
// cog.outl(f"{a} = wpi.{b};")
// ]]]
seq = wpi.seq;
mission_type = wpi.mission_type;
frame = wpi.frame;
command = wpi.command;
is_current = wpi.current;
autocontinue = wpi.autocontinue;
param1 = wpi.param1;
param2 = wpi.param2;
param3 = wpi.param3;
param4 = wpi.param4;
x_lat = wpi.x;
y_long = wpi.y;
z_alt = wpi.z;
// [[[end]]] (checksum: 64a5d4f7b3428e8e72d5ce216d51935c)
}
explicit MissionItem(const MISSION_ITEM_INT & wpi)
: mavros_msgs::msg::Waypoint()
{
// [[[cog:
// fields = mission_item_msg + waypoint_item_msg + waypoint_coords
// for i, (a, b) in enumerate(fields):
// if a.startswith(('x', 'y')):
// cog.outl(f"{a} = wpi.{b} / encode_factor(wpi.frame);")
// else:
// cog.outl(f"{a} = wpi.{b};")
// ]]]
seq = wpi.seq;
mission_type = wpi.mission_type;
frame = wpi.frame;
command = wpi.command;
is_current = wpi.current;
autocontinue = wpi.autocontinue;
param1 = wpi.param1;
param2 = wpi.param2;
param3 = wpi.param3;
param4 = wpi.param4;
x_lat = wpi.x / encode_factor(wpi.frame);
y_long = wpi.y / encode_factor(wpi.frame);
z_alt = wpi.z;
// [[[end]]] (checksum: 2e01c028ecde023cc049aba04f6a6df5)
}
explicit MissionItem(const mavros_msgs::msg::Waypoint & other)
{
*this = other;
}
MissionItem & operator=(const mavros_msgs::msg::Waypoint & other)
{
// [[[cog:
// fields = waypoint_item_msg + waypoint_coords
// for a, b in fields:
// cog.outl(f"{a} = other.{a};")
// ]]]
frame = other.frame;
command = other.command;
is_current = other.is_current;
autocontinue = other.autocontinue;
param1 = other.param1;
param2 = other.param2;
param3 = other.param3;
param4 = other.param4;
x_lat = other.x_lat;
y_long = other.y_long;
z_alt = other.z_alt;
// [[[end]]] (checksum: 6879b829d6e6e1e28a879dd2ca3afdac)
return *this;
}
void to_msg(MISSION_ITEM & out) const
{
// [[[cog:
// fields = mission_item_msg + waypoint_item_msg + waypoint_coords
// for a, b in fields:
// cog.outl(f"out.{b} = {a};")
// ]]]
out.seq = seq;
out.mission_type = mission_type;
out.frame = frame;
out.command = command;
out.current = is_current;
out.autocontinue = autocontinue;
out.param1 = param1;
out.param2 = param2;
out.param3 = param3;
out.param4 = param4;
out.x = x_lat;
out.y = y_long;
out.z = z_alt;
// [[[end]]] (checksum: 799ed1c97af022223371c42c8c1f09d4)
}
void to_msg(MISSION_ITEM_INT & out) const
{
// [[[cog:
// fields = mission_item_msg + waypoint_item_msg + waypoint_coords
// for a, b in fields:
// if b.startswith(('x', 'y')):
// cog.outl(f"out.{b} = int32_t({a} * encode_factor(frame));")
// else:
// cog.outl(f"out.{b} = {a};")
// ]]]
out.seq = seq;
out.mission_type = mission_type;
out.frame = frame;
out.command = command;
out.current = is_current;
out.autocontinue = autocontinue;
out.param1 = param1;
out.param2 = param2;
out.param3 = param3;
out.param4 = param4;
out.x = int32_t(x_lat * encode_factor(frame));
out.y = int32_t(y_long * encode_factor(frame));
out.z = z_alt;
// [[[end]]] (checksum: b79ee415a09746786ba2a7cec99c5ab5)
}
friend std::ostream & operator<<(std::ostream & os, const MissionItem & mi);
};
std::ostream & operator<<(std::ostream & os, const MissionItem & mi);
class MissionBase : public plugin::Plugin
{
public:
MissionBase(
plugin::UASPtr uas_, const std::string & name_, MTYPE mission_type_ = MTYPE::MISSION,
const char * log_prefix_ = "WP",
const std::chrono::nanoseconds bootup_time_ = 15s)
: Plugin(uas_, name_),
mission_type(mission_type_),
log_prefix(log_prefix_),
wp_state(WP::IDLE),
wp_count(0),
wp_start_id(0),
wp_end_id(0),
wp_cur_id(0),
wp_cur_active(0),
wp_set_active(0),
wp_retries(RETRIES_COUNT),
is_timedout(false),
reschedule_pull(false),
do_pull_after_gcs(false),
enable_partial_push(false),
use_mission_item_int(false),
mission_item_int_support_confirmed(false),
BOOTUP_TIME(bootup_time_),
LIST_TIMEOUT(30s),
WP_TIMEOUT(1s),
RESCHEDULE_TIME(5s)
{
timeout_timer = node->create_wall_timer(WP_TIMEOUT, std::bind(&MissionBase::timeout_cb, this));
timeout_timer->cancel();
}
Subscriptions get_subscriptions() override
{
Subscriptions ret{
make_handler(&MissionBase::handle_mission_item),
make_handler(&MissionBase::handle_mission_item_int),
make_handler(&MissionBase::handle_mission_request),
make_handler(&MissionBase::handle_mission_request_int),
make_handler(&MissionBase::handle_mission_count),
make_handler(&MissionBase::handle_mission_ack),
};
// NOTE(vooon): those messages do not have mission_type and only needed for waypoint plugin
if (mission_type == MTYPE::MISSION) {
ret.push_back(make_handler(&MissionBase::handle_mission_current));
ret.push_back(make_handler(&MissionBase::handle_mission_item_reached));
}
return ret;
}
protected:
using unique_lock = std::unique_lock<std::recursive_mutex>;
using lock_guard = std::lock_guard<std::recursive_mutex>;
std::recursive_mutex mutex;
std::vector<MissionItem> waypoints;
std::vector<MissionItem> send_waypoints;
const MTYPE mission_type;
const char * log_prefix;
enum class WP
{
IDLE,
RXLIST,
RXWP,
RXWPINT,
TXLIST,
TXPARTIAL,
TXWP,
TXWPINT,
CLEAR,
SET_CUR
};
WP wp_state;
size_t wp_count;
size_t wp_start_id;
size_t wp_end_id;
size_t wp_cur_id;
size_t wp_cur_active;
size_t wp_set_active;
size_t wp_retries;
bool is_timedout;
std::mutex recv_cond_mutex;
std::mutex send_cond_mutex;
std::condition_variable list_receiving;
std::condition_variable list_sending;
rclcpp::TimerBase::SharedPtr timeout_timer;
rclcpp::TimerBase::SharedPtr schedule_timer;
bool reschedule_pull;
bool do_pull_after_gcs;
bool enable_partial_push;
bool use_mission_item_int;
bool mission_item_int_support_confirmed;
static constexpr int RETRIES_COUNT = 3;
const std::chrono::nanoseconds BOOTUP_TIME;
const std::chrono::nanoseconds LIST_TIMEOUT;
const std::chrono::nanoseconds WP_TIMEOUT;
const std::chrono::nanoseconds RESCHEDULE_TIME;
/* -*- rx handlers -*- */
template<class MsgT>
bool filter_message(const MsgT & m)
{
return m.mission_type != enum_value(mission_type);
}
template<class MsgT>
bool sequence_mismatch(const MsgT & m)
{
if (m.seq != wp_cur_id && m.seq != wp_cur_id + 1) {
RCLCPP_WARN(
get_logger(), "%s: Seq mismatch, dropping %s (%d != %zu)", log_prefix,
m.get_name().c_str(), m.seq, wp_cur_id);
return true;
}
return false;
}
virtual void handle_mission_item(
const mavlink::mavlink_message_t * msg,
MISSION_ITEM & wpi,
MFilter filter);
virtual void handle_mission_item_int(
const mavlink::mavlink_message_t * msg,
MISSION_ITEM_INT & wpi,
MFilter filter);
virtual void handle_mission_request(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_REQUEST & mreq,
MFilter filter);
virtual void handle_mission_request_int(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_REQUEST_INT & mreq,
MFilter filter);
virtual void handle_mission_count(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_COUNT & mcnt,
MFilter filter);
virtual void handle_mission_ack(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_ACK & mack,
MFilter filter);
virtual void handle_mission_current(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_CURRENT & mcur,
MFilter filter);
virtual void handle_mission_item_reached(
const mavlink::mavlink_message_t * msg,
mavlink::common::msg::MISSION_ITEM_REACHED & mitr,
MFilter filter);
/* -*- mid-level helpers -*- */
void timeout_cb();
void scheduled_pull_cb()
{
lock_guard lock(mutex);
// run once
schedule_timer->cancel();
if (wp_state != WP::IDLE) {
/* try later */
RCLCPP_DEBUG(get_logger(), "%s: busy, reschedule pull", log_prefix);
schedule_pull(RESCHEDULE_TIME);
return;
}
RCLCPP_DEBUG(get_logger(), "%s: start scheduled pull", log_prefix);
wp_state = WP::RXLIST;
wp_count = 0;
restart_timeout_timer();
mission_request_list();
}
void request_mission_done(void)
{
/* possibly not needed if count == 0 (QGC impl) */
mission_ack(MRES::ACCEPTED);
go_idle();
list_receiving.notify_all();
RCLCPP_INFO(get_logger(), "%s: mission received", log_prefix);
}
void go_idle(void)
{
reschedule_pull = false;
wp_state = WP::IDLE;
timeout_timer->cancel();
}
void restart_timeout_timer(void)
{
wp_retries = RETRIES_COUNT;
restart_timeout_timer_int();
}
void restart_timeout_timer_int(void)
{
is_timedout = false;
timeout_timer->reset();
}
void schedule_pull(const std::chrono::nanoseconds & dt)
{
if (schedule_timer) {
schedule_timer->cancel();
schedule_timer.reset();
}
schedule_timer = node->create_wall_timer(dt, std::bind(&MissionBase::scheduled_pull_cb, this));
}
template<class MsgT>
void send_waypoint(size_t seq)
{
static_assert(
std::is_same<MsgT, MISSION_ITEM>::value || std::is_same<MsgT,
MISSION_ITEM_INT>::value, "wrong type");
if (seq < send_waypoints.size()) {
MsgT wpi{};
auto wp_msg = send_waypoints.at(seq);
wp_msg.seq = seq;
wp_msg.mission_type = enum_value(mission_type);
wp_msg.to_msg(wpi);
RCLCPP_DEBUG_STREAM(get_logger(), log_prefix << ": send item " << wp_msg);
mission_send(wpi);
}
}
bool wait_fetch_all()
{
std::unique_lock<std::mutex> lock(recv_cond_mutex);
return list_receiving.wait_for(lock, LIST_TIMEOUT) == std::cv_status::no_timeout &&
!is_timedout;
}
bool wait_push_all()
{
std::unique_lock<std::mutex> lock(send_cond_mutex);
return list_sending.wait_for(lock, LIST_TIMEOUT) == std::cv_status::no_timeout &&
!is_timedout;
}
void set_current_waypoint(size_t seq)
{
size_t i = 0;
for (auto & it : waypoints) {
it.is_current = !!(i++ == seq);
}
}
virtual void publish_waypoints() = 0;
virtual void publish_reached(const uint16_t seq) = 0;
/* -*- low-level send functions -*- */
template<class MsgT>
void mission_send(MsgT & wpi)
{
static_assert(
std::is_same<MsgT, MISSION_ITEM>::value || std::is_same<MsgT,
MISSION_ITEM_INT>::value, "wrong type");
uas->msg_set_target(wpi);
uas->send_message(wpi);
}
void mission_request(const uint16_t seq);
void mission_request_int(const uint16_t seq);
void mission_set_current(const uint16_t seq);
void mission_request_list();
void mission_count(const uint16_t cnt);
void mission_write_partial_list(const uint16_t start_index, const uint16_t end_index);
void mission_clear_all();
void mission_ack(const MRES type);
};
} // namespace plugin
} // namespace mavros
#endif // MAVROS__MISSION_PROTOCOL_BASE_HPP_