Mission base plugin. More...
#include <mission_protocol_base.h>
Public Member Functions | |
virtual void | initialize (ros::NodeHandle *_wp_nh) |
MissionBase (std::string _name) | |
Public Member Functions inherited from mavros::plugin::PluginBase | |
virtual Subscriptions | get_subscriptions ()=0 |
Return vector of MAVLink message subscriptions (handlers) More... | |
virtual void | initialize (UAS &uas) |
Plugin initializer. More... | |
virtual | ~PluginBase () |
Protected Types | |
using | lock_guard = std::lock_guard< std::recursive_mutex > |
using | unique_lock = std::unique_lock< std::recursive_mutex > |
enum | WP { WP::IDLE, WP::RXLIST, WP::RXWP, WP::RXWPINT, WP::TXLIST, WP::TXPARTIAL, WP::TXWP, WP::TXWPINT, WP::CLEAR, WP::SET_CUR } |
Protected Member Functions | |
void | go_idle (void) |
void | handle_mission_ack (const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ACK &mack) |
handle MISSION_ACK mavlink msg Handles a MISSION_ACK which marks the end of a push, or a failure More... | |
void | handle_mission_count (const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_COUNT &mcnt) |
handle MISSION_COUNT mavlink msg Handles a mission count from FCU in a Waypoint Pull Triggers a pull GCS seems to be requesting mission More... | |
void | handle_mission_item (const mavlink::mavlink_message_t *msg, WP_ITEM &wpi) |
handle MISSION_ITEM mavlink msg handles and stores mission items when pulling waypoints More... | |
void | handle_mission_item_int (const mavlink::mavlink_message_t *msg, WP_ITEM_INT &wpi) |
handle MISSION_ITEM_INT mavlink msg handles and stores mission items when pulling waypoints More... | |
void | handle_mission_request (const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST &mreq) |
handle MISSION_REQUEST mavlink msg handles and acts on misison request from FCU More... | |
void | handle_mission_request_int (const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST_INT &mreq) |
handle MISSION_REQUEST_INT mavlink msg handles and acts on misison request from FCU More... | |
void | mission_ack (MRES type) |
void | mission_clear_all () |
void | mission_count (uint16_t cnt) |
void | mission_request (uint16_t seq) |
void | mission_request_int (uint16_t seq) |
void | mission_request_list () |
template<class ITEM > | |
void | mission_send (ITEM &wp) |
void | mission_set_current (uint16_t seq) |
void | mission_write_partial_list (uint16_t start_index, uint16_t end_index) |
virtual void | publish_waypoints ()=0 |
publish the updated waypoint list after operation More... | |
void | request_mission_done (void) |
Send ACK back to FCU after pull. More... | |
void | restart_timeout_timer (void) |
void | restart_timeout_timer_int (void) |
void | schedule_pull (const ros::Duration &dt) |
void | scheduled_pull_cb (const ros::TimerEvent &event) |
Callback for scheduled waypoint pull. More... | |
template<class ITEM > | |
void | send_waypoint (size_t seq) |
send a single waypoint to FCU More... | |
bool | sequence_mismatch (const uint16_t &seq) |
checks for a sequence mismatch between a MISSION_REQUEST(_INT) sequence and the current waypoint that should be sent. More... | |
void | set_current_waypoint (size_t seq) |
set the FCU current waypoint More... | |
void | timeout_cb (const ros::TimerEvent &event) |
Act on a timeout Resend the message that may have been lost. More... | |
bool | wait_fetch_all () |
wait until a waypoint pull is complete. Pull happens asynchronously, this function blocks until it is done. More... | |
bool | wait_push_all () |
wait until a waypoint push is complete. Push happens asynchronously, this function blocks until it is done. More... | |
Protected Member Functions inherited from mavros::plugin::PluginBase | |
virtual void | capabilities_cb (UAS::MAV_CAP capabilities) |
virtual void | connection_cb (bool connected) |
void | enable_capabilities_cb () |
void | enable_connection_cb () |
template<class _C > | |
HandlerInfo | make_handler (const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing)) |
template<class _C , class _T > | |
HandlerInfo | make_handler (void(_C::*fn)(const mavlink::mavlink_message_t *, _T &)) |
PluginBase () | |
Plugin constructor Should not do anything before initialize() More... | |
Protected Attributes | |
const ros::Duration | BOOTUP_TIME_DT |
bool | do_pull_after_gcs |
bool | enable_partial_push |
bool | is_timedout |
std::condition_variable | list_receiving |
std::condition_variable | list_sending |
const ros::Duration | LIST_TIMEOUT_DT |
std::string | log_ns |
bool | mission_item_int_support_confirmed |
std::recursive_mutex | mutex |
std::mutex | recv_cond_mutex |
const ros::Duration | RESCHEDULE_DT |
bool | reschedule_pull |
ros::Timer | schedule_timer |
std::mutex | send_cond_mutex |
std::vector< mavros_msgs::Waypoint > | send_waypoints |
bool | use_mission_item_int |
std::vector< mavros_msgs::Waypoint > | waypoints |
size_t | wp_count |
size_t | wp_cur_active |
size_t | wp_cur_id |
size_t | wp_end_id |
size_t | wp_retries |
size_t | wp_set_active |
size_t | wp_start_id |
WP | wp_state |
const ros::Duration | WP_TIMEOUT_DT |
ros::Timer | wp_timer |
WP_TYPE | wp_type |
Protected Attributes inherited from mavros::plugin::PluginBase | |
UAS * | m_uas |
Static Protected Attributes | |
static constexpr int | BOOTUP_TIME_MS = 15000 |
static constexpr int | LIST_TIMEOUT_MS = 30000 |
system startup delay before start pull More... | |
static constexpr unsigned int | MAV_PROTOCOL_CAPABILITY_MISSION_INT = 4 |
static constexpr int | RESCHEDULE_MS = 5000 |
static constexpr int | RETRIES_COUNT = 3 |
static constexpr int | WP_TIMEOUT_MS = 1000 |
Timeout for pull/push operations. More... | |
Additional Inherited Members | |
Public Types inherited from mavros::plugin::PluginBase | |
using | ConstPtr = boost::shared_ptr< PluginBase const > |
using | HandlerCb = mavconn::MAVConnInterface::ReceivedCb |
generic message handler callback More... | |
using | HandlerInfo = std::tuple< mavlink::msgid_t, const char *, size_t, HandlerCb > |
Tuple: MSG ID, MSG NAME, message type into hash_code, message handler callback. More... | |
using | Ptr = boost::shared_ptr< PluginBase > |
using | Subscriptions = std::vector< HandlerInfo > |
Subscriptions vector. More... | |
Mission base plugin.
Definition at line 232 of file mission_protocol_base.h.