PX4 ROS 2 Interface Library
Library to interface with PX4 from a companion computer using ROS 2
setpoint_base.hpp
1 /****************************************************************************
2  * Copyright (c) 2023 PX4 Development Team.
3  * SPDX-License-Identifier: BSD-3-Clause
4  ****************************************************************************/
5 
6 #pragma once
7 
8 #include <cstdint>
9 #include <functional>
10 #include <memory>
11 
12 #include <rclcpp/rclcpp.hpp>
13 #include <px4_msgs/msg/vehicle_control_mode.hpp>
14 #include "context.hpp"
15 #include "exception.hpp"
16 
17 namespace px4_ros2
18 {
19 
20 class SetpointBase : public std::enable_shared_from_this<SetpointBase>
21 {
22 public:
23  using ShouldActivateCB = std::function<void ()>;
24 
26  {
27  void fillControlMode(px4_msgs::msg::VehicleControlMode & control_mode)
28  {
29  control_mode.flag_control_rates_enabled = rates_enabled;
30  control_mode.flag_control_attitude_enabled = attitude_enabled;
31  control_mode.flag_control_acceleration_enabled = acceleration_enabled;
32  control_mode.flag_control_velocity_enabled = velocity_enabled;
33  control_mode.flag_control_position_enabled = position_enabled;
34  control_mode.flag_control_altitude_enabled = altitude_enabled;
35  control_mode.flag_control_allocation_enabled = control_allocation_enabled;
36  control_mode.flag_control_climb_rate_enabled = climb_rate_enabled;
37  }
38 
39  bool control_allocation_enabled{true};
40  bool rates_enabled{true};
41  bool attitude_enabled{true};
42  bool altitude_enabled{true};
43  bool acceleration_enabled{true};
44  bool velocity_enabled{true};
45  bool position_enabled{true};
46  bool local_position_is_optional{false};
47  bool climb_rate_enabled{false};
48  };
49 
50  explicit SetpointBase(Context & context)
51  {
52  context.addSetpointType(this);
53  }
54 
55  virtual ~SetpointBase() = default;
56 
57  std::shared_ptr<SetpointBase> getSharedPtr()
58  {
59  try {
60  return shared_from_this();
61  } catch (const std::bad_weak_ptr & exception) {
62  throw Exception("Setpoint must be instantiated with std::make_shared<>");
63  }
64  return {};
65  }
66 
67  virtual Configuration getConfiguration() = 0;
68 
69  virtual float desiredUpdateRateHz() {return 50.f;}
70 
71 
72  void setShouldActivateCallback(const ShouldActivateCB & should_activate_cb)
73  {
74  _should_activate_cb = should_activate_cb;
75  }
76  void setActive(bool active) {_active = active;}
77 
78 protected:
79  void onUpdate()
80  {
81  if (!_active && _should_activate_cb) {
82  _should_activate_cb();
83  }
84  }
85 
86 private:
87  ShouldActivateCB _should_activate_cb;
88  bool _active{false};
89 };
90 
91 } /* namespace px4_ros2 */
Definition: context.hpp:20
Definition: setpoint_base.hpp:21
Definition: setpoint_base.hpp:26