actuator_control.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2015 Marcel Stüttgen <stuettgen@fh-aachen.de>
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 
19 #include <mavros_msgs/ActuatorControl.h>
20 
21 namespace mavros {
22 namespace std_plugins {
29 public:
31  nh("~")
32  { }
33 
34  void initialize(UAS &uas_) override
35  {
36  PluginBase::initialize(uas_);
37 
38  target_actuator_control_pub = nh.advertise<mavros_msgs::ActuatorControl>("target_actuator_control", 10);
40  }
41 
43  {
44  return {
46  };
47  }
48 
49 private:
51 
54 
55  /* -*- rx handlers -*- */
56 
57  void handle_actuator_control_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ACTUATOR_CONTROL_TARGET &actuator_control_target)
58  {
59  auto actuator_control_target_msg = boost::make_shared<mavros_msgs::ActuatorControl>();
60  actuator_control_target_msg->header.stamp = m_uas->synchronise_stamp(actuator_control_target.time_usec);
61 
62  actuator_control_target_msg->group_mix = actuator_control_target.group_mlx;
63  const auto &arr = actuator_control_target.controls;
64  std::copy(arr.cbegin(), arr.cend(), actuator_control_target_msg->controls.begin());
65 
66  target_actuator_control_pub.publish(actuator_control_target_msg);
67  }
68 
69  /* -*- callbacks -*- */
70 
74  mavlink::common::msg::SET_ACTUATOR_CONTROL_TARGET act{};
75 
76  act.time_usec = req->header.stamp.toNSec() / 1000;
77  act.group_mlx = req->group_mix;
78  act.target_system = m_uas->get_tgt_system();
79  act.target_component = m_uas->get_tgt_component();
80  std::copy(req->controls.begin(), req->controls.end(), act.controls.begin()); // std::array = boost::array
81 
82  UAS_FCU(m_uas)->send_message_ignore_drop(act);
83  }
84 };
85 } // namespace std_plugins
86 } // namespace mavros
87 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
void handle_actuator_control_target(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ACTUATOR_CONTROL_TARGET &actuator_control_target)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
MAVROS Plugin base.
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
void initialize(UAS &uas_) override
Plugin initializer.
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:159
UAS for plugins.
Definition: mavros_uas.h:67
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
void actuator_control_cb(const mavros_msgs::ActuatorControl::ConstPtr &req)
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
uint8_t get_tgt_component()
Return communication target component.
Definition: mavros_uas.h:166


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26