aerostack2

struct Acro_command
#include <differential_flatness_controller.hpp>

ACRO output command (body rates plus thrust).

Public Members

Eigen::Vector3d PQR = Eigen::Vector3d::Zero()
double thrust = 0.0
class AcroBridge : public rclcpp::Node

Public Functions

AcroBridge()

Private Functions

void acroCallback(const as2_msgs::msg::Acro &acro_msg)

Private Members

std::shared_ptr<gz::transport::Node> gz_node_ptr_
std::string world_name
std::string name_space
std::string sensor_name
std::string link_name
std::string sensor_type
rclcpp::Subscription<as2_msgs::msg::Acro>::SharedPtr acro_sub_

Private Static Attributes

static bool use_sim_time_ = false
static std::shared_ptr<gz::transport::Node::Publisher> acro_pub_ = nullptr
class ACROMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <acro_motion.hpp>

The ACROMotion class is a motion reference handler that moves the robot to a given acro.

Public Functions

explicit ACROMotion(as2::Node *node_ptr, const std::string &ns = "")

ACROMotion Constructor.

Parameters:

node – as2::Node pointer.

inline ~ACROMotion()

ACROMotion Destructor.

bool sendACRO(const as2_msgs::msg::Thrust &thrust, const geometry_msgs::msg::Vector3 &angular_rates)

sendACRO sends a acro to the robot.

Using the time stamp and frame id from the thrust message. Frame id should be base_link.

Parameters:

  • thrust – as2_msgs::msg::Thrust to be sent.

  • angular_rates – geometry_msgs::msg::Vector3 to be sent.

Returns:

true if the command was sent successfully, false otherwise.

class AerialPlatform : public as2::Node
#include <aerial_platform.hpp>

Base class for all Aerial platforms. It provides the basic functionality for the platform. It is responsible for handling the platform state machine and the platform status. It also handles the command subscriptions and the basic platform services.

Subclassed by as2_platform_multirotor_simulator::MultirotorSimulatorPlatform, gazebo_platform::GazeboPlatform

Public Functions

explicit AerialPlatform(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct a new Aerial Platform object, with default parameters.

AerialPlatform(const std::string &ns, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct a new Aerial Platform object, with default parameters.

inline ~AerialPlatform()
virtual void configureSensors() = 0

Configures the platform sensors.

virtual bool ownSendCommand() = 0

Handles how a command must be sended in the concrete platform.

Returns:

true command is sended successfully.

Returns:

false command is not sended.

virtual bool ownSetArmingState(bool state) = 0

Handles how arming state has to be settled in the concrete platform.

Parameters:

state – true for arming the platform, false to disarm.

Returns:

true Arming state is settled successfully.

Returns:

false Arming state is not settled.

virtual bool ownSetOffboardControl(bool offboard) = 0

Handles how offboard mode has to be settled in the concrete platform.

Parameters:

offboard – true if offboard mode is enabled.

Returns:

true Offboard mode is settled successfully.

Returns:

false Offboard mode is not settled.

virtual bool ownSetPlatformControlMode(const as2_msgs::msg::ControlMode &msg) = 0

Handles how the control mode has to be settled in the concrete platform.

Parameters:

control_mode – as2_msgs::msg::PlatformControlMode with the new control mode.

Returns:

true Control mode is settled successfully.

Returns:

false Control mode is not settled.

inline virtual bool ownTakeoff()

Handles the platform takeoff command.

Returns:

true Takeoff command is sended successfully.

Returns:

false Takeoff command is not sended.

inline virtual bool ownLand()

Handles the platform landing command.

Returns:

true Landing command is sended successfully.

Returns:

false Landing command is not sended.

virtual void ownKillSwitch() = 0

Handles the platform emergency kill switch command. This means stop the motors inmediately, this cannot be reversed. USE WITH CAUTION.

virtual void ownStopPlatform() = 0

Handles the platform emergency stop command. STOP means to hover as best as possible. This hover is different from the hover in the platform control mode. And when it is activated the platform will stop hearing commands from AS2. USE WITH CAUTION.

inline bool handleStateMachineEvent(const as2_msgs::msg::PlatformStateMachineEvent &event)

Set the State Machine Event object.

Parameters:

event – Event to

Returns:

true

Returns:

false

inline bool handleStateMachineEvent(const int8_t &event)
inline bool getArmingState() const

Get whether the platform is armed or not.

Returns:

true Armed

Returns:

false Disarmed

inline bool getConnectedStatus() const

Get wheter the connection is established or not.

Returns:

true Connection active

Returns:

false Connection not active

inline bool getOffboardMode() const

Get whether offboard mode is active or not.

Returns:

true Offboard mode enabled

Returns:

false Offboard mode disabled

inline as2_msgs::msg::ControlMode &getControlMode()

Get current platform control mode.

Returns:

as2_msgs::msg::PlatformControlMode current platform control mode

inline bool isControlModeSettled() const

Get whether a control mode is active or not.

Returns:

true Control mode set and valid

Returns:

false Control mode unset

Protected Functions

bool setArmingState(bool state)

Set the arm state of the platform.

Parameters:

state – True to arm the platform, false to disarm it.

Returns:

true Armimg state setted successfully.

Returns:

false Armimg state not setted successfully.

bool setOffboardControl(bool offboard)

Set the offboard control mode.

Parameters:

offboard – True if the offboard control mode is enabled.

Returns:

true if the offboard control mode is setted properly

Returns:

false if the offboard control mode could not be setted.

bool setPlatformControlMode(const as2_msgs::msg::ControlMode &msg)

Set the control mode of the platform.

Parameters:

msg – as2_msgs::msg::ControlMode message with the new control mode desired.

Returns:

true If the control mode is set properly.

Returns:

false If the control mode could not be set properly.

bool takeoff()

Handles the platform takeoff command.

Returns:

true Takeoff command is sended successfully.

Returns:

false Takeoff command is not sended.

bool land()

Handles the platform landing command.

Returns:

true Landing command is sended successfully.

Returns:

false Landing command is not sended.

void alertEvent(const as2_msgs::msg::AlertEvent &msg)

Handles the platform emergency event.

Parameters:

msg – as2_msgs::msg::AlertEvent message with the emergency event.

virtual void sendCommand()

Send command to the platform.

void resetPlatform()

Reset the platform to its initial state. This is primarily used for reseting simulation environments. or at the initialization of the platform.

void resetActuatorCommandMsgs()

Protected Attributes

float cmd_freq_
float info_freq_
as2_msgs::msg::TrajectorySetpoints command_trajectory_msg_
geometry_msgs::msg::PoseStamped command_pose_msg_
geometry_msgs::msg::TwistStamped command_twist_msg_
as2_msgs::msg::Thrust command_thrust_msg_
as2_msgs::msg::PlatformInfo platform_info_msg_
bool has_new_references_ = false

Private Functions

void initialize()
void loadControlModes(const std::string &filename)
inline void publishPlatformInfo()

Publishes the platform info message.

void alertEventCallback(const as2_msgs::msg::AlertEvent::ConstSharedPtr msg)
void setPlatformControlModeSrvCall(const std::shared_ptr<as2_msgs::srv::SetControlMode::Request> request, std::shared_ptr<as2_msgs::srv::SetControlMode::Response> response)

Set Aircraft Control Mode Service Callback.

Parameters:

  • request –

  • response –

void setArmingStateSrvCall(const std::shared_ptr<std_srvs::srv::SetBool::Request> request, std::shared_ptr<std_srvs::srv::SetBool::Response> response)

Set Aircraft Arming State Service Callback.

Parameters:

  • request –

  • response –

void setOffboardModeSrvCall(const std::shared_ptr<std_srvs::srv::SetBool::Request> request, std::shared_ptr<std_srvs::srv::SetBool::Response> response)

Set Aircraft Offboard Mode Service Callback.

Parameters:

  • request –

  • response –

void platformTakeoffSrvCall(const std::shared_ptr<std_srvs::srv::SetBool::Request> request, std::shared_ptr<std_srvs::srv::SetBool::Response> response)

Takeoff Service Callback.

Parameters:

  • request –

  • response –

void platformLandSrvCall(const std::shared_ptr<std_srvs::srv::SetBool::Request> request, std::shared_ptr<std_srvs::srv::SetBool::Response> response)

Land Service Callback.

Parameters:

  • request –

  • response –

void listControlModesSrvCall(const std::shared_ptr<as2_msgs::srv::ListControlModes::Request> request, std::shared_ptr<as2_msgs::srv::ListControlModes::Response> response)

get list of available Control Modes Service Callback

Parameters:

  • request –

  • response –

Private Members

bool sending_commands_ = false
rclcpp::TimerBase::SharedPtr platform_cmd_timer_
rclcpp::TimerBase::SharedPtr platform_info_timer_
as2::PlatformStateMachine state_machine_
std::vector<uint8_t> available_control_modes_
rclcpp::Publisher<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_pub_
rclcpp::Subscription<as2_msgs::msg::TrajectorySetpoints>::SharedPtr trajectory_command_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_command_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_command_sub_
rclcpp::Subscription<as2_msgs::msg::Thrust>::SharedPtr thrust_command_sub_
rclcpp::Subscription<as2_msgs::msg::AlertEvent>::SharedPtr alert_event_sub_
rclcpp::Service<as2_msgs::srv::SetControlMode>::SharedPtr set_platform_mode_srv_
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_arming_state_srv_
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_offboard_mode_srv_
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr platform_takeoff_srv_
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr platform_land_srv_
rclcpp::Service<as2_msgs::srv::ListControlModes>::SharedPtr list_control_modes_srv_
class AlphanumericViewer : public as2::Node

Public Types

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Public Functions

AlphanumericViewer()
void run()
void setupNode()
void printSummaryMenu()
void printSensorMenu()
void printNavigationMenu()
void printPlatformMenu()
void printStream(double var, bool aux)
void printStream3(float var, bool aux)
void printStream(float var, bool aux)
void printSummaryValues()
void printNavigationValues()
void printSensorValues()
void printPlatformValues()
void printBattery()
void printQuadrotorState()
void printControlModeInYaw()
void printControlModeInControl()
void printControlModeInFrame()
void printControlModeOutYaw()
void printControlModeOutControl()
void printControlModeOutFrame()
void printPlatformStatus(int line)
void clearValues()
virtual CallbackReturn on_configure(const rclcpp_lifecycle::State&) override

Callback for the configure state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override

Callback for the deactivate state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override

Callback for the shutdown state.

Parameters:

state –

Returns:

CallbackReturn

Private Functions

void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr _msg)
void twistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr _msg)
void batteryCallback(const sensor_msgs::msg::BatteryState::SharedPtr _msg)
void imuCallback(const sensor_msgs::msg::Imu::SharedPtr _msg)
void platformCallback(const as2_msgs::msg::PlatformInfo::SharedPtr _msg)
void actuatorPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr _msg)
void actuatorThrustCallback(const as2_msgs::msg::Thrust::SharedPtr _msg)
void actuatorSpeedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr _msg)
void controllerCallback(const as2_msgs::msg::ControllerInfo::SharedPtr _msg)
void poseReferenceCallback(const geometry_msgs::msg::PoseStamped::SharedPtr _msg)
void speedReferenceCallback(const geometry_msgs::msg::TwistStamped::SharedPtr _msg)
void gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr _msg)

Private Members

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr self_localization_pose_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr self_localization_speed_sub_
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_
rclcpp::Subscription<sensor_msgs::msg::Temperature>::SharedPtr temperature_sub_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr status_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr actuator_command_pose_sub_
rclcpp::Subscription<as2_msgs::msg::Thrust>::SharedPtr actuator_command_thrust_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr actuator_command_twist_sub_
rclcpp::Subscription<as2_msgs::msg::ControllerInfo>::SharedPtr controller_info_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr position_reference_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr speed_reference_sub_
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_
rclcpp::Subscription<as2_msgs::msg::ControlMode>::SharedPtr control_mode_sub_
geometry_msgs::msg::PoseStamped self_localization_pose_
geometry_msgs::msg::TwistStamped self_localization_twist_
sensor_msgs::msg::BatteryState battery_status_
sensor_msgs::msg::Imu imu_
as2_msgs::msg::PlatformInfo platform_info_
geometry_msgs::msg::PoseStamped actuator_pose_
as2_msgs::msg::Thrust actuator_thrust_
geometry_msgs::msg::TwistStamped actuator_twist_
as2_msgs::msg::ControllerInfo controller_info_
geometry_msgs::msg::PoseStamped reference_pose_
geometry_msgs::msg::TwistStamped reference_twist_
sensor_msgs::msg::NavSatFix gps_
int battery_mode_ = 0
std::stringstream interface_printout_stream
std::stringstream pinterface_printout_stream
bool battery_aux = false
bool altitude_aux = false
bool altitude_sea_level_aux = false
bool ground_speed_aux = false
bool imu_aux = false
bool temperature_aux = false
bool platform_info_aux = false
bool current_speed_reference_aux = false
bool current_pose_reference_aux = false
bool current_trajectory_reference_aux = false
bool actuator_command_pose_aux = false
bool actuator_command_twist_aux = false
bool actuator_command_thrust_aux = false
bool current_pose_aux = false
bool current_speed_aux = false
bool controller_info_aux = false
bool gps_aux = false
bool thrust_aux = false
int last_received_yaw_mode
int last_received_control_mode
int last_received_reference_frame
char command = 0
int window = 0
class ArmService : public as2_behavior_tree::BtServiceNode<std_srvs::srv::SetBool>

Public Functions

ArmService(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion(std::shared_ptr<std_srvs::srv::SetBool::Response> response)
class As2ExternalObjectToTf : public as2::Node

Public Types

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Public Functions

As2ExternalObjectToTf()
void setupNode()
void cleanupNode()
void run()
virtual CallbackReturn on_configure(const rclcpp_lifecycle::State&) override

Callback for the configure state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override

Callback for the deactivate state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override

Callback for the shutdown state.

Parameters:

state –

Returns:

CallbackReturn

Private Functions

void loadObjects(const std::string path)
void setupGPS()
geometry_msgs::msg::TransformStamped gpsToTransform(const sensor_msgs::msg::NavSatFix::SharedPtr gps_pose, const float azimuth, const float pitch, const std::string frame_id, const std::string parent_frame_id)
geometry_msgs::msg::Quaternion azimuthToQuaternion(const std_msgs::msg::Float32::SharedPtr azimuth)
geometry_msgs::msg::Quaternion azimuthPitchToQuaternion(const float azimuth, const float pitch)
void publishPoseAsTransform(const geometry_msgs::msg::PoseStamped::SharedPtr msg, std::string frame_id, std::string parent_frame_id)
void publishPoseAsTransform(const geometry_msgs::msg::Pose::SharedPtr msg, std::string frame_id, std::string parent_frame_id)
void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg, std::string frame_id, std::string parent_frame_id)
void gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg, std::string frame_id, std::string parent_frame_id)
void azimuthCallback(const std_msgs::msg::Float32::SharedPtr msg, std::string frame_id, std::string parent_frame_id)
void mocapCallback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr msg, std::vector<std::tuple<std::string, std::string>> mappings)
void addStaticTransform(const as2_msgs::srv::AddStaticTransform::Request::SharedPtr request, const as2_msgs::srv::AddStaticTransform::Response::SharedPtr response)
void addStaticTransformGps(const as2_msgs::srv::AddStaticTransformGps::Request::SharedPtr request, const as2_msgs::srv::AddStaticTransformGps::Response::SharedPtr response)

Private Members

bool origin_set_ = false
bool use_sim_time = false
std::string config_path_
std::string mocap_topic_
std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> pose_subs_
std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> gps_subs_
std::vector<rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr> azimuth_subs_
rclcpp::Subscription<mocap4r2_msgs::msg::RigidBodies>::SharedPtr mocap_sub_
rclcpp::Service<as2_msgs::srv::AddStaticTransform>::SharedPtr setTrasformSrv
rclcpp::Service<as2_msgs::srv::AddStaticTransformGps>::SharedPtr setTrasformGpsSrv
geographic_msgs::msg::GeoPoint::UniquePtr origin_
std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> objects_subscriptions_
rclcpp::Client<as2_msgs::srv::GetOrigin>::SharedPtr get_origin_srv_
std::unique_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> staticTfBroadcaster
std::unique_ptr<as2::gps::GpsHandler> gps_handler
std::map<std::string, gps_object> gps_poses
class As2MultirotorSimulatorInterface

Public Functions

explicit As2MultirotorSimulatorInterface(as2::Node *node_ptr)
inline ~As2MultirotorSimulatorInterface()
template<typename T>
inline void getParam(const std::string &param_name, T &param_value, bool use_default = false)

Get parameter from the parameter server.

Parameters:

  • param_name – Name of the parameter

  • param_value – Value of the parameter

  • use_default – Use default value if parameter is not found

void convertToOdom(const Kinematics &kinematics, nav_msgs::msg::Odometry &odom, const builtin_interfaces::msg::Time &current_time)

Convert simulator data to odometry message.

Parameters:

  • kinematics – Kinematics data

  • odom – Return odometry message

  • current_time – Current time

void convertToGroundTruth(const Kinematics &kinematics, geometry_msgs::msg::PoseStamped &ground_truth_pose, geometry_msgs::msg::TwistStamped &ground_truth_twist, const builtin_interfaces::msg::Time &current_time)

Convert simulator data to ground truth message.

Parameters:

  • kinematics – Kinematics data

  • ground_truth_pose – Return ground truth pose message

  • ground_truth_twist – Return ground truth twist message

  • current_time – Current time

bool processCommand(geometry_msgs::msg::PoseStamped &pose_command)
bool processCommand(geometry_msgs::msg::TwistStamped &twist_command)
bool processCommand(as2_msgs::msg::TrajectorySetpoints trajectory_command)

Private Types

using Simulator = multirotor::Simulator<double, 4>
using SimulatorParams = multirotor::SimulatorParams<double, 4>
using Kinematics = multirotor::state::internal::Kinematics<double>

Private Members

as2::Node *node_ptr_
as2::tf::TfHandler tf_handler_
std::string frame_id_baselink_ = "base_link"
std::string frame_id_odom_ = "odom"
std::string frame_id_earth_ = "earth"
Eigen::Vector3d initial_position_
Eigen::Quaterniond initial_orientation_
bool using_odom_for_control_ = false
class AStarPlanner

Public Functions

inline AStarPlanner()
inline void setOcuppancyGrid(const cv::Mat &mat)
inline void setOriginPoint(const cv::Point2i &point)
inline void setGoal(const cv::Point2i &point)
inline std::vector<cv::Point2i> solveGraph()

Private Functions

inline void addNeighborsToVisit(const NodePtr &node)
inline NodePtr findNextNodeToVisit()

Private Members

cv::Mat ocuppancy_grid_
cv::Point2i goal_
cv::Point2i origin_point_
std::unordered_map<int, NodePtr> nodes_visited_
std::unordered_map<int, NodePtr> nodes_to_visit_
std::vector<cv::Point2i> valid_movements_
class AStarSearcher : public GraphSearcher<cv::Mat>

Public Functions

nav_msgs::msg::OccupancyGrid update_grid(const nav_msgs::msg::OccupancyGrid &occ_grid, const Point2i &drone_pose, double safety_distance)

Update the occupancy grid.

Parameters:

  • occ_grid – occupancy grid

  • drone_pose – drone pose in cell coordinates

  • safety_distance – safety distance in meters

cv::Point2i cellToPixel(Point2i cell, int rows, int cols)

Convert cell coordinates to pixel coordinates.

Parameters:

  • cell – cell coordinates

  • rows – number of rows

  • cols – number of columns

cv::Point2i cellToPixel(Point2i cell, cv::Mat map)

Convert cell coordinates to pixel coordinates.

Parameters:

  • cell – cell coordinates

  • map – map

cv::Point2i cellToPixel(Point2i cell, nav_msgs::msg::MapMetaData map_info)

Convert cell coordinates to pixel coordinates.

Parameters:

  • cell – cell coordinates

  • map_info – map metadata

Point2i pixelToCell(cv::Point2i pixel, nav_msgs::msg::MapMetaData map_info)

Convert pixel coordinates to cell coordinates.

Parameters:

  • pixel – pixel coordinates

  • map_info – map metadata

cv::Mat gridToImg(nav_msgs::msg::OccupancyGrid occ_grid, double thresh = 30, bool unknown_as_free = false)

Occupancy grid to binary image

Parameters:

  • occ_grid – occupancy grid

  • thresh – threshold value

  • unknown_as_free – if true, unknown cells are considered free

Returns:

: binary image

nav_msgs::msg::OccupancyGrid imgToGrid(const cv::Mat img, const std_msgs::msg::Header &header, double grid_resolution)

Binary image to occupancy grid

Parameters:

  • img – binary image

  • header – header of the occupancy grid

  • grid_resolution – resolution of the occupancy grid

Returns:

: occupancy grid

Protected Functions

virtual double calc_h_cost(Point2i current, Point2i end) override
virtual double calc_g_cost(Point2i current) override
virtual int hash_key(Point2i point) override
virtual bool cell_in_limits(Point2i point) override
virtual bool cell_occuppied(Point2i point) override

Protected Attributes

bool use_heuristic_ = true
class AzimuthBridge : public rclcpp::Node

Public Functions

AzimuthBridge()

Private Members

std::shared_ptr<gz::transport::Node> ign_node_ptr_
std::string model_name_

Private Static Functions

static void ignitionGroundTruthCallback(const gz::msgs::Odometry &ign_msg, const gz::transport::MessageInfo &msg_info)
static float toEulerYaw(Quaternion q)
static float toAzimuth(float yaw)

Private Static Attributes

static rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr ps_pub_ = nullptr
template<class MessageT>
class BasicBehavior : public as2::Node, public as2::Node

Public Types

using GoalHandleAction = rclcpp_action::ServerGoalHandle<MessageT>
using GoalHandleAction = rclcpp_action::ServerGoalHandle<MessageT>

Public Functions

inline explicit BasicBehavior(const std::string &name)
virtual rclcpp_action::GoalResponse onAccepted(const std::shared_ptr<const typename MessageT::Goal> goal) = 0
virtual rclcpp_action::CancelResponse onCancel(const std::shared_ptr<GoalHandleAction> goal_handle) = 0
virtual void onExecute(const std::shared_ptr<GoalHandleAction> goal_handle) = 0
inline explicit BasicBehavior(const std::string &name)
virtual rclcpp_action::GoalResponse onAccepted(const std::shared_ptr<const typename MessageT::Goal> goal) = 0
virtual rclcpp_action::CancelResponse onCancel(const std::shared_ptr<GoalHandleAction> goal_handle) = 0
virtual void onExecute(const std::shared_ptr<GoalHandleAction> goal_handle) = 0

Private Functions

inline rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const typename MessageT::Goal> goal)
inline rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandleAction> goal_handle)
inline void handleAccepted(const std::shared_ptr<GoalHandleAction> goal_handle)
inline rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const typename MessageT::Goal> goal)
inline rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandleAction> goal_handle)
inline void handleAccepted(const std::shared_ptr<GoalHandleAction> goal_handle)

Private Members

std::thread execution_thread_
rclcpp_action::Server<MessageT>::SharedPtr action_server_
class BasicMotionReferenceHandler

Subclassed by as2::motionReferenceHandlers::ACROMotion, as2::motionReferenceHandlers::HoverMotion, as2::motionReferenceHandlers::PositionMotion, as2::motionReferenceHandlers::SpeedInAPlaneMotion, as2::motionReferenceHandlers::SpeedMotion, as2::motionReferenceHandlers::TrajectoryMotion

Public Functions

explicit BasicMotionReferenceHandler(as2::Node *as2_ptr, const std::string &ns = "")
~BasicMotionReferenceHandler()

Protected Functions

bool sendThrustCommand()
bool sendPoseCommand()
bool sendTwistCommand()
bool sendTrajectoryCommand()
bool checkMode()

Protected Attributes

as2::Node *node_ptr_
std::string namespace_
as2_msgs::msg::TrajectorySetpoints command_trajectory_msg_
geometry_msgs::msg::PoseStamped command_pose_msg_
geometry_msgs::msg::TwistStamped command_twist_msg_
as2_msgs::msg::Thrust command_thrust_msg_
as2_msgs::msg::ControlMode desired_control_mode_

Private Functions

bool setMode(const as2_msgs::msg::ControlMode &mode)

Private Static Attributes

static int number_of_instances_ = 0
static rclcpp::Subscription<as2_msgs::msg::ControllerInfo>::SharedPtr controller_info_sub_ = nullptr
static as2_msgs::msg::ControlMode current_mode_ = as2_msgs::msg::ControlMode()
static rclcpp::Publisher<as2_msgs::msg::TrajectorySetpoints>::SharedPtr command_traj_pub_ = nullptr
static rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr command_pose_pub_ = nullptr
static rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr command_twist_pub_ = nullptr
static rclcpp::Publisher<as2_msgs::msg::Thrust>::SharedPtr command_thrust_pub_ = nullptr
template<typename actionT>
class BehaviorServer : public as2::Node

Public Types

using GoalHandleAction = rclcpp_action::ServerGoalHandle<actionT>
using BehaviorStatus = as2_msgs::msg::BehaviorStatus
using start_srv = typename actionT::Impl::SendGoalService
using modify_srv = start_srv
using result_srv = typename actionT::Impl::GetResultService
using feedback_msg = typename actionT::Impl::FeedbackMessage
using goal_status_msg = typename actionT::Impl::GoalStatusMessage
using cancel_srv = typename actionT::Impl::CancelGoalService

Public Functions

void register_action()
rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const typename actionT::Goal> goal)
rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandleAction> goal_handle)
void handleAccepted(const std::shared_ptr<GoalHandleAction> goal_handle)
BehaviorServer(const std::string &name, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
virtual bool on_activate(std::shared_ptr<const typename actionT::Goal> goal)
virtual bool on_modify(std::shared_ptr<const typename actionT::Goal> goal)
virtual bool on_deactivate(const std::shared_ptr<std::string> &message)
virtual bool on_pause(const std::shared_ptr<std::string> &message)
virtual bool on_resume(const std::shared_ptr<std::string> &message)
inline virtual void on_execution_end(const ExecutionStatus &state)
virtual ExecutionStatus on_run(const typename std::shared_ptr<const typename actionT::Goal> &goal, typename std::shared_ptr<typename actionT::Feedback> &feedback_msg, typename std::shared_ptr<typename actionT::Result> &result_msg)
bool activate(std::shared_ptr<const typename actionT::Goal> goal)
void modify(const typename modify_srv::Request::SharedPtr request, typename modify_srv::Response::SharedPtr response)
void deactivate(const typename std_srvs::srv::Trigger::Request::SharedPtr goal, typename std_srvs::srv::Trigger::Response::SharedPtr result)
void pause(const typename std_srvs::srv::Trigger::Request::SharedPtr goal, typename std_srvs::srv::Trigger::Response::SharedPtr result)
void resume(const typename std_srvs::srv::Trigger::Request::SharedPtr goal, typename std_srvs::srv::Trigger::Response::SharedPtr result)
void run(const typename std::shared_ptr<GoalHandleAction> &goal_handle_action)
inline void timer_callback()
void publish_behavior_status()

Public Members

std::string action_name_
rclcpp_action::Server<actionT>::SharedPtr action_server_
std::shared_ptr<GoalHandleAction> goal_handle_
as2_msgs::msg::BehaviorStatus behavior_status_

Private Functions

std::string generate_name(const std::string &name)
void register_service_servers()
void register_publishers()
void register_timers()
void register_run_timer()
void cleanup_run_timer(const ExecutionStatus &status)

Private Members

rclcpp::Service<start_srv>::SharedPtr start_srv_
rclcpp::Service<modify_srv>::SharedPtr modify_srv_
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_srv_
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_srv_
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resume_srv_
rclcpp::Publisher<feedback_msg>::SharedPtr feedback_pub_
rclcpp::Publisher<goal_status_msg>::SharedPtr goal_status_pub_
rclcpp::Publisher<BehaviorStatus>::SharedPtr behavior_status_pub_
rclcpp::TimerBase::SharedPtr behavior_status_timer_
rclcpp::TimerBase::SharedPtr run_timer_
template<class ActionT>
class BtActionNode : public BT::ActionNodeBase
#include <bt_action_node.hpp>

Abstract class representing an action based BT node.

Template Parameters:

ActionT – Type of action

Public Functions

inline BtActionNode(const std::string &xml_tag_name, const std::string &action_name, const BT::NodeConfiguration &conf)

A as2_behavior_tree::BtActionNode constructor.

Parameters:

  • xml_tag_name – Name for the XML tag for this node

  • action_name – Action name this node creates a client for

  • conf – BT node configuration

BtActionNode() = delete
inline virtual ~BtActionNode()
inline void createActionClient(const std::string &action_name)

Create instance of an action client.

Parameters:

action_name – Action name to create client for

inline virtual void on_tick()

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

inline virtual void on_wait_for_result(std::shared_ptr<const typename ActionT::Feedback>)

Function to perform some user-defined operation after a timeout waiting for a result that hasn’t been received yet. Also provides access to the latest feedback message from the action server. Feedback will be nullptr in subsequent calls to this function if no new feedback is received while waiting for a result.

Parameters:

feedback – shared_ptr to latest feedback message, nullptr if no new feedback was received

inline virtual BT::NodeStatus on_success()

Function to perform some user-defined operation upon successful completion of the action. Could put a value on the blackboard.

Returns:

BT::NodeStatus Returns SUCCESS by default, user may override return another value

inline virtual BT::NodeStatus on_aborted()

Function to perform some user-defined operation whe the action is aborted.

Returns:

BT::NodeStatus Returns FAILURE by default, user may override return another value

inline virtual BT::NodeStatus on_cancelled()

Function to perform some user-defined operation when the action is cancelled.

Returns:

BT::NodeStatus Returns SUCCESS by default, user may override return another value

inline BT::NodeStatus tick() override

The main override required by a BT action.

Returns:

BT::NodeStatus Status of tick execution

inline void halt() override

The other (optional) override required by a BT action. In this case, we make sure to cancel the ROS2 action if it is still running.

Public Static Functions

static inline BT::PortsList providedBasicPorts(BT::PortsList addition)

Any subclass of BtActionNode that accepts parameters must provide a providedPorts method and call providedBasicPorts in it.

Parameters:

addition – Additional ports to add to BT port list

Returns:

BT::PortsList Containing basic ports along with node-specific ports

static inline BT::PortsList providedPorts()

Creates list of BT ports.

Returns:

BT::PortsList Containing basic ports along with node-specific ports

Protected Functions

inline bool should_cancel_goal()

Function to check if current goal should be cancelled.

Returns:

bool True if current goal should be cancelled, false otherwise

inline void send_new_goal()

Function to send new goal to action server.

inline void result_callback(const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult &result)
inline bool is_future_goal_handle_complete(std::chrono::milliseconds &elapsed)

Function to check if the action server acknowledged a new goal.

Parameters:

elapsed – Duration since the last goal was sent and future goal handle has not completed. After waiting for the future to complete, this value is incremented with the timeout value.

Returns:

boolean True if future_goal_handle_ returns SUCCESS, False otherwise

inline void increment_recovery_count()

Function to increment recovery count on blackboard if this node wraps a recovery.

Protected Attributes

std::string action_name_
std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_
ActionT::Goal goal_
bool goal_updated_ = {false}
bool goal_result_available_ = {false}
rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_
rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_
std::shared_ptr<const typename ActionT::Feedback> feedback_
rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
std::chrono::milliseconds server_timeout_
std::chrono::milliseconds bt_loop_duration_
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>> future_goal_handle_
rclcpp::Time time_goal_sent_
template<class ServiceT>
class BtServiceNode : public BT::ActionNodeBase
#include <bt_service_node.hpp>

Abstract class representing a service based BT node.

Template Parameters:

ServiceT – Type of service

Public Functions

inline BtServiceNode(const std::string &service_node_name, const BT::NodeConfiguration &conf)

A as2_behavior_tree::BtServiceNode constructor.

Parameters:

  • service_node_name – Service name this node creates a client for

  • conf – BT node configuration

BtServiceNode() = delete
inline virtual ~BtServiceNode()
inline BT::NodeStatus tick() override

The main override required by a BT service.

Returns:

BT::NodeStatus Status of tick execution

inline void halt() override

The other (optional) override required by a BT service.

inline virtual void on_tick()

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

inline virtual BT::NodeStatus on_completion(std::shared_ptr<typename ServiceT::Response>)

Function to perform some user-defined operation upon successful completion of the service. Could put a value on the blackboard.

Parameters:

response – can be used to get the result of the service call in the BT Node.

Returns:

BT::NodeStatus Returns SUCCESS by default, user may override to return another value

inline virtual BT::NodeStatus check_future()

Check the future and decide the status of BT.

Returns:

BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise

inline virtual void on_wait_for_result()

Function to perform some user-defined operation after a timeout waiting for a result that hasn’t been received yet.

Public Static Functions

static inline BT::PortsList providedBasicPorts(BT::PortsList addition)

Any subclass of BtServiceNode that accepts parameters must provide a providedPorts method and call providedBasicPorts in it.

Parameters:

addition – Additional ports to add to BT port list

Returns:

BT::PortsList Containing basic ports along with node-specific ports

static inline BT::PortsList providedPorts()

Creates list of BT ports.

Returns:

BT::PortsList Containing basic ports along with node-specific ports

Protected Functions

inline void increment_recovery_count()

Function to increment recovery count on blackboard if this node wraps a recovery.

Protected Attributes

std::string service_name_
std::string service_node_name_
std::shared_ptr<rclcpp::Client<ServiceT>> service_client_
std::shared_ptr<typename ServiceT::Request> request_
rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
std::chrono::milliseconds server_timeout_
std::chrono::milliseconds bt_loop_duration_
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_
bool request_sent_ = {false}
rclcpp::Time sent_time_
template<typename T>
class BucketPrioQueue
#include <bucketedqueue.h>

Priority queue for integer coordinates with squared distances as priority.

A priority queue that uses buckets to group elements with the same priority. The individual buckets are unsorted, which increases efficiency if these groups are large. The elements are assumed to be integer coordinates, and the priorities are assumed to be squared Euclidean distances (integers).

Public Functions

BucketPrioQueue()

Standard constructor.

Standard constructor. When called for the first time it creates a look up table that maps square distances to bucket numbers, which might take some time…

inline void clear()
bool empty()

Checks whether the Queue is empty.

void push(int prio, T t)

push an element

T pop()

return and pop the element with the lowest squared distance */

inline int size()
inline int getNumBuckets()
inline int getTopPriority()

Private Types

typedef std::map<int, std::queue<T>> BucketType

Private Members

int count
BucketType buckets
BucketType::iterator nextPop
class Camera : public as2::sensors::TFStatic, protected as2::sensors::GenericSensor
#include <sensor.hpp>

Class to handle the camera sensor.

Public Functions

Camera(as2::Node *node_ptr, const std::string &prefix = "", const float pub_freq = -1.0f, bool add_sensor_measurements_base = true, const std::string &camera_link = "camera_link")

Construct a new Camera object.

Parameters:

  • node_ptr – Pointer to the node

  • prefix – ROS 2 parameter prefix. If not using ROS 2 parameters, give the camera name (e.g. “camera_front”)

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

  • info_name – Name of the camera info topic. Default is “camera_info”

  • camera_link – Name of the camera link frame id. Default is “camera_link”

inline Camera(const std::string &prefix, as2::Node *node_ptr, const float pub_freq = -1.0f, bool add_sensor_measurements_base = true, const std::string &camera_link = "camera_link")

Construct a new Camera object. DEPRECATED.

Parameters:

  • prefix – ROS 2 parameter prefix. If not using ROS 2 parameters, give the camera name (e.g. “camera_front”)

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

  • info_name – Name of the camera info topic. Default is “camera_info”

  • camera_link – Name of the camera link frame id. Default is “camera_link”

inline void setParameters(const sensor_msgs::msg::CameraInfo &camera_info, const std::string &encoding, const std::string &camera_model = "pinhole")

Set Camera parameters. DEPRECATED.

Parameters:

  • camera_info – Camera info message

  • encoding – Encoding of the camera

  • camera_model – Camera model. Default is “pinhole”

virtual ~Camera()

Destroy the Camera object.

void updateData(const sensor_msgs::msg::Image &img)

Update the data of the camera.

Parameters:

img – Image message

void updateData(const cv::Mat &img)

Update the data of the camera.

Parameters:

img – Image message

void setCameraInfo(const sensor_msgs::msg::CameraInfo &camera_info)

Set camera info parameters.

Parameters:

camera_info – Camera info message

void setCameraLinkTransform(const std::string &parent_frame_id, const float x, const float y, const float z, const float roll, const float pitch, const float yaw)

Set camera link frame transformation relative to the parent frame.

Parameters:

  • parent_frame_id – Parent frame ID (e.g. base_link)

  • x – X position (m)

  • y – Y position (m)

  • z – Z position (m)

  • roll – Roll (rad)

  • pitch – Pitch (rad)

  • yaw – Yaw (rad)

void setEncoding(const std::string &encoding)

Set camera encoding.

Parameters:

encoding – Encoding of the camera

void readCameraInfoFromROSParameters(const std::string &prefix = "")

Read camera info from ROS parameters.

Parameters:

  • node_ptr – Pointer to the node

  • prefix – ROS 2 parameter prefix. By default is “”

void readCameraTranformFromROSParameters(const std::string &prefix = "")

Read camera transform from ROS parameters.

Parameters:

  • node_ptr – Pointer to the node

  • prefix – ROS 2 parameter prefix. By default is “”

Private Functions

void setup()

Setup the camera info.

virtual void publishData()

Publish the data in a topic.

std::string processParametersPrefix(const std::string &prefix)

Process the parameters prefix If not empty and not ending with a dot, add a dot at the end.

Parameters:

prefix – Prefix

Returns:

std::string Processed prefix

template<std::size_t N>
inline bool convertVectorToArray(const std::vector<double> &vec, std::array<double, N> &array)

Convert a vector to an array.

Parameters:

  • vec – Vector to convert.

  • array – Array to store the data.

Returns:

true If the conversion was successful.

Private Members

as2::Node *node_ptr_ = nullptr
std::string camera_base_topic_
std::string camera_link_frame_
bool setup_ = false
std::string encoding_ = "rgb8"
std::string camera_name_
std::shared_ptr<image_transport::CameraPublisher> it_camera_publisher_ptr_
sensor_msgs::msg::Image image_data_
sensor_msgs::msg::CameraInfo camera_info_
class CellNode
#include <cell_node.hpp>

CellNode class, a node in a grid.

Public Functions

inline CellNode(const Point2i &coordinates, const CellNodePtr &parent_ptr, double g_cost, double h_cost = 0)

Constructor for CellNode class.

Parameters:

  • coordinates – coordinates of the cell

  • parent_ptr – parent node

  • g_cost – cost to reach this node

  • h_cost – heuristic cost to reach the goal

inline void set_g_cost(double g_cost)
inline Point2i coordinates()
inline int x()
inline int y()
inline CellNodePtr parent_ptr()
inline double get_g_cost()
inline double get_h_cost()
inline double get_total_cost()

Protected Attributes

double g_cost_
double h_cost_

Private Members

Point2i coordinates_
CellNodePtr parent_ptr_
class ControllerBase
#include <controller_base.hpp>

Base class for controller plugins loaded by ControllerManager.

Plugins inherit from this class and implement the pure-virtual hooks. The base owns the per-tick state cache, hover latch and essential-parameter tracking so plugins only deal with controller-specific logic.

Subclassed by differential_flatness_controller::Plugin, pid_speed_controller::Plugin

Public Functions

ControllerBase() = default
virtual ~ControllerBase() = default
ControllerBase(const ControllerBase&) = delete
ControllerBase &operator=(const ControllerBase&) = delete
inline void initialize(as2::Node *node_ptr)

Initialize the plugin.

Called by ControllerManager after the per-plugin setters have been configured. Declares frame parameters, runs ownInitialize() and seeds the pending-essentials set from getEssentialParameters().

Parameters:

node_ptr – Non-owning pointer to the controller node.

inline void setTfHandler(as2::tf::TfHandler *tf_handler)

Inject the TfHandler owned by ControllerManager.

Parameters:

tf_handler – Non-owning pointer to the TfHandler instance.

inline void setBaseLinkFrameId(const std::string &frame_id)

Set the namespaced FLU (base_link) frame id used by the controller node.

Parameters:

frame_id – Fully-qualified base_link frame id.

inline void setPluginParamNamespace(const std::string &ns)

Set the per-plugin parameter namespace (e.g. “pid_speed_controller”).

Plugins compose their parameter names with param(“foo”) which returns “<namespace>.foo”.

Parameters:

ns – Plugin parameter namespace.

inline void requestHoverLatch()

Request that the next state update synthesizes a hover reference.

Called by ControllerHandler after a successful setMode(HOVER).

inline void setDesiredPoseFrameId(const std::string &frame_id)

Override the pose frame id used by the controller for state and references.

Typically called from setMode() to react to the active control mode.

Parameters:

frame_id – Fully-qualified pose frame id.

inline void setDesiredTwistFrameId(const std::string &frame_id)

Override the twist frame id used by the controller for state and references.

See setDesiredPoseFrameId().

Parameters:

frame_id – Fully-qualified twist frame id.

inline std::string getDesiredPoseFrameId() const

Frame id (already namespaced) that the plugin expects for pose state and references.

inline std::string getDesiredTwistFrameId() const

Frame id (already namespaced) that the plugin expects for twist state and references.

inline bool essentialParamsReady() const

Whether all essential parameters have been received and applied.

Consulted by ControllerHandler before accepting setMode.

inline void dispatchParameters(const std::vector<rclcpp::Parameter> &batch)

Filter a parameter batch by the plugin namespace and dispatch each match to updateParameter().

Tracks the essential names still pending and, the first time the set empties, flips essential_params_ready_ and calls onAllParametersRead() exactly once. Invoked by ControllerManager with the initial bulk and by ControllerHandler::parametersCallback for runtime changes. The pending set is populated at the end of initialize() from getEssentialParameters() so plugins do not need to manage this state themselves.

Parameters:

batch – Parameter batch from rclcpp.

inline void updateState(const geometry_msgs::msg::PoseStamped &pose_msg, const geometry_msgs::msg::TwistStamped &twist_msg)

Update the latest state (pose + twist) seen by the controller.

Validates that the incoming frames match the desired ones, caches the state, consumes a pending hover latch (if any) and forwards the state to onUpdateState().

Parameters:

  • pose_msg – Latest pose message received by the controller node.

  • twist_msg – Latest twist message received by the controller node.

inline void updateReference(const geometry_msgs::msg::PoseStamped &ref)

Update the pose reference.

Parameters:

ref – Latest pose reference message received by the controller node.

inline void updateReference(const geometry_msgs::msg::TwistStamped &ref)

Update the twist reference.

Parameters:

ref – Latest twist reference message received by the controller node.

inline void updateReference(const as2_msgs::msg::TrajectorySetpoints &ref)

Update the trajectory reference.

Parameters:

ref – Latest trajectory reference message received by the controller node.

inline void updateReference(const as2_msgs::msg::Thrust &ref)

Update the thrust reference.

Parameters:

ref – Latest thrust reference message received by the controller node.

inline virtual void ownInitialize()

Plugin-specific initialization, called from initialize().

virtual void onUpdateState(const geometry_msgs::msg::PoseStamped &pose_msg, const geometry_msgs::msg::TwistStamped &twist_msg) = 0

Plugin hook called by the base after frame validation and hover latch.

The plugin updates its internal state/integrators here.

Parameters:

  • pose_msg – Latest validated pose message.

  • twist_msg – Latest validated twist message.

inline virtual void onUpdateReference(const geometry_msgs::msg::PoseStamped&)

Plugin hook for pose reference. Default: no-op.

Parameters:

ref – Latest pose reference message.

inline virtual void onUpdateReference(const geometry_msgs::msg::TwistStamped&)

Plugin hook for twist reference. Default: no-op.

Parameters:

ref – Latest twist reference message.

inline virtual void onUpdateReference(const as2_msgs::msg::TrajectorySetpoints&)

Plugin hook for trajectory reference. Default: no-op.

Parameters:

ref – Latest trajectory reference message.

inline virtual void onUpdateReference(const as2_msgs::msg::Thrust&)

Plugin hook for thrust reference. Default: no-op.

Parameters:

ref – Latest thrust reference message.

virtual bool computeOutput(double dt, geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::TwistStamped &twist, as2_msgs::msg::Thrust &thrust) = 0

Compute the output signal of the controller plugin.

Parameters:

  • dt – Time elapsed since the last call to computeOutput().

  • pose – Output pose; frame depends on the output control mode.

  • twist – Output twist; frame depends on the output control mode.

  • thrust – Output thrust.

Returns:

true if the output is valid.

virtual bool setMode(const as2_msgs::msg::ControlMode &mode_in, const as2_msgs::msg::ControlMode &mode_out) = 0

Update the control mode to be used by the controller plugin.

Parameters:

  • mode_in – Input control mode requested.

  • mode_out – Output control mode requested.

Returns:

true if the in-out control mode configuration is valid.

virtual std::vector<std::string> getEssentialParameters() const = 0

Names of the parameters whose presence is required before the plugin can accept setMode.

Names must be already namespaced with <plugin_name>.. The manager tracks reception of these names and invokes onAllParametersRead() once the last one arrives.

Returns:

Vector of fully-qualified essential parameter names.

virtual void updateParameter(const rclcpp::Parameter &parameter) = 0

Apply a single parameter to the plugin.

Called by the manager for every parameter (essential or not) with a name starting with <plugin_name>., both at startup and on runtime changes. The plugin can read essentialParamsReady() to decide whether to apply at runtime or defer the configuration to onAllParametersRead().

Parameters:

parameter – Parameter to apply.

inline virtual void onAllParametersRead()

Hook fired once when every essential parameter has been delivered.

The latch essentialParamsReady() is already true on entry. Plugins use this hook to perform first-time configuration of the underlying solver/controller from the now-fully-populated parameter set.

inline virtual void reset()

Reset the controller.

Default implementation clears the per-mode flags maintained by the base. Plugins should override and call ControllerBase::reset() so the base state is also cleared. essential_params_ready_ is intentionally NOT cleared here; it is a monotonic latch — parameters are read once at startup via the rclcpp parameter callback, and reset() runs on every successful setMode and would otherwise leave the latch permanently false, rejecting all subsequent mode transitions.

inline virtual void latchHoverReference(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped&)

Default hover latch: synthesize a single-point trajectory at the cached state.

Override in plugins that do not consume trajectory references; in that case feed the hover via updateReference(pose) / updateReference(twist) with twist set to zero.

Parameters:

  • pose – Cached state pose used as the hover anchor.

  • twist – Cached state twist (unused by the default).

inline bool isReferenceReceived() const

Whether at least one motion reference has been received.

Protected Functions

inline void setReferenceReceived(bool value)

Mark the reference as received.

Parameters:

value – Value to set for reference_received_.

inline as2::Node *getNodePtr() const

Non-owning pointer to the controller node.

inline as2::tf::TfHandler *getTfHandler() const

TfHandler owned by ControllerManager.

May be null until the manager injects it; access from ownInitialize() or later.

inline const std::string &getBaseLinkFrameId() const

Namespaced frame id of the body FLU/base_link frame.

inline const std::string &getPluginParamNamespace() const

Per-plugin parameter namespace (e.g. “pid_speed_controller”).

inline std::string param(const std::string &tail) const

Compose a fully-qualified parameter name under the plugin namespace.

Parameters:

tail – Trailing parameter name to append to the plugin namespace.

Returns:

Fully-qualified parameter name.

inline const geometry_msgs::msg::PoseStamped &getStatePose() const

Last validated state pose cached by the base.

inline const geometry_msgs::msg::TwistStamped &getStateTwist() const

Last validated state twist cached by the base.

inline bool isStateReceived() const

Whether at least one state message has been received and validated.

inline bool isHoverPending() const

Whether a hover latch is pending consumption on the next state update.

Private Functions

inline void declareFrameParameters()

Declare and read the desired_pose_frame / desired_twist_frame parameters.

Stores their namespaced values in desired_pose_frame_id_ and desired_twist_frame_id_.

Private Members

as2::Node *node_ptr_ = nullptr
as2::tf::TfHandler *tf_handler_ = nullptr
std::string base_link_frame_id_
std::string plugin_param_namespace_
std::string desired_pose_frame_id_
std::string desired_twist_frame_id_
geometry_msgs::msg::PoseStamped state_pose_
geometry_msgs::msg::TwistStamped state_twist_
bool state_received_ = false
bool reference_received_ = false
bool hover_pending_ = false
bool essential_params_ready_ = false
std::set<std::string> pending_essentials_
class ControllerHandler
#include <controller_handler.hpp>

Orchestrates the controller plugin life cycle inside the ControllerManager.

Owns the subscriptions for state and motion references, the publishers for actuator commands and debug topics, the controller/set_control_mode service server and the periodic control timer. The ControllerHandler does not own the plugin nor the TfHandler: both are injected by the ControllerManager.

Public Functions

ControllerHandler(std::shared_ptr<as2_motion_controller_plugin_base::ControllerBase> controller, as2::Node *node, as2::tf::TfHandler *tf_handler)

Construct a handler bound to a controller plugin and a node.

Parameters:

  • controller – Loaded controller plugin shared with ControllerManager.

  • node – Controller node providing logger, parameters and IO.

  • tf_handler – Non-owning pointer to the TfHandler owned by ControllerManager.

inline virtual ~ControllerHandler()
rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)

rclcpp on_set_parameters callback applied to all parameter changes.

Forwards plugin-namespaced entries to ControllerBase::dispatchParameters and updates handler-owned state (frame ids, debug publishers).

Parameters:

parameters – Batch of parameters being set.

Returns:

SetParametersResult with the merged success flag.

void getMode(as2_msgs::msg::ControlMode &mode_in, as2_msgs::msg::ControlMode &mode_out)

Read the currently negotiated input/output control modes.

Parameters:

  • mode_in – Output: latest negotiated input control mode.

  • mode_out – Output: latest negotiated output control mode.

void setInputControlModesAvailables(const std::vector<uint8_t> &available_modes)

Declare the input control modes the plugin can accept.

Parameters:

available_modes – Bitmasks of supported input modes.

void setOutputControlModesAvailables(const std::vector<uint8_t> &available_modes)

Declare the output control modes the plugin can produce.

Parameters:

available_modes – Bitmasks of supported output modes.

void reset()

Clear the handler-side mode and reference state.

Private Functions

void stateCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)

Subscription callback for self_localization/twist.

Looks up the matching pose via TF, transforms both messages into the desired frames and forwards the pair to ControllerBase::updateState.

Parameters:

msg – Latest twist message published by the localization stack.

void refPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)

Subscription callback for motion_reference/pose.

Parameters:

msg – Latest pose reference message.

void refTwistCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)

Subscription callback for motion_reference/twist.

Parameters:

msg – Latest twist reference message.

void refTrajCallback(const as2_msgs::msg::TrajectorySetpoints::SharedPtr msg)

Subscription callback for motion_reference/trajectory.

Parameters:

msg – Latest trajectory reference message.

void refThrustCallback(const as2_msgs::msg::Thrust::SharedPtr msg)

Subscription callback for motion_reference/thrust.

Parameters:

msg – Latest thrust reference message.

void platformInfoCallback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)

Subscription callback for platform/info.

Parameters:

msg – Latest platform info message used to track platform readiness.

void setControlModeSrvCall(const as2_msgs::srv::SetControlMode::Request::SharedPtr request, as2_msgs::srv::SetControlMode::Response::SharedPtr response)

Service handler for controller/set_control_mode.

Negotiates the input/output mode pair with the platform, applies the negotiated mode to the plugin, and arms the hover latch when the requested mode is HOVER.

Parameters:

  • request – Mode requested by the upstream client.

  • response – Service response with the success flag.

bool listPlatformAvailableControlModes()

Query the platform for the supported input control modes.

Returns:

true if the platform service responded successfully.

void controlTimerCallback()

Periodic callback that runs one tick of the control loop.

std::string getFrameIdByReferenceFrame(uint8_t reference_frame)

Map a control-mode reference-frame enum to the corresponding TF frame id.

Parameters:

reference_frame – Reference-frame enum from as2_msgs::msg::ControlMode.

Returns:

Fully-qualified TF frame id.

bool findSuitableOutputControlModeForPlatformInputMode(uint8_t &output_mode, const uint8_t input_mode)

Find a controller output mode that matches the platform’s input mode.

Parameters:

  • output_mode – Output: matching output mode bitmask, if any.

  • input_mode – Platform input mode the output must feed.

Returns:

true if a match was found.

bool checkSuitabilityInputMode(uint8_t &input_mode, const uint8_t output_mode)

Check whether a controller input mode is compatible with a given output mode.

Parameters:

  • input_mode – In/out: input mode under evaluation; refined on success.

  • output_mode – Output mode the input must feed.

Returns:

true if the combination is supported.

bool setPlatformControlMode(const as2_msgs::msg::ControlMode &mode)

Send a set_platform_control_mode request to the platform.

Parameters:

mode – Control mode the platform should enter.

Returns:

true if the platform accepted the new mode.

bool findSuitableControlModes(uint8_t &input_mode, uint8_t &output_mode)

Find a self-consistent input/output mode pair given the active modes.

Parameters:

  • input_mode – In/out: candidate input mode; refined on success.

  • output_mode – In/out: candidate output mode; refined on success.

Returns:

true if a compatible pair was found.

bool trySetPlatformHover()

Drive the platform into HOVER directly, bypassing the plugin.

Returns:

true if the platform accepted the hover request.

bool tryToBypassController(const uint8_t input_mode, uint8_t &output_mode)

Negotiate a bypass path that skips the plugin for the requested input mode.

Parameters:

  • input_mode – Input mode the upstream client asked for.

  • output_mode – Output: bypass output mode, if any.

Returns:

true if the platform can ingest input_mode without the plugin.

void sendCommand()

Compute the controller output for the current tick.

Builds the input messages from the cached references, calls ControllerBase::computeOutput and stores the result for publishCommand.

void publishCommand()

Publish the latest controller command on actuator_command/*.

void initializeDebugPublishers()

Read the optional debug.<name>_topic parameters and create the debug publishers.

An empty topic name disables the corresponding publisher.

void publishDebug(const rclcpp::Time &tick)

Publish the standard state and reference debug topics for the current tick.

Parameters:

tick – Timestamp shared by all standard debug messages.

Private Members

as2::Node *node_ptr_
std::vector<uint8_t> controller_available_modes_in_
std::vector<uint8_t> controller_available_modes_out_
std::vector<uint8_t> platform_available_modes_in_
std::string enu_frame_id_ = "odom"
std::string flu_frame_id_ = "base_link"
std::string input_pose_frame_id_ = "odom"
std::string input_twist_frame_id_ = "odom"
std::string output_pose_frame_id_ = "odom"
std::string output_twist_frame_id_ = "odom"
as2::tf::TfHandler *tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr ref_pose_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr ref_twist_sub_
rclcpp::Subscription<as2_msgs::msg::TrajectorySetpoints>::SharedPtr ref_traj_sub_
rclcpp::Subscription<as2_msgs::msg::Thrust>::SharedPtr ref_thrust_sub_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_sub_
rclcpp::Publisher<as2_msgs::msg::TrajectorySetpoints>::SharedPtr trajectory_pub_
rclcpp::Publisher<as2_msgs::msg::Thrust>::SharedPtr thrust_pub_
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr debug_state_pose_pub_
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr debug_state_twist_pub_
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr debug_reference_pose_pub_
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr debug_reference_twist_pub_
rclcpp::Publisher<as2_msgs::msg::TrajectorySetpoints>::SharedPtr debug_reference_trajectory_pub_
rclcpp::Publisher<as2_msgs::msg::Thrust>::SharedPtr debug_reference_thrust_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr debug_compute_output_time_pub_
rclcpp::Service<as2_msgs::srv::SetControlMode>::SharedPtr set_control_mode_srv_
as2::SynchronousServiceClient<as2_msgs::srv::SetControlMode>::SharedPtr set_control_mode_client_
as2::SynchronousServiceClient<as2_msgs::srv::ListControlModes>::SharedPtr list_control_modes_client_
rclcpp::TimerBase::SharedPtr control_timer_
bool control_mode_established_ = false
bool ref_pose_acquired_ = false
bool ref_twist_acquired_ = false
bool ref_traj_acquired_ = false
bool ref_thrust_acquired_ = false
bool state_acquired_ = false
bool use_bypass_ = false
bool bypass_controller_ = false
uint8_t preferred_output_mode_ = 0b00000000
rclcpp::Time last_time_
as2_msgs::msg::PlatformInfo platform_info_
as2_msgs::msg::ControlMode control_mode_in_
as2_msgs::msg::ControlMode control_mode_out_
geometry_msgs::msg::PoseStamped state_pose_
geometry_msgs::msg::TwistStamped state_twist_
geometry_msgs::msg::PoseStamped ref_pose_
geometry_msgs::msg::TwistStamped ref_twist_
as2_msgs::msg::TrajectorySetpoints ref_traj_
as2_msgs::msg::Thrust ref_thrust_
geometry_msgs::msg::PoseStamped command_pose_
geometry_msgs::msg::TwistStamped command_twist_
as2_msgs::msg::Thrust command_thrust_
std::shared_ptr<as2_motion_controller_plugin_base::ControllerBase> controller_ptr_
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_
class ControllerManager : public as2::Node
#include <controller_manager.hpp>

ROS 2 node that loads a controller plugin and runs the control loop.

Loads the plugin selected by the plugin_name parameter through pluginlib, owns the shared TfHandler, hosts the ControllerHandler that orchestrates the control cycle, and publishes the active control modes on controller/info.

Public Functions

explicit ControllerManager(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct the manager and load the configured controller plugin.

Parameters:

options – ROS 2 node options forwarded to the as2::Node base.

~ControllerManager()

Destroy the manager, releasing the plugin loader and the handler.

Public Members

double cmd_freq_

Control loop frequency, in Hz.

Private Functions

void configAvailableControlModes(const std::filesystem::path project_path)

Read the available control modes file and configure the handler.

Parameters:

project_path – Path to the YAML file describing the modes the plugin can handle.

void modeTimerCallback()

Periodic callback that publishes the active control modes on controller/info.

Private Members

double info_freq_
std::string plugin_name_
std::filesystem::path available_modes_config_file_
as2::tf::TfHandler tf_handler_
std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<as2_motion_controller_plugin_base::ControllerBase>> loader_
std::shared_ptr<controller_handler::ControllerHandler> controller_handler_
std::shared_ptr<as2_motion_controller_plugin_base::ControllerBase> controller_
rclcpp::Publisher<as2_msgs::msg::ControllerInfo>::SharedPtr mode_pub_
rclcpp::TimerBase::SharedPtr mode_timer_

Private Static Functions

static rclcpp::NodeOptions get_modified_options(const rclcpp::NodeOptions &options)

Modify the node options to allow undeclared parameters.

Parameters:

options – Original NodeOptions.

Returns:

NodeOptions with undeclared parameters allowed.

struct ControlModeRequest
#include <mock_platform.hpp>

Configuration of the synthetic set_control_mode request fired by the mock.

Public Members

uint8_t control_mode = as2_msgs::msg::ControlMode::TRAJECTORY
uint8_t yaw_mode = as2_msgs::msg::ControlMode::YAW_ANGLE
uint8_t reference_frame = as2_msgs::msg::ControlMode::LOCAL_ENU_FRAME
struct dataCell

Public Members

float dist
char voronoi
char queueing
int obstX
int obstY
bool needsRaise
int sqdist
class DetectArucoMarkersBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::DetectArucoMarkers>

Public Functions

DetectArucoMarkersBehavior()

Construct a new Aruco Detector object.

inline ~DetectArucoMarkersBehavior()

Destroy the Aruco Detector object.

void imageCallback(const sensor_msgs::msg::Image::SharedPtr img)
void camerainfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info)

Private Functions

void loadParameters()
void setup()
void setCameraParameters(const sensor_msgs::msg::CameraInfo &_camera_info)
bool checkIdIsTarget(const int _id)
bool on_activate(std::shared_ptr<const as2_msgs::action::DetectArucoMarkers::Goal> goal) override

As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::DetectArucoMarkers::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::DetectArucoMarkers::Goal> &goal, std::shared_ptr<as2_msgs::action::DetectArucoMarkers::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::DetectArucoMarkers::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_image_sub_
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_
rclcpp::Publisher<as2_msgs::msg::PoseStampedWithIDArray>::SharedPtr aruco_pose_pub_
std::shared_ptr<as2::sensors::Camera> aruco_img_transport_
std::vector<uint16_t> target_ids_
float aruco_size_
std::string camera_model_
std::string distorsion_model_
bool camera_qos_reliable_
bool camera_params_available_
cv::Mat camera_matrix_
cv::Mat dist_coeffs_
cv::Ptr<cv::aruco::Dictionary> aruco_dict_
std::string img_encoding_
std::string camera_image_topic_ = "camera/image_raw"
std::string camera_info_topic_ = "camera/camera_info"
class DisarmService : public as2_behavior_tree::BtServiceNode<std_srvs::srv::SetBool>

Public Functions

DisarmService(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion(std::shared_ptr<std_srvs::srv::SetBool::Response> response)
class DroneSwarm

Public Functions

DroneSwarm(as2::Node *node_ptr, std::string drone_id, geometry_msgs::msg::Pose init_pose, rclcpp::CallbackGroup::SharedPtr cbk_group)
inline ~DroneSwarm()
std::shared_ptr<rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowReference>> initFollowReference()

Initialize the follow refrence.

Returns:

std::shared_ptr<rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowReference>> Future of the FollowReference behavior

bool stopFollowReference()

Send request to stop following the refernce in the swarm.

Returns:

bool Response of the service

bool checkPosition()

Check if the drones are in their reference position.

Returns:

bool Return true if the drones are less than 0.3 meters from the reference

bool updateStaticTf(geometry_msgs::msg::Pose pose)

Update the static tf of the drones.

Parameters:

pose – The new reference pose

Public Members

std::string drone_id_
geometry_msgs::msg::Pose init_pose_
geometry_msgs::msg::PoseStamped drone_pose_
geometry_msgs::msg::TransformStamped transform_

Private Functions

void dronePoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr _pose_msg)

Callback to update the current pose of the drone.

Parameters:

_pose_msg – The curent pose of the drone

void follow_reference_feedback_cbk(rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowReference>::SharedPtr goal_handle, const std::shared_ptr<const as2_msgs::action::FollowReference::Feedback> feedback)

Callback to update the feedback of the follow reference action.

Parameters:

  • goal_handle – The goal handle of the follow reference action

  • feedback – The feedback of the follow reference action

Private Members

as2::Node *node_ptr_
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfstatic_broadcaster_
std::string base_link_frame_id_
std::string parent_frame_id
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr drone_pose_sub_
rclcpp_action::Client<as2_msgs::action::FollowReference>::SharedPtr follow_reference_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr follow_reference_stop_client_ = nullptr
rclcpp::CallbackGroup::SharedPtr cbk_group_
std::shared_ptr<const as2_msgs::action::FollowReference::Feedback> follow_reference_feedback_
float max_speed_
class DynamicVoronoi
#include <dynamicvoronoi.h>

A DynamicVoronoi object computes and updates a distance map and Voronoi diagram.

Public Functions

DynamicVoronoi()
~DynamicVoronoi()
void initializeEmpty(int _sizeX, int _sizeY, bool initGridMap = true)

Initialization with an empty map.

void initializeMap(int _sizeX, int _sizeY, bool **_gridMap)

Initialization with a given binary map (false==free, true==occupied)

void occupyCell(int x, int y)

add an obstacle at the specified cell coordinate

void clearCell(int x, int y)

remove an obstacle at the specified cell coordinate

void exchangeObstacles(std::vector<INTPOINT> &newObstacles)

remove old dynamic obstacles and add the new ones

void update(bool updateRealDist = true)

update distance map and Voronoi diagram to reflect the changes

void prune()

prune the Voronoi diagram

void updateAlternativePrunedDiagram()

prune the Voronoi diagram by globally revisiting all Voronoi nodes. Takes more time but gives a more sparsely pruned Voronoi graph. You need to call this after every call to udpate()

inline int **alternativePrunedDiagram()

retrieve the alternatively pruned diagram. see updateAlternativePrunedDiagram()

int getNumVoronoiNeighborsAlternative(int x, int y)

retrieve the number of neighbors that are Voronoi nodes (4-connected)

bool isVoronoiAlternative(int x, int y)

returns whether the specified cell is part of the alternatively pruned diagram. See updateAlternativePrunedDiagram.

float getDistance(int x, int y)

returns the obstacle distance at the specified location

bool isVoronoi(int x, int y)

returns whether the specified cell is part of the (pruned) Voronoi graph

bool isOccupied(int x, int y)

checks whether the specficied location is occupied

void visualize(const char *filename = "result.ppm")

write the current distance map and voronoi diagram as ppm file

inline unsigned int getSizeX()

returns the horizontal size of the workspace/map

inline unsigned int getSizeY()

returns the vertical size of the workspace/map

Private Types

enum State

Values:

enumerator voronoiKeep
enumerator freeQueued
enumerator voronoiRetry
enumerator voronoiPrune
enumerator free
enumerator occupied
enum QueueingState

Values:

enumerator fwNotQueued
enumerator fwQueued
enumerator fwProcessed
enumerator bwQueued
enumerator bwProcessed
enum ObstDataState

Values:

enumerator invalidObstData
enum markerMatchResult

Values:

enumerator pruned
enumerator keep
enumerator retry

Private Functions

void setObstacle(int x, int y)
void removeObstacle(int x, int y)
inline void checkVoro(int x, int y, int nx, int ny, dataCell &c, dataCell &nc)
void recheckVoro()
void commitAndColorize(bool updateRealDist = true)
inline void reviveVoroNeighbors(int &x, int &y)
inline bool isOccupied(int &x, int &y, dataCell &c)
inline markerMatchResult markerMatch(int x, int y)
inline bool markerMatchAlternative(int x, int y)
inline int getVoronoiPruneValence(int x, int y)

Private Members

BucketPrioQueue<INTPOINT> open
std::queue<INTPOINT> pruneQueue
BucketPrioQueue<INTPOINT> sortedPruneQueue
std::vector<INTPOINT> removeList
std::vector<INTPOINT> addList
std::vector<INTPOINT> lastObstacles
int sizeY
int sizeX
dataCell **data
bool **gridMap
bool allocatedGridMap
int padding
double doubleThreshold
double sqrt2
int **alternativeDiagram
class Echo : public BT::SyncActionNode

Public Functions

Echo(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
BT::NodeStatus tick() override

Public Static Functions

static inline BT::PortsList providedPorts()

Private Members

rclcpp::Node::SharedPtr node_
struct EigenTwist
#include <Common.hpp>

Struct containing linear and angular velocities.

Public Members

Eigen::Vector3d linear
Eigen::Vector3d angular
struct ExpandedPath
#include <gcopter_trajectory_generator.hpp>

Path with waypoint anchors and per-segment corridor margins.

Used internally to insert anchor points around each user waypoint to tighten the AABB Safe Flight Corridor near them, while keeping a loose margin between anchors. original_indices stores the position of each input waypoint inside positions so the host can map mission ids back to backend breakpoints.

Public Members

std::vector<Eigen::Vector3d> positions
std::vector<double> margins
std::vector<std::size_t> original_indices
struct follow_path_plugin_params

Public Members

double follow_path_speed = 0.0
double follow_path_threshold = 0.0
struct follow_reference_plugin_params

Public Members

double follow_reference_max_speed_x = 0.0
double follow_reference_max_speed_y = 0.0
double follow_reference_max_speed_z = 0.0
class FollowPathAction : public as2_behavior_tree::BtActionNode<as2_msgs::action::FollowPath>

Public Functions

inline FollowPathAction(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
inline virtual void on_tick()

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

inline void on_wait_for_result(std::shared_ptr<const as2_msgs::action::FollowPath::Feedback> feedback)

Public Static Functions

static inline BT::PortsList providedPorts()

Private Members

std::vector<as2_msgs::msg::PoseWithID> path_
double max_speed_
int yaw_mode_
class FollowPathBase

Subclassed by follow_path_plugin_position::Plugin, follow_path_plugin_trajectory::Plugin

Public Types

using GoalHandleFollowPath = rclcpp_action::ServerGoalHandle<as2_msgs::action::FollowPath>

Public Functions

inline FollowPathBase()
inline virtual ~FollowPathBase()
inline void initialize(as2::Node *node_ptr, std::shared_ptr<as2::tf::TfHandler> tf_handler, follow_path_plugin_params &params)
inline virtual void state_callback(geometry_msgs::msg::PoseStamped &pose_msg, geometry_msgs::msg::TwistStamped &twist_msg)
inline void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline virtual as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal, std::shared_ptr<as2_msgs::action::FollowPath::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::FollowPath::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::FollowPath::Goal &goal) = 0
inline virtual bool own_modify(as2_msgs::action::FollowPath::Goal &goal)
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
inline virtual bool own_pause(const std::shared_ptr<std::string> &message)
inline virtual bool own_resume(const std::shared_ptr<std::string> &message)
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0
virtual Eigen::Vector3d getTargetPosition() = 0
inline void sendHover()
inline float getActualYaw()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler = nullptr
as2_msgs::action::FollowPath::Goal goal_
as2_msgs::action::FollowPath::Feedback feedback_
as2_msgs::action::FollowPath::Result result_
int platform_state_
follow_path_plugin_params params_
geometry_msgs::msg::PoseStamped actual_pose_
bool localization_flag_ = false

Private Functions

inline bool processGoal(as2_msgs::action::FollowPath::Goal &_goal)
inline void reset()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr
bool goal_accepted_ = false
class FollowPathBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::FollowPath>

Public Types

using GoalHandleFollowPath = rclcpp_action::ServerGoalHandle<as2_msgs::action::FollowPath>

Public Functions

explicit FollowPathBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~FollowPathBehavior()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
bool process_goal(std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal, as2_msgs::action::FollowPath::Goal &new_goal)
bool on_activate(std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::FollowPath::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::FollowPath::Goal> &goal, std::shared_ptr<as2_msgs::action::FollowPath::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::FollowPath::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<follow_path_base::FollowPathBase>> loader_
std::shared_ptr<follow_path_base::FollowPathBase> follow_path_plugin_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_sub_
class FollowReferenceBase

Subclassed by follow_reference_plugin_position::Plugin, follow_reference_plugin_trajectory::Plugin

Public Types

using GoalHandleFollowReference = rclcpp_action::ServerGoalHandle<as2_msgs::action::FollowReference>

Public Functions

inline FollowReferenceBase()
inline virtual ~FollowReferenceBase()
inline void initialize(as2::Node *node_ptr, std::shared_ptr<as2::tf::TfHandler> tf_handler)
inline void state_callback(geometry_msgs::msg::PoseStamped &pose_msg, geometry_msgs::msg::TwistStamped &twist_msg)
inline void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal, std::shared_ptr<as2_msgs::action::FollowReference::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::FollowReference::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::FollowReference::Goal &goal) = 0
inline virtual bool own_modify(as2_msgs::action::FollowReference::Goal &goal)
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
inline virtual bool own_pause(const std::shared_ptr<std::string> &message)
inline virtual bool own_resume(const std::shared_ptr<std::string> &message)
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0
inline void sendHover()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler_ = nullptr
as2_msgs::action::FollowReference::Goal goal_
as2_msgs::action::FollowReference::Feedback feedback_
as2_msgs::action::FollowReference::Result result_
int platform_state_
follow_reference_plugin_params params_
geometry_msgs::msg::PoseStamped actual_pose_
bool localization_flag_

Private Functions

inline double declareAndGetDouble(const std::string &param_name)
inline bool processGoal(as2_msgs::action::FollowReference::Goal &_goal)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr
class FollowReferenceBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::FollowReference>

Public Types

using GoalHandleFollowReference = rclcpp_action::ServerGoalHandle<as2_msgs::action::FollowReference>

Public Functions

explicit FollowReferenceBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~FollowReferenceBehavior()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
bool process_goal(std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal, as2_msgs::action::FollowReference::Goal &new_goal)
bool on_activate(std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::FollowReference::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::FollowReference::Goal> &goal, std::shared_ptr<as2_msgs::action::FollowReference::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::FollowReference::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<follow_reference_base::FollowReferenceBase>> loader_
std::shared_ptr<follow_reference_base::FollowReferenceBase> follow_reference_plugin_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_sub_
class ForceEstimation

Public Functions

ForceEstimation(double alpha, size_t n_samples)
inline ~ForceEstimation()
double computeThrustError(const double &current_mass, const double a_z_mean, const double &u_thrust)

Compute thrust from IMU measurements.

Parameters:

  • current_mass – Current mass

  • a_z_mean – Mean of the last n samples of the z-acceleration

  • u_thrust – Current thrust command

Returns:

Thrust error estimation in Newtons

double computedMeanFromVector(std::vector<double> &vec)

Compute mean from a vector of doubles.

Parameters:

vec – Vector of doubles

Returns:

Mean value

double computedMeanFromNSamples(const std::vector<double> &vec)

Compute mean from the last n samples of a vector of doubles.

Parameters:

vec – Vector of doubles

Returns:

Mean value

double lowPassFiltered(double &error, double &last_filtered_error)

Low-pass filter.

Parameters:

  • error – Input thrust value

  • last_filtered_error – Last filtered error value

Returns:

Filtered thrust value

Private Members

double alpha_
size_t n_samples_
class ForceEstimationBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::ForceEstimation>

Public Functions

explicit ForceEstimationBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~ForceEstimationBehavior()

Private Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::ForceEstimation::Goal> goal) override

As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::ForceEstimation::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::ForceEstimation::Goal> &goal, std::shared_ptr<as2_msgs::action::ForceEstimation::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::ForceEstimation::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_msg)
void commandedThrustCallback(const as2_msgs::msg::Thrust::SharedPtr thrust_msg)
void filterForceError()
void updateForceParameter()

Private Members

std::shared_ptr<ForceEstimation> force_estimation_lib
double force_error_
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_
rclcpp::Subscription<as2_msgs::msg::Thrust>::SharedPtr commanded_thrust_sub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr force_error_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr force_filtered_error_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr force_limited_error_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr force_update_error_pub_
as2::SynchronousServiceClient<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_
as2::SynchronousServiceClient<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_
rcl_interfaces::msg::Parameter param_
std::string controler_node_
std::string force_param_name_
std::string mass_param_name_
double alpha_
size_t n_samples_
double threshold_time_sync
double fz_filtered_error_
double fz_update_error
double minimum_error_
double maximum_error_
double mass_
double published_force_error_
double last_filtered_error_
double default_mass
std::vector<double> measured_az_stack_
rclcpp::Time imu_time_
std::vector<double> estimated_thrust_error_vector_
double thrust_comanded_msg_
rclcpp::Time thrust_time_
rclcpp::Duration threshold_time_sync_ = {0, 0}
rclcpp::TimerBase::SharedPtr filter_force_error_timer_
rclcpp::Time last_force_error_update_time_
std::string force_error_topic_
std::string force_filtered_error_topic_
std::string force_limited_error_topic_
std::string force_update_error_topic_
bool first_thrust_
struct FrameData
#include <Common.hpp>

Frame data of a link including its pose and linear velocity in world frame as well as its angular velocity in body frame.

Public Functions

inline explicit FrameData(const Eigen::Vector3d &ang_vel_in)
inline explicit FrameData(const Eigen::Vector3d &ang_vel_in)

Public Members

Eigen::Vector3d angularVelocityBody
class GazeboPlatform : public as2::AerialPlatform

Public Functions

explicit GazeboPlatform(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~GazeboPlatform()
inline virtual void configureSensors()

Configures the platform sensors.

virtual bool ownSendCommand() override

Handles how a command must be sended in the concrete platform.

Returns:

true command is sended successfully.

Returns:

false command is not sended.

virtual bool ownSetArmingState(bool state) override

Handles how arming state has to be settled in the concrete platform.

Parameters:

state – true for arming the platform, false to disarm.

Returns:

true Arming state is settled successfully.

Returns:

false Arming state is not settled.

virtual bool ownSetOffboardControl(bool offboard) override

Handles how offboard mode has to be settled in the concrete platform.

Parameters:

offboard – true if offboard mode is enabled.

Returns:

true Offboard mode is settled successfully.

Returns:

false Offboard mode is not settled.

virtual bool ownSetPlatformControlMode(const as2_msgs::msg::ControlMode &msg) override

Handles how the control mode has to be settled in the concrete platform.

Parameters:

control_mode – as2_msgs::msg::PlatformControlMode with the new control mode.

Returns:

true Control mode is settled successfully.

Returns:

false Control mode is not settled.

virtual void ownKillSwitch() override

Handles the platform emergency kill switch command. This means stop the motors inmediately, this cannot be reversed. USE WITH CAUTION.

virtual void ownStopPlatform() override

Handles the platform emergency stop command. STOP means to hover as best as possible. This hover is different from the hover in the platform control mode. And when it is activated the platform will stop hearing commands from AS2. USE WITH CAUTION.

virtual bool ownTakeoff() override

Handles the platform takeoff command.

Returns:

true Takeoff command is sended successfully.

Returns:

false Takeoff command is not sended.

virtual bool ownLand() override

Handles the platform landing command.

Returns:

true Landing command is sended successfully.

Returns:

false Landing command is not sended.

Public Members

rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr arm_pub_
rclcpp::Publisher<as2_msgs::msg::Acro>::SharedPtr acro_pub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_state_sub_
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_srv_

Private Functions

void resetCommandTwistMsg()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
void reset_callback(const std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response)

Private Members

as2_msgs::msg::ControlMode control_in_
double yaw_rate_limit_ = M_PI_2
bool enable_takeoff_ = false
bool enable_land_ = false
bool state_received_ = false
double current_height_ = 0.0
double current_vertical_speed_ = 0.0
std::shared_ptr<as2::tf::TfHandler> tf_handler_
class GeneratePolynomialTrajectoryBase
#include <generate_polynomial_trajectory_base.hpp>

Abstract plugin contract for polynomial trajectory generation.

Public Functions

GeneratePolynomialTrajectoryBase() = default

Construct plugin base instance.

virtual ~GeneratePolynomialTrajectoryBase() = default

Virtual destructor.

void initialize(as2::Node *node, const std::string &plugin_name)

Initialize the plugin with the hosting node and its namespace.

Plugins may use the node only for configuration and logging.

Parameters:

  • node – Hosting node pointer.

  • plugin_name – Plugin namespace identifier.

virtual bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) = 0

Generate a fresh trajectory from mission waypoints.

The initial vehicle state (position and velocity) is NOT passed as an argument: the plugin reads it from the protected base members vehicle_pose_ / vehicle_twist_ (refreshed by the host via setVehicleState before this call). Plugins must inject vehicle_pose_ as the trajectory start position and may use vehicle_twist_ as the starting velocity boundary condition when their backend supports it. The buildCurrentWaypoint() helper provides a ready-to-use entry for backends that expect the initial state as the first waypoint.

The host owns a single logical trajectory time axis exposed as t_trajectory (seconds, monotonic, reset to 0 on every fresh generation). Plugins are responsible for mapping t_trajectory to whatever internal time axis their backend uses; the host never observes that mapping. On entry, t_trajectory_now indicates the current value of the host’s axis, so the plugin can anchor its internal offset such that t_backend == 0 when t_trajectory == t_trajectory_now (or whichever convention the backend prefers).

Parameters:

  • waypoints – Ordered mission waypoints (the user-specified path only, no synthetic “current” entry).

  • max_speed – Maximum cruise speed in m/s.

  • t_trajectory_now – Current value of the host’s trajectory time axis at the moment of generation (typically 0.0 for a fresh activation, or the live trajectory_time_ for a regeneration triggered by updateWaypoints()).

Returns:

true when trajectory generation succeeds.

virtual bool evaluate(double t_trajectory, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample = false) = 0

Evaluate trajectory references at time t_trajectory.

The vehicle pose and twist are not passed in this call: they are kept as protected members (vehicle_pose_, vehicle_twist_) refreshed by the host via setVehicleState(). Implementations that need closed-loop feedback (e.g. dynamic re-planners) read them directly and must gate any state-mutating use on is_horizon_sample being false, since is_horizon_sample is true for both horizon predictions and debug evaluations.

The plugin maps t_trajectory to its internal backend axis using its private offset and clamps to its own valid range; values outside the trajectory horizon are saturated to the closest endpoint rather than returning false.

Parameters:

  • t_trajectory – Evaluation time in seconds, in the host’s trajectory time axis.

  • out – Output reference point.

  • is_horizon_sample – True for horizon predictions and debug sampling; false for the live control setpoint.

Returns:

true when evaluation succeeds.

virtual bool isFinished(double t_trajectory) const = 0

Whether the trajectory is exhausted at the given t_trajectory.

Returns true when t_trajectory has reached or exceeded the end of the currently held trajectory in the host’s time axis.

Parameters:

t_trajectory – Current host trajectory time in seconds.

Returns:

true when the trajectory has finished.

virtual double getDuration() const = 0

Total duration of the currently held trajectory.

Returns the temporal length of the trajectory in seconds (i.e. backend max - backend min in the plugin’s internal axis). Used by the host for diagnostics and feedback only; it does NOT expose the backend axis offset to the wrapper.

Returns:

Duration in seconds, or 0.0 when no trajectory is held.

virtual bool isTrajectoryGenerated() = 0

Check whether a trajectory is currently generated.

Returns:

true when a valid trajectory is available.

virtual void reset() = 0

Reset internal plugin trajectory state.

inline virtual bool updateWaypoints(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now)

Update the active trajectory with a new pending waypoint list.

Default implementation: regenerate from scratch by calling reset() followed by generateTrajectory() with the current vehicle state. Plugins that support smooth online re-planning (i.e. preserving the trajectory time origin so evaluation continues without restart) override this method and keep their internal time-axis offset unchanged so the host’s t_trajectory keeps mapping to the same backend trajectory.

Parameters:

  • waypoints – Ordered pending waypoints (no helper “current” entry).

  • max_speed – Maximum cruise speed in m/s.

  • t_trajectory_now – Current value of the host’s trajectory time axis. Plugins that regenerate must re-anchor their offset to this value; plugins that stitch must keep their offset untouched.

Returns:

true when the update succeeds.

inline virtual bool consumeRegeneratedFlag()

Consume and return whether the underlying backend trajectory has been regenerated since the previous call.

Plugins with a synchronous updateWaypoints() (the base default) always return false: their swap is observable immediately on return from updateWaypoints() and the host can rely on the publish that happens right after in pushPendingToPlugin().

Plugins that maintain an asynchronous regeneration pipeline override this to expose the deferred swap event so the host can refresh debug publications once the backend has actually swapped its internal trajectory.

Returns:

true at most once per regeneration event.

virtual std::string getNextWaypointId() = 0

Get the id of the next pending waypoint as known by the plugin.

Returns:

Empty string when there is no pending waypoint, else its id.

void setVehicleState(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

Refresh the vehicle pose and twist held by the plugin.

The host is expected to call this once per incoming state message (in its odom/twist subscription callback). The plugin therefore always holds the latest valid pose and twist; plugin entry points (generateTrajectory, updateWaypoints, evaluate) do not need a per-call refresh from the host. Both fields are stored as-is; coordinate frame consistency with the trajectory frame is the host’s responsibility.

Parameters:

  • pose – Current vehicle pose in the trajectory frame.

  • twist – Current vehicle twist (linear in body or world per the host configuration; not interpreted by the base class).

inline const geometry_msgs::msg::PoseStamped &getVehiclePose() const

Read-only access to the latest vehicle pose set by the host.

The host refreshes the underlying member through setVehicleState() once per state message; the value seen here is the latest value successfully delivered by the host.

Returns:

Const reference to the cached vehicle pose.

inline const geometry_msgs::msg::TwistStamped &getVehicleTwist() const

Read-only access to the latest vehicle twist set by the host.

Returns:

Const reference to the cached vehicle twist.

Protected Functions

inline virtual void ownInitialize()

Optional plugin-specific initialization hook.

as2_msgs::msg::PoseStampedWithID buildCurrentWaypoint() const

Build a synthetic waypoint capturing the current vehicle pose.

Returns a PoseStampedWithID with id=”current” and pose=vehicle_pose_, intended to be prepended to the waypoint list before feeding the backend whenever the backend expects the start state as the first waypoint. The accompanying initial velocity must be read separately from vehicle_twist_ — PoseStampedWithID does not carry twist.

Returns:

Synthetic “current” waypoint built from vehicle_pose_.

template<typename T>
inline void getParameter(const std::string &param_name, T &param_value, bool use_default = false)

Read a plugin-scoped parameter.

Template Parameters:

T – Parameter type.

Parameters:

  • param_name – Parameter name.

  • param_value – [in] default value when use_default is true, [out] read value.

  • use_default – Whether to declare with default value.

inline as2::Node *getNodePtr()

Get mutable hosting node pointer.

Returns:

Mutable hosting node pointer.

inline const as2::Node *getNodePtr() const

Get const hosting node pointer.

Returns:

Const hosting node pointer.

inline const std::string &getPluginName() const

Get plugin namespace name.

Returns:

Plugin namespace string.

Protected Attributes

geometry_msgs::msg::PoseStamped vehicle_pose_
geometry_msgs::msg::TwistStamped vehicle_twist_
as2::Node *node_ptr_ = {nullptr}

Private Functions

std::string qualifyParameterName(const std::string &param_name) const

Qualify parameter name with plugin namespace.

Parameters:

param_name – Parameter key.

Returns:

Namespaced parameter key.

Private Members

std::string plugin_name_
class GeneratePolynomialTrajectoryBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::GeneratePolynomialTrajectory>
#include <generate_polynomial_trajectory_behavior.hpp>

ROS2 BehaviorServer that wraps polynomial trajectory generator plugins. It receives a path of waypoints and other parameters as a goal, and then uses the specified plugin to generate a trajectory.

Public Types

using Action = as2_msgs::action::GeneratePolynomialTrajectory
using PluginBase = generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase

Public Functions

explicit GeneratePolynomialTrajectoryBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct behavior server node.

Parameters:

options – ROS node options.

~GeneratePolynomialTrajectoryBehavior() override = default

Virtual destructor.

Private Functions

bool on_activate(std::shared_ptr<const Action::Goal> goal) override

Activate behavior with a new goal.

Parameters:

goal – Requested behavior goal.

Returns:

true when goal activation succeeds.

bool on_modify(std::shared_ptr<const Action::Goal> goal) override

Modify currently active goal.

Parameters:

goal – New goal request.

Returns:

true when goal modification succeeds.

virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override

Deactivate behavior.

Parameters:

message – Optional deactivation message.

Returns:

true when deactivation succeeds.

virtual bool on_pause(const std::shared_ptr<std::string> &message) override

Pause behavior and hand over to hover.

Parameters:

message – Optional pause message.

Returns:

true when pause handling succeeds.

virtual bool on_resume(const std::shared_ptr<std::string> &message) override

Resume behavior from stored progress.

Parameters:

message – Optional resume message.

Returns:

true when resume activation succeeds.

as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const Action::Goal> &goal, std::shared_ptr<Action::Feedback> &feedback_msg, std::shared_ptr<Action::Result> &result_msg) override

Execute behavior run step.

Parameters:

  • goal – Active goal.

  • feedback_msg – Output feedback message.

  • result_msg – Output result message.

Returns:

Current execution status.

virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Finalize execution and cleanup runtime state.

Parameters:

state – Final behavior execution state.

template<typename T>
inline void getParameter(const std::string &param_name, T &param_value, bool use_default = false)

Read parameter through shared helper.

Template Parameters:

T – Parameter type.

Parameters:

  • param_name – Parameter name.

  • param_value – [in] default value when use_default is true, [out] read value.

  • use_default – Whether to use param_value as default.

void loadPlugin()

Load trajectory generation plugin.

bool buildWaypoints(const std::shared_ptr<const Action::Goal> &goal, std::vector<as2_msgs::msg::PoseStampedWithID> &out)

Build internal waypoint list from goal.

The output contains only the user-specified mission waypoints converted to desired_frame_id_ with normalized ids. The synthetic “current” entry is no longer prepended here: each plugin reads the vehicle state from the protected base members and injects it into its backend if needed.

Parameters:

  • goal – Input goal.

  • out – Output waypoint vector.

Returns:

true when waypoint list is valid and converted.

bool validateWaypoints(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints) const

Validate waypoint constraints.

Parameters:

waypoints – Waypoint list to validate.

Returns:

true when waypoint list is valid.

bool convertWaypointsToDesiredFrame(const std::vector<as2_msgs::msg::PoseStampedWithID> &in, std::vector<as2_msgs::msg::PoseStampedWithID> &out, const char *log_context)

Convert waypoint poses to desired_frame_id_ and normalize ids.

Empty ids are auto-generated as waypoint_XXX using the waypoint index.

Parameters:

  • in – Input waypoint list.

  • out – Output converted waypoint list.

  • log_context – Context string for error logs.

Returns:

true when all waypoints are converted.

void logGoalWaypoints(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, const char *log_context) const

Log goal waypoints (id, position, yaw) in their input frame.

Emitted before any TF conversion, so the trace reflects exactly what the client sent in goal->path. Yaw is extracted from each pose’s orientation (radians).

Parameters:

  • waypoints – Goal waypoint list (as received).

  • log_context – Tag prepended to each line (e.g. “on_activate”).

bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed)

Generate a fresh trajectory through the plugin and reset time bases.

Refreshes the plugin vehicle state (pose + twist) before delegating to plugin_->generateTrajectory(). The plugin reads the live state from the protected base members and is responsible for injecting it into its backend, so the waypoint list passed here contains only the user-specified mission waypoints (no synthetic “current” entry).

Parameters:

  • waypoints – Mission waypoints in desired_frame_id_.

  • max_speed – Maximum speed in m/s.

Returns:

true when generation succeeds.

std::vector<as2_msgs::msg::PoseStampedWithID> getPendingWaypoints(const std::string &next_id) const

Get pending waypoints from goal_.path starting at next_id.

Parameters:

next_id – Id of the next pending waypoint.

Returns:

Subset of goal_.path from next_id (inclusive) to the end. If next_id is empty or not found, returns the full path.

uint16_t remainingWaypointCount(const std::string &next_id) const

Count remaining waypoints in goal_.path from next_id onwards.

Parameters:

next_id – Id of the next pending waypoint.

Returns:

Number of waypoints from next_id (inclusive) to end of path.

std::vector<as2_msgs::msg::PoseStampedWithID> mergeModifyIntoGoal(const std::vector<as2_msgs::msg::PoseStampedWithID> &modify_waypoints, const std::string &next_id)

Apply a modify request onto goal_.path.

Drops already-passed waypoints (those before next_id), updates the pose for any matching id, and appends new ids at the end. Waypoints not referenced in the modify list are kept with their previous pose.

Parameters:

  • modify_waypoints – Waypoints from the modify goal, already in desired_frame_id_.

  • next_id – Id of the next pending waypoint at modify time.

Returns:

The resulting pending waypoint list (= new goal_.path).

void publishGenerationDebug()

Publish trajectory and waypoints debug topics.

bool pushPendingToPlugin()

Push the current pending queue to the plugin via updateWaypoints.

Refreshes the plugin vehicle state and calls plugin_->updateWaypoints(…, trajectory_time_) with the pending list derived from goal_.path and the plugin’s getNextWaypointId(). The plugin decides whether to stitch smoothly (preserving its internal offset) or regenerate from scratch (re-anchoring its offset against the current trajectory_time_). The wrapper does NOT realign anything: trajectory_time_ keeps progressing monotonically. Triggers publishGenerationDebug() on success.

Returns:

true when the plugin accepts the update.

bool evaluatePoint(double t, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample)

Evaluate one trajectory setpoint.

Refreshes the plugin vehicle state (pose + twist) before delegating to the plugin so closed-loop backends can read it via the protected members.

Parameters:

  • t – Evaluation time in seconds.

  • out – Output trajectory point.

  • is_horizon_sample – True for horizon predictions and debug sampling; false for the live control setpoint.

Returns:

true when evaluation succeeds.

bool evaluateHorizon(double t0, double dt, std::size_t n, as2_msgs::msg::TrajectorySetpoints &out)

Evaluate a horizon of setpoints.

Parameters:

  • t0 – Initial evaluation time in seconds.

  • dt – Sampling period in seconds.

  • n – Number of samples.

  • out – Output setpoint array.

Returns:

true when all samples are evaluated.

double computeYaw(const as2_msgs::msg::TrajectoryPoint &point, bool is_horizon_sample)

Compute yaw command for a trajectory point.

Parameters:

  • point – Current trajectory point.

  • is_horizon_sample – If is first sample of the horizon

Returns:

Yaw angle in radians.

double computeYawAnglePathFacing(double vx, double vy) const

Compute yaw aligned to XY velocity vector.

Parameters:

  • vx – X velocity in m/s.

  • vy – Y velocity in m/s.

Returns:

Yaw angle in radians.

double computeYawFaceReference(double current_yaw)

Compute yaw facing the next reference waypoint, rate-limited.

Parameters:

current_yaw – Yaw of the previous sample

Returns:

Yaw angle in radians.

const as2_msgs::msg::PoseStampedWithID *getNextReferenceWaypoint() const

Get next reference waypoint based on plugin progress.

Returns:

Pointer to waypoint within goal_.path, or nullptr when none.

void resetRuntimeState()

Reset runtime state variables.

bool isDegenerateTarget(const as2_msgs::msg::PoseStampedWithID &last_wp) const

Test whether the active goal’s last waypoint is closer than kDegenerateDistanceM to the current vehicle pose.

Used to short-circuit the plugin and either terminate with SUCCESS (when follow_reference_mode is disabled) or enter a static hold (when follow_reference_mode is enabled).

Parameters:

last_wp – Reference waypoint (typically active_waypoints_.back()).

Returns:

true when the distance is below kDegenerateDistanceM.

bool tryEnterDegenerateHold(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints)

Try to latch the host into degenerate-hold for waypoints.

Convenience wrapper around isDegenerateTarget + enterDegenerateHold, meant to be called BEFORE any plugin invocation (generateTrajectory / updateWaypoints).

The hold is only engaged when the active goal contains a SINGLE waypoint (the only case where a near-target trajectory is degenerate by itself — a multi-waypoint mission still has intermediate motion even if its last waypoint sits close to the vehicle).

Parameters:

waypoints – Active waypoint list (typically active_waypoints_).

Returns:

true when the hold has been engaged (the plugin must not be called); false when the size is not 1 or the target is outside the threshold.

void enterDegenerateHold(const as2_msgs::msg::PoseStampedWithID &last_wp)

Latch the host into degenerate-hold and emit a WARN log.

Stores last_wp.pose.pose.position and last_wp.id so subsequent on_run ticks can publish a static reference even if the active window changes (e.g. modify_waypoint replacing the entry).

Parameters:

last_wp – Reference waypoint whose pose is held.

as2_behavior::ExecutionStatus runDegenerateHold(std::shared_ptr<Action::Feedback> &feedback_msg, std::shared_ptr<Action::Result> &result_msg)

Tick the degenerate-hold branch.

  • If !follow_reference_mode_, returns SUCCESS immediately.

  • Else if the live last waypoint is again above kDegenerateDistanceM, clears the hold flag, regenerates the trajectory and returns RUNNING for the next tick (the caller falls through to normal evaluation).

  • Otherwise publishes a static horizon at degenerate_target_ and returns RUNNING.

Parameters:

  • feedback_msg – Output feedback (next_waypoint_id, remaining).

  • result_msg – Output result (success flag).

Returns:

ExecutionStatus appropriate for the current tick.

void buildHoldHorizon(const geometry_msgs::msg::Point &target, as2_msgs::msg::TrajectorySetpoints &out) const

Build a static horizon of sampling_n_ setpoints at target.

Velocity and acceleration are zero; yaw is held at init_yaw_angle_.

Parameters:

  • target – Position to hold.

  • out – Output setpoints (resized to sampling_n_).

void stateCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)

Handle vehicle state updates.

Parameters:

msg – Incoming twist message.

void yawCallback(const std_msgs::msg::Float32::SharedPtr msg)

Handle external yaw updates.

Parameters:

msg – Incoming yaw message.

void modifyWaypointCallback(const as2_msgs::msg::PoseStampedWithIDArray::SharedPtr msg)

Handle waypoint modification requests.

Parameters:

msg – Incoming waypoint updates.

void timerUpdateFrameCallback()

Periodic frame-drift watchdog callback.

bool computeFrameError()

Compute whether frame drift exceeds threshold.

Returns:

true when regeneration is required.

void initDebugPublishers()

Initialize debug publishers.

void publishGenerationTime(double seconds)

Publish trajectory generation/update compute time.

No-op when the publisher is disabled (empty topic name in config).

Parameters:

seconds – Compute time in seconds.

void splitForPathLength(std::vector<as2_msgs::msg::PoseStampedWithID> &active, std::deque<as2_msgs::msg::PoseStampedWithID> &queued) const

Split a waypoint list into the active window fed to the plugin and a queue drained incrementally by on_run.

No-op when path_length_ <= 0. The active vector is truncated in place. When the plugin’s updateWaypoints() falls back to the base default (i.e. regeneration), each window slide implies a fresh trajectory; this is functionally correct but may cause time-base realignment per slide.

Parameters:

  • active – [in/out] Full pending list. On return, contains only the first path_length_ entries.

  • queued – [in/out] Receives the truncated tail (appended in order).

void publishGeneratedTrajectory()

Publish sampled generated trajectory for visualization.

void publishWaypoints()

Publish active waypoints markers.

void publishEvaluatedPoint(const as2_msgs::msg::TrajectoryPoint &point, bool is_last_in_horizon)

Publish evaluated point marker.

Parameters:

  • point – Point to publish.

  • is_last_in_horizon – True when it is the horizon last sample.

Private Members

std::string desired_frame_id_
std::string map_frame_id_
std::string base_link_frame_id_
as2::tf::TfHandler tf_handler_
as2::motionReferenceHandlers::TrajectoryMotion trajectory_motion_handler_
as2::motionReferenceHandlers::HoverMotion hover_motion_handler_
std::shared_ptr<pluginlib::ClassLoader<PluginBase>> plugin_loader_
std::shared_ptr<PluginBase> plugin_
std::string plugin_name_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr state_sub_
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr yaw_sub_
rclcpp::Subscription<as2_msgs::msg::PoseStampedWithIDArray>::SharedPtr mod_waypoint_sub_
int sampling_n_ = {1}
double sampling_dt_ = {0.01}
double yaw_threshold_ = {0.1}
double yaw_speed_threshold_ = {0.0}
double horizon_yaw_ = {0.0}
int path_length_ = {0}
std::deque<as2_msgs::msg::PoseStampedWithID> pending_waypoints_queue_
std::vector<as2_msgs::msg::PoseStampedWithID> active_waypoints_
rclcpp::TimerBase::SharedPtr timer_update_frame_
double frequency_update_frame_ = {0.0}
double transform_threshold_ = {1.0}
geometry_msgs::msg::TransformStamped last_map_to_desired_
bool has_vehicle_state_ = {false}
Action::Goal goal_
Action::Feedback feedback_
double trajectory_time_ = {0.0}
rclcpp::Time last_tick_time_
bool first_tick_after_anchor_ = {true}
double init_yaw_angle_ = {0.0}
rclcpp::Time time_zero_yaw_
bool has_yaw_from_topic_ = {false}
float yaw_from_topic_ = {0.0f}
std::string paused_next_waypoint_id_
bool start_on_paused_ = {false}
bool has_paused_ = {false}
bool external_pause_ = {false}
bool follow_reference_mode_ = {false}
bool degenerate_hold_ = {false}
geometry_msgs::msg::Point degenerate_target_
std::string degenerate_target_id_
bool enable_debug_ = {false}
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr debug_path_pub_
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_waypoints_pub_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr debug_ref_point_pub_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr debug_end_ref_point_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr debug_generation_time_pub_
template<class Clock = std::chrono::high_resolution_clock>
class GenericRate : public as2::rate::RateBase

Public Functions

inline explicit GenericRate(double rate)
inline explicit GenericRate(std::chrono::nanoseconds period)
inline virtual bool sleep()
inline virtual void set_period(double rate)
inline virtual void set_period(std::chrono::nanoseconds period)
inline virtual bool is_steady() const
inline virtual void reset()
inline std::chrono::nanoseconds period() const

Private Types

using ClockDurationNano = std::chrono::duration<typename Clock::rep, std::nano>

Private Members

std::chrono::nanoseconds period_
std::chrono::time_point<Clock, ClockDurationNano> last_interval_
class GenericSensor
#include <sensor.hpp>

GenericSensor object to publish sensor data at a given frequency.

Subclassed by as2::sensors::Camera, as2::sensors::Gimbal, as2::sensors::GroundTruth, as2::sensors::Sensor< T >

Public Functions

explicit GenericSensor(as2::Node *node_ptr, const float pub_freq = -1.0f)

Construct a new GenericSensor object.

Parameters:

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

virtual ~GenericSensor()

Destroy the GenericSensor object.

void dataUpdated()

User must call this function when the data is updated.

virtual void publishData() = 0

User must implement the data publishing in this function.

Private Functions

void timerCallback()

Callback function for the timer. Publishes the data.

Private Members

rclcpp::TimerBase::SharedPtr timer_
float pub_freq_
struct geozone

Public Members

std::string type
std::vector<std::array<double, 2>> polygon
float z_up
float z_down
int id
int alert
std::string data_type
bool in
class Geozones : public as2::Node

Public Functions

Geozones()
void setupNode()
void cleanupNode()
void run()
void loadGeozones(const std::string path)

Private Types

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Private Functions

void gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr _msg)
void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr _msg)
void setGeozoneCb(const std::shared_ptr<as2_msgs::srv::SetGeozone::Request> request, std::shared_ptr<as2_msgs::srv::SetGeozone::Response> response)
void getGeozoneCb(const std::shared_ptr<as2_msgs::srv::GetGeozone::Request> request, std::shared_ptr<as2_msgs::srv::GetGeozone::Response> response)
void rvizVisualizationCb()
void checkGeozones()
bool checkValidity(int size, int id, std::string type, std::string data_type)
bool findGeozoneId(int id)
void setupGPS()
virtual CallbackReturn on_configure(const rclcpp_lifecycle::State&) override

Callback for the configure state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_activate(const rclcpp_lifecycle::State&) override
virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override

Callback for the deactivate state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override

Callback for the shutdown state.

Parameters:

state –

Returns:

CallbackReturn

Private Members

bool start_run_
bool origin_set_ = false
float self_latitude_
float self_longitude_
float self_altitude_
float self_x_
float self_y_
float self_z_
int max_priority
bool geofence_detected
std::string config_path_
bool rviz_visualization_ = false
std::array<double, 2> point_
std::vector<geozone> geozones_
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_
rclcpp::Publisher<as2_msgs::msg::AlertEvent>::SharedPtr alert_pub_
rclcpp::Publisher<as2_msgs::msg::PolygonList>::SharedPtr rviz_pub_
rclcpp::TimerBase::SharedPtr timer_
rclcpp::Service<as2_msgs::srv::SetGeozone>::SharedPtr set_geozone_srv_
rclcpp::Service<as2_msgs::srv::GetGeozone>::SharedPtr get_geozone_srv_
geographic_msgs::msg::GeoPoint::UniquePtr origin_
rclcpp::Client<as2_msgs::srv::GetOrigin>::SharedPtr get_origin_srv_
std::unique_ptr<as2::gps::GpsHandler> gps_handler
class GetOrigin : public as2_behavior_tree::BtServiceNode<as2_msgs::srv::GetOrigin>

Public Functions

GetOrigin(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion()

Public Static Functions

static inline BT::PortsList providedPorts()
class Gimbal : public as2::sensors::TFStatic, protected as2::sensors::TFDynamic, protected as2::sensors::GenericSensor, protected as2::sensors::SensorData<geometry_msgs::msg::PoseStamped>
#include <sensor.hpp>

Class to handle the gimbal sensor.

Public Functions

explicit Gimbal(const std::string &gimbal_id, const std::string &gimbal_base_id, as2::Node *node_ptr, const float pub_freq = -1.0f, bool add_sensor_measurements_base = true)

Construct a new Gimbal object.

Parameters:

  • gimbal_id – Frame ID of the gimbal

  • gimbal_base_id – Frame ID of the gimbal base

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

virtual ~Gimbal()

Destroy the Gimbal object.

void setGimbalBaseTransform(const geometry_msgs::msg::Transform &gimbal_base_transform, const std::string &gimbal_parent_frame_id = "base_link")

Set the gimbal base transformation respect to the parent frame.

Parameters:

  • gimbal_base_transform – Transform message

  • gimbal_parent_frame_id – Parent frame ID (default is “base_link”)

void updateData(const geometry_msgs::msg::PoseStamped &pose_msg)

Update the gimbal transformation respect to the base.

Parameters:

pose_msg – Pose message

void updateData(const geometry_msgs::msg::QuaternionStamped &orientation_msg)

Update the gimbal transformation respect to the base.

Parameters:

pose_msg – Pose message

const std::string &getGimbalFrameId() const

Get the Gimbal Frame ID.

Returns:

const std::string& Frame ID of the gimbal

const std::string &getGimbalBaseFrameId() const

Get the Gimbal Base Frame ID.

Returns:

const std::string& Frame ID of the gimbal base

Protected Functions

virtual void publishData()

Publish the data in a topic and in TF.

Protected Attributes

std::string gimbal_frame_id_
std::string gimbal_base_frame_id_
geometry_msgs::msg::TransformStamped gimbal_transform_
struct gimbal_status

Public Members

geometry_msgs::msg::Vector3 orientation
class GimbalBridge : public rclcpp::Node

Public Functions

GimbalBridge()

Private Functions

void gimbalCmdCallback(const as2_msgs::msg::GimbalControl::SharedPtr msg)

Private Members

rclcpp::Subscription<as2_msgs::msg::GimbalControl>::SharedPtr gimbal_cmd_sub_
std::shared_ptr<gz::transport::Node> gz_node_ptr_
gz::transport::Node::Publisher gimbal_roll_pub
gz::transport::Node::Publisher gimbal_pitch_pub
gz::transport::Node::Publisher gimbal_yaw_pub

Private Static Functions

static void gzJointStateCallback(const gz::msgs::Model &gz_msg, const gz::transport::MessageInfo &msg_info)

Private Static Attributes

static std::shared_ptr<std::string> model_name_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> gimbal_name_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> sensor_name_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> control_mode_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> world_name_ = std::make_shared<std::string>()
static rclcpp::Publisher<geometry_msgs::msg::QuaternionStamped>::SharedPtr gimbal_attitude_pub_ = nullptr
static rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr gimbal_angular_velocity_pub_ = nullptr
static std::shared_ptr<rclcpp::Clock> clock_ = nullptr
struct go_to_plugin_params

Public Members

double go_to_speed = 0.0
double go_to_threshold = 0.0
class GoToAction : public as2_behavior_tree::BtActionNode<as2_msgs::action::GoToWaypoint>

Public Functions

GoToAction(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick()

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

void on_wait_for_result(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Feedback> feedback)

Public Static Functions

static inline BT::PortsList providedPorts()
class GoToBase

Subclassed by go_to_plugin_position::Plugin, go_to_plugin_trajectory::Plugin

Public Types

using GoalHandleGoTo = rclcpp_action::ServerGoalHandle<as2_msgs::action::GoToWaypoint>

Public Functions

inline GoToBase()
inline virtual ~GoToBase()
inline void initialize(as2::Node *node_ptr, std::shared_ptr<as2::tf::TfHandler> tf_handler, go_to_plugin_params &params)
inline void state_callback(geometry_msgs::msg::PoseStamped &pose_msg, geometry_msgs::msg::TwistStamped &twist_msg)
inline void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal, std::shared_ptr<as2_msgs::action::GoToWaypoint::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::GoToWaypoint::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::GoToWaypoint::Goal &goal) = 0
inline virtual bool own_modify(as2_msgs::action::GoToWaypoint::Goal &goal)
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
inline virtual bool own_pause(const std::shared_ptr<std::string> &message)
inline virtual bool own_resume(const std::shared_ptr<std::string> &message)
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0
inline void sendHover()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler = nullptr
as2_msgs::action::GoToWaypoint::Goal goal_
as2_msgs::action::GoToWaypoint::Feedback feedback_
as2_msgs::action::GoToWaypoint::Result result_
int platform_state_
go_to_plugin_params params_
geometry_msgs::msg::PoseStamped actual_pose_
bool localization_flag_

Private Functions

inline bool processGoal(as2_msgs::action::GoToWaypoint::Goal &_goal)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr
class GoToBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::GoToWaypoint>

Public Types

using GoalHandleGoTo = rclcpp_action::ServerGoalHandle<as2_msgs::action::GoToWaypoint>

Public Functions

explicit GoToBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~GoToBehavior()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
void platform_info_callback(const as2_msgs::msg::PlatformInfo::SharedPtr msg)
bool process_goal(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal, as2_msgs::action::GoToWaypoint::Goal &new_goal)
bool on_activate(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::GoToWaypoint::Goal> &goal, std::shared_ptr<as2_msgs::action::GoToWaypoint::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::GoToWaypoint::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<go_to_base::GoToBase>> loader_
std::shared_ptr<go_to_base::GoToBase> go_to_plugin_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_sub_
class GoToGpsAction : public as2_behavior_tree::BtActionNode<as2_msgs::action::GoToWaypoint>

Public Functions

GoToGpsAction(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

void on_wait_for_result(std::shared_ptr<const as2_msgs::action::GoToWaypoint::Feedback> feedback)

Public Static Functions

static inline BT::PortsList providedPorts()

Private Members

rclcpp::Node::SharedPtr node_
rclcpp::Client<as2_msgs::srv::GeopathToPath>::SharedPtr client
geographic_msgs::msg::GeoPoseStamped geopose
geometry_msgs::msg::Point point
std::string service_name_
struct gps_object

Public Members

sensor_msgs::msg::NavSatFix::SharedPtr gps_pose
std_msgs::msg::Float32::SharedPtr azimuth
class GPSBridge : public rclcpp::Node

Public Functions

GPSBridge()

Private Members

std::shared_ptr<gz::transport::Node> ign_node_ptr_
std::string world_name
std::string name_space
std::string sensor_name
std::string link_name
std::string sensor_type

Private Static Functions

static std::string replace_delimiter(const std::string &input, const std::string &old_delim, const std::string new_delim)
static void ignitionGPSCallback(const gz::msgs::NavSat &ign_msg, const gz::transport::MessageInfo &msg_info)

Private Static Attributes

static bool use_sim_time_ = false
static rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_ = nullptr
class GpsHandler : private GeographicLib::LocalCartesian

Public Functions

inline GpsHandler()

Construct a new Gps Handler object based on WGS84 ellipsoid.

inline GpsHandler(double lat0, double lon0, double h0 = 0)

Construct a new Gps Handler object based on WGS84 ellipsoid with a given origin.

Parameters:

  • lat0 – Latitude at origin (degrees)

  • lon0 – Longitude at origin (degrees)

  • h0 – Altitude at origin (meters)

void setOrigin(const double &lat0, const double &lon0, const double &h0 = 0)
void setOrigin(const sensor_msgs::msg::NavSatFix &fix)
void setOrigin(const geographic_msgs::msg::GeoPoseStamped &gps)
void getOrigin(double &rLat, double &rLon, double &rH)
void getOrigin(geographic_msgs::msg::GeoPoseStamped &gps)
void LatLon2Local(const double &lat, const double &lon, const double &h, double &rX, double &rY, double &rZ)
void LatLon2Local(const sensor_msgs::msg::NavSatFix &fix, double &rX, double &rY, double &rZ)
void LatLon2Local(const geographic_msgs::msg::GeoPoseStamped &gps, double &rX, double &rY, double &rZ)
void LatLon2Local(const double &lat, const double &lon, const double &h, geometry_msgs::msg::PoseStamped &ps)
void LatLon2Local(const sensor_msgs::msg::NavSatFix &fix, geometry_msgs::msg::PoseStamped &ps)
void LatLon2Local(const geographic_msgs::msg::GeoPoseStamped &gps, geometry_msgs::msg::PoseStamped &ps)
void Local2LatLon(const double &x, const double &y, const double &z, double &rLat, double &rLon, double &rH)
void Local2LatLon(const double &x, const double &y, const double &z, geographic_msgs::msg::GeoPoseStamped &gps)
void Local2LatLon(const geometry_msgs::msg::PoseStamped &ps, double &rLat, double &rLon, double &rH)
void Local2LatLon(const geometry_msgs::msg::PoseStamped &ps, geographic_msgs::msg::GeoPoseStamped &gps)

Public Static Functions

static void LatLon2Ecef(const double &lat, const double &lon, const double &h, double &rX, double &rY, double &rZ)
static void LatLon2Ecef(const sensor_msgs::msg::NavSatFix &fix, double &rX, double &rY, double &rZ)
static void LatLon2Ecef(const geographic_msgs::msg::GeoPoseStamped &gps, double &rX, double &rY, double &rZ)
static void LatLon2Ecef(const double &lat, const double &lon, const double &h, geometry_msgs::msg::PoseStamped &ps)
static void LatLon2Ecef(const sensor_msgs::msg::NavSatFix &fix, geometry_msgs::msg::PoseStamped &ps)
static void LatLon2Ecef(const geographic_msgs::msg::GeoPoseStamped &gps, geometry_msgs::msg::PoseStamped &ps)
static void Ecef2LatLon(const double &x, const double &y, const double &z, double &rLat, double &rLon, double &rH)
static void Ecef2LatLon(const double &x, const double &y, const double &z, geographic_msgs::msg::GeoPoseStamped &gps)
static void Ecef2LatLon(const geometry_msgs::msg::PoseStamped &ps, double &rLat, double &rLon, double &rH)
static void Ecef2LatLon(const geometry_msgs::msg::PoseStamped &ps, geographic_msgs::msg::GeoPoseStamped &gps)

Private Members

const std::string local_frame_ = "map"
bool is_origin_set_ = false
class GpsToCartesian : public as2_behavior_tree::BtServiceNode<as2_msgs::srv::GeopathToPath>

Public Functions

GpsToCartesian(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion()

Public Static Functions

static inline BT::PortsList providedPorts()

Private Members

geometry_msgs::msg::Pose pose
geographic_msgs::msg::GeoPoseStamped geopose
geographic_msgs::msg::GeoPath geopath
template<typename T>
class GraphSearcher

Public Functions

inline GraphSearcher()
inline std::vector<Point2i> solve_graph(Point2i start, Point2i end)

Protected Functions

inline virtual void update_graph(const T &graph)
virtual double calc_h_cost(Point2i current, Point2i end) = 0
virtual double calc_g_cost(Point2i current) = 0
virtual int hash_key(Point2i point) = 0
virtual bool cell_in_limits(Point2i point) = 0
virtual bool cell_occuppied(Point2i point) = 0

Protected Attributes

T graph_
bool use_heuristic_ = false

Private Members

std::unordered_map<int, CellNodePtr> nodes_visited_
std::unordered_map<int, CellNodePtr> nodes_to_visit_
std::vector<Point2i> valid_movements_
class GripperBase

Subclassed by dc_servo::Plugin, two_fingers::Plugin

Public Functions

inline GripperBase()
inline virtual ~GripperBase()
inline void initialize(as2::Node *node_ptr)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> goal)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> goal, std::shared_ptr<as2_msgs::action::GripperHandler::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::GripperHandler::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::GripperHandler::Goal &goal) = 0
virtual bool own_modify(as2_msgs::action::GripperHandler::Goal &goal) = 0
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
virtual bool own_pause(const std::shared_ptr<std::string> &message) = 0
virtual bool own_resume(const std::shared_ptr<std::string> &message) = 0
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0

Protected Attributes

as2::Node *node_ptr_
as2_msgs::action::GripperHandler::Goal goal_
as2_msgs::action::GripperHandler::Feedback feedback_
as2_msgs::action::GripperHandler::Result result_
class GripperHandlerBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::GripperHandler>

Public Functions

explicit GripperHandlerBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~GripperHandlerBehavior()

Private Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> goal) override

As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> goal) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::GripperHandler::Goal> &goal, std::shared_ptr<as2_msgs::action::GripperHandler::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::GripperHandler::Result> &result_msg) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

as2_msgs::action::GripperHandler::Goal goal_
as2_msgs::action::GripperHandler::Result result_
as2_msgs::action::GripperHandler::Feedback feedback_
std::filesystem::path plugin_name_

Gripper handler Behavior plugin

std::shared_ptr<pluginlib::ClassLoader<gripper_behavior_plugin_base::GripperBase>> loader_
std::shared_ptr<gripper_behavior_plugin_base::GripperBase> gripper_handler_plugin_
class GroundTruth : protected as2::sensors::GenericSensor
#include <sensor.hpp>

Class to handle the ground truth of the platform.

Public Functions

explicit GroundTruth(as2::Node *node_ptr, const float pub_freq = -1, const std::string &topic_name_base = "")

Construct a new GroundTruth object.

Parameters:

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • topic_name_base – Base name of the topics. Default is “”

virtual ~GroundTruth()

Destroy the GroundTruth object.

void updateData(const geometry_msgs::msg::PoseStamped &pose_msg)

Update the data of the ground truth.

Parameters:

pose_msg – Pose message

void updateData(const geometry_msgs::msg::TwistStamped &twist_msg)

Update the data of the ground truth.

Parameters:

twist_msg – Twist message

void updateData(const geometry_msgs::msg::PoseStamped &pose_msg, const geometry_msgs::msg::TwistStamped &twist_msg)

Update the data of the ground truth.

Parameters:

  • pose_msg – Pose message

  • twist_msg – Twist message

Protected Functions

virtual void publishData() override

User must implement the data publishing in this function.

Protected Attributes

std::shared_ptr<SensorData<geometry_msgs::msg::PoseStamped>> pose_sensor_
std::shared_ptr<SensorData<geometry_msgs::msg::TwistStamped>> twist_sensor_
class GroundTruthBridge : public rclcpp::Node

Public Functions

GroundTruthBridge()

Private Members

std::shared_ptr<gz::transport::Node> ign_node_ptr_
std::string model_name_

Private Static Functions

static std::string replace_delimiter(const std::string &input, const std::string &old_delim, const std::string new_delim)
static void ignitionGroundTruthCallback(const gz::msgs::Odometry &ign_msg, const gz::transport::MessageInfo &msg_info)

Private Static Attributes

static std::shared_ptr<std::string> pose_frame_id_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> twist_frame_id_ = std::make_shared<std::string>()
static rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ps_pub_ = nullptr
static rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr ts_pub_ = nullptr
class HoverMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <hover_motion.hpp>

The HoverMotion class is a motion reference handler that allows the robot to hover at the current position.

Public Functions

explicit HoverMotion(as2::Node *node_ptr, const std::string &ns = "")

HoverMotion Constructor.

Parameters:

node – as2::Node pointer.

inline ~HoverMotion()

HoverMotion Destructor.

bool sendHover()

Send hover motion command.

Returns:

true if the motion reference was sent successfully.

template<typename P = double, int num_rotors = 4>
class IndiController : public pid_controller::PID<double>, public pid_controller::PID<double>
#include <IndiController.hpp>

Incremental Nonlinear Dynamic Inversion (INDI) controller.

Convert the desired thrust and angular velocity to motor angular velocity.

Template Parameters:

  • P – Precision type of the controller

  • num_rotors – Number of rotors of the multirotor

Public Functions

inline IndiController(const Matrix3 &inertia, const Matrix4 &mixer_matrix_inverse, const PIDParams &pid_params)

Construct a new Indi Controller object.

Parameters:

  • inertia – Vehicle inertia matrix (kg m^2)

  • mixer_matrix_inverse – Mixer matrix inverse [num_rotors x 6]

  • pid_params – PID parameters

inline explicit IndiController(const IndiControllerParams<P> &params = IndiControllerParams<P>())

Construct a new Indi Controller object.

Parameters:

params – IndiControllerParams parameters

inline ~IndiController()

Destroy the Indi Controller object.

inline VectorN acro_to_motor_angular_velocity(const Vector3 &current_vehicle_angular_velocity, const Scalar thrust, const Vector3 &desired_angular_velocity, const Scalar dt)

Compute the control action.

Parameters:

  • current_vehicle_angular_velocity – Vector3 with the current vehicle angular velocity (rad/s) in body frame

  • thrust – Scalar with the desired thrust (N) in body frame

  • desired_angular_velocity – Vector3 with the desired angular velocity (rad/s) in body frame

  • dt – Scalar with the time step (s)

Returns:

VectorN with the desired motor angular velocity squared (rad^2/s^2)

inline void update_inertia(const Matrix3 &inertia)

Update inertial.

Parameters:

inertia – Matrix3 Inertia matrix (kg m^2)

inline void update_mixer_matrix_inverse(const Matrix4 &mixer_matrix_inverse)

Update mixer matrix inverse.

Parameters:

mixer_matrix_inverse – MatrixN Mixer matrix inverse

inline void update_params(const IndiControllerParams<P> &params)

Update controller parameters.

Parameters:

params – IndiControllerParams

inline const Matrix3 &get_inertia() const

Get the inertia.

Returns:

Matrix3 Inertia matrix (kg m^2)

inline const Matrix4 &get_mixer_matrix_inverse() const

Get the mixer matrix inverse.

Returns:

MatrixN Mixer matrix inverse

inline const Vector3 &get_desired_angular_acceleration() const

Get the desired angular acceleration.

Returns:

constVector3& Desired angular acceleration (rad/s^2)

inline const Vector3 &get_desired_thrust() const

Get the desired thrust.

Returns:

const Vector3& Desired thrust (N)

inline const Vector3 &get_desired_torque() const

Get the desired torque.

Returns:

const Vector3& Desired torque (N m)

inline const VectorN &get_motor_angular_velocity() const

Get the motor angular velocity.

Returns:

const VectorN& Motor angular velocity squared (rad^2/s^2)

inline const Vector3 &get_angular_velocity_error() const

Get the angular velocity error.

Returns:

Vector3& Angular velocity error (rad/s)

inline IndiController(const Matrix3 &inertia, const Matrix4 &mixer_matrix_inverse, const PIDParams &pid_params)

Construct a new Indi Controller object.

Parameters:

  • inertia – Vehicle inertia matrix (kg m^2)

  • mixer_matrix_inverse – Mixer matrix inverse [num_rotors x 6]

  • pid_params – PID parameters

inline explicit IndiController(const IndiControllerParams<P> &params = IndiControllerParams<P>())

Construct a new Indi Controller object.

Parameters:

params – IndiControllerParams parameters

inline ~IndiController()

Destroy the Indi Controller object.

inline VectorN acro_to_motor_angular_velocity(const Vector3 &current_vehicle_angular_velocity, const Scalar thrust, const Vector3 &desired_angular_velocity, const Scalar dt)

Compute the control action.

Parameters:

  • current_vehicle_angular_velocity – Vector3 with the current vehicle angular velocity (rad/s) in body frame

  • thrust – Scalar with the desired thrust (N) in body frame

  • desired_angular_velocity – Vector3 with the desired angular velocity (rad/s) in body frame

  • dt – Scalar with the time step (s)

Returns:

VectorN with the desired motor angular velocity squared (rad^2/s^2)

inline void update_inertia(const Matrix3 &inertia)

Update inertial.

Parameters:

inertia – Matrix3 Inertia matrix (kg m^2)

inline void update_mixer_matrix_inverse(const Matrix4 &mixer_matrix_inverse)

Update mixer matrix inverse.

Parameters:

mixer_matrix_inverse – MatrixN Mixer matrix inverse

inline void update_params(const IndiControllerParams<P> &params)

Update controller parameters.

Parameters:

params – IndiControllerParams

inline const Matrix3 &get_inertia() const

Get the inertia.

Returns:

Matrix3 Inertia matrix (kg m^2)

inline const Matrix4 &get_mixer_matrix_inverse() const

Get the mixer matrix inverse.

Returns:

MatrixN Mixer matrix inverse

inline const Vector3 &get_desired_angular_acceleration() const

Get the desired angular acceleration.

Returns:

constVector3& Desired angular acceleration (rad/s^2)

inline const Vector3 &get_desired_thrust() const

Get the desired thrust.

Returns:

const Vector3& Desired thrust (N)

inline const Vector3 &get_desired_torque() const

Get the desired torque.

Returns:

const Vector3& Desired torque (N m)

inline const VectorN &get_motor_angular_velocity() const

Get the motor angular velocity.

Returns:

const VectorN& Motor angular velocity squared (rad^2/s^2)

inline const Vector3 &get_angular_velocity_error() const

Get the angular velocity error.

Returns:

Vector3& Angular velocity error (rad/s)

Protected Attributes

Matrix3 inertia_ = Matrix3::Zero()
Matrix4 mixer_matrix_inverse_ = Matrix4::Zero()
Vector3 desired_thrust_ = Vector3::Zero()
Vector3 desired_torque_ = Vector3::Zero()
VectorN motor_angular_velocity_ = VectorN::Zero()

Private Types

using Scalar = P
using Vector3 = Eigen::Matrix<P, 3, 1>
using Vector4 = Eigen::Matrix<P, 4, 1>
using Vector6 = Eigen::Matrix<P, 6, 1>
using VectorN = Eigen::Matrix<P, num_rotors, 1>
using Matrix3 = Eigen::Matrix<P, 3, 3>
using Matrix4 = Eigen::Matrix<P, 4, 4>
using Matrix6 = Eigen::Matrix<P, 6, 6>
using MatrixN = Eigen::Matrix<P, num_rotors, 6>
using Matrix6N = Eigen::Matrix<P, 6, num_rotors>
using PID = pid_controller::PID<P>
using PIDParams = pid_controller::PIDParams<P>
using Scalar = P
using Vector3 = Eigen::Matrix<P, 3, 1>
using Vector4 = Eigen::Matrix<P, 4, 1>
using Vector6 = Eigen::Matrix<P, 6, 1>
using VectorN = Eigen::Matrix<P, num_rotors, 1>
using Matrix3 = Eigen::Matrix<P, 3, 3>
using Matrix4 = Eigen::Matrix<P, 4, 4>
using Matrix6 = Eigen::Matrix<P, 6, 6>
using MatrixN = Eigen::Matrix<P, num_rotors, 6>
using Matrix6N = Eigen::Matrix<P, 6, num_rotors>
using PID = pid_controller::PID<P>
using PIDParams = pid_controller::PIDParams<P>
template<typename P = double, int num_rotors = 4>
struct IndiControllerParams
#include <IndiController.hpp>

INDI controller parameters.

Template Parameters:

P – Precision type of the controller

Public Types

using Matrix3 = Eigen::Matrix<P, 3, 3>
using MatrixN = Eigen::Matrix<P, num_rotors, 4>
using PIDParams = pid_controller::PIDParams<P>
using Matrix3 = Eigen::Matrix<P, 3, 3>
using MatrixN = Eigen::Matrix<P, num_rotors, 4>
using PIDParams = pid_controller::PIDParams<P>

Public Members

Matrix3 inertia = Matrix3::Zero()
MatrixN mixer_matrix_inverse = MatrixN::Zero()
PIDParams pid_params = PIDParams()
class IntPoint
#include <point.h>

A light-weight integer point with fields x,y

Public Functions

inline IntPoint()
inline IntPoint(int _x, int _y)

Public Members

int x
int y
class IsFlyingCondition : public BT::ConditionNode

Public Functions

IsFlyingCondition(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
IsFlyingCondition() = delete
BT::NodeStatus tick() override

Public Static Functions

static inline BT::PortsList providedPorts()

Private Functions

inline void stateCallback(as2_msgs::msg::PlatformInfo::SharedPtr msg)

Private Members

rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr state_sub_
bool is_flying_ = false
struct land_plugin_params

Public Members

double land_speed = 0.0
class LandAction : public as2_behavior_tree::BtActionNode<as2_msgs::action::Land>

Public Functions

LandAction(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

void on_wait_for_result(std::shared_ptr<const as2_msgs::action::Land::Feedback> feedback)

Public Static Functions

static inline BT::PortsList providedPorts()
class LandBase

Subclassed by land_plugin_platform::Plugin, land_plugin_position::Plugin, land_plugin_speed::Plugin, land_plugin_trajectory::Plugin

Public Types

using GoalHandleLand = rclcpp_action::ServerGoalHandle<as2_msgs::action::Land>

Public Functions

inline LandBase()
inline virtual ~LandBase()
inline void initialize(as2::Node *node_ptr, std::shared_ptr<as2::tf::TfHandler> tf_handler, land_plugin_params &params)
inline virtual void state_callback(geometry_msgs::msg::PoseStamped &pose_msg, geometry_msgs::msg::TwistStamped &twist_msg)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::Land::Goal> goal)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::Land::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::Land::Goal> goal, std::shared_ptr<as2_msgs::action::Land::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::Land::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::Land::Goal &goal) = 0
inline virtual bool own_modify(as2_msgs::action::Land::Goal &goal)
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
inline virtual bool own_pause(const std::shared_ptr<std::string> &message)
inline virtual bool own_resume(const std::shared_ptr<std::string> &message)
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0
inline void sendHover()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler = nullptr
as2_msgs::action::Land::Goal goal_
as2_msgs::action::Land::Feedback feedback_
as2_msgs::action::Land::Result result_
land_plugin_params params_
geometry_msgs::msg::PoseStamped actual_pose_
bool localization_flag_ = false

Private Functions

inline bool processGoal(as2_msgs::action::Land::Goal &_goal)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr
class LandBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::Land>

Public Types

using GoalHandleLand = rclcpp_action::ServerGoalHandle<as2_msgs::action::Land>
using PSME = as2_msgs::msg::PlatformStateMachineEvent

Public Functions

explicit LandBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~LandBehavior()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
bool sendEventFSME(const int8_t _event)
bool sendDisarm()
bool process_goal(std::shared_ptr<const as2_msgs::action::Land::Goal> goal, as2_msgs::action::Land::Goal &new_goal)
bool on_activate(std::shared_ptr<const as2_msgs::action::Land::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::Land::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::Land::Goal> &goal, std::shared_ptr<as2_msgs::action::Land::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::Land::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<land_base::LandBase>> loader_
std::shared_ptr<land_base::LandBase> land_plugin_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
as2::SynchronousServiceClient<as2_msgs::srv::SetPlatformStateMachineEvent>::SharedPtr platform_land_cli_
as2::SynchronousServiceClient<std_srvs::srv::SetBool>::SharedPtr platform_disarm_cli_
class MapServer : public as2::Node

Public Functions

MapServer()
virtual ~MapServer() = default
virtual CallbackReturn on_configure(const rclcpp_lifecycle::State&) override

Callback for the configure state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override

Callback for the deactivate state.

Parameters:

state –

Returns:

CallbackReturn

virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State&) override

Callback for the shutdown state.

Parameters:

state –

Returns:

CallbackReturn

Private Types

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Private Members

std::filesystem::path plugin_name_
std::shared_ptr<pluginlib::ClassLoader<as2_map_server_plugin_base::MapServerBase>> loader_
std::shared_ptr<as2_map_server_plugin_base::MapServerBase> plugin_ptr_
class MapServerBase

Subclassed by scan2occ_grid::Plugin

Public Functions

inline MapServerBase()
inline void setup(as2::Node *node)
virtual void on_setup() = 0

Protected Attributes

as2::Node *node_ptr_
class MassEstimationBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::MassEstimation>

Public Functions

explicit MassEstimationBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~MassEstimationBehavior()

Private Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::MassEstimation::Goal> goal) override

As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::MassEstimation::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::MassEstimation::Goal> &goal, std::shared_ptr<as2_msgs::action::MassEstimation::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::MassEstimation::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
void comandedThrustCallback(const as2_msgs::msg::Thrust::SharedPtr thrust_msg)
void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_msg)
void updateMassParameter()

Private Members

std::shared_ptr<ParamEstimation> param_estimation_lib
rclcpp::Subscription<as2_msgs::msg::Thrust>::SharedPtr comanded_thrust_sub_
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr debug_mass_estimation_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr debug_mass_filtered_pub_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr debug_mass_update_pub_
as2::SynchronousServiceClient<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_
as2::SynchronousServiceClient<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_
rcl_interfaces::msg::Parameter param_
double mass_threshold_
double thrust_threshold_
double alpha_
size_t n_samples_ = 3
double mass_publish_interval_
double minimum_mass_
double maximum_mass_
double initial_mass_
std::string controler_node_
std::string mass_param_name_
std::string mass_update_topic_
std::string mass_filtered_topic_
std::string mass_estimation_topic_
float last_commanded_thrust_ = 0.0
float next_commanded_thrust_ = 0.0
rclcpp::Time last_mass_update_time_
double estimated_mass_
double filtered_mass_
double last_filtered_mass_
std::vector<double> measured_az_stack_
bool thrust_received_ = false
bool behvaior_paused_ = false
bool mass_publish_ = true
class MockPlatform : public as2::Node
#include <mock_platform.hpp>

Fake platform node used by *_mock.cpp smoke executables.

Serves the two services the controller manager needs at startup (list_control_modes, set_platform_control_mode) and, after a configurable delay, drives the controller into the requested control mode through the public controller/set_control_mode service. Replaces the ~150 lines of boilerplate that used to be duplicated verbatim in every plugin’s <plugin>_mock.cpp.

Public Functions

explicit MockPlatform(const std::vector<uint8_t> &available_modes, const ControlModeRequest &request, std::chrono::milliseconds init_delay = std::chrono::seconds(1), const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct the fake platform node and arm the synthetic mode request.

Parameters:

  • available_modes – Modes returned by list_control_modes (typically a HOVER input + an ACRO output bitmap).

  • request – Mode requested through controller/set_control_mode after init_delay.

  • init_delay – Delay before issuing the request, to give the manager time to advertise its services.

  • options – Standard NodeOptions (namespace, params, etc.).

Private Functions

void callSetControlMode()

One-shot timer callback that fires the synthetic controller/set_control_mode request.

void handleListControlModes(const std::shared_ptr<as2_msgs::srv::ListControlModes::Request> request, std::shared_ptr<as2_msgs::srv::ListControlModes::Response> response)

Service handler for list_control_modes.

Returns the modes configured at construction time as the platform-supported list.

Parameters:

  • request – Service request (ignored).

  • response – Service response filled with the configured modes.

void handleSetPlatformControlMode(const std::shared_ptr<as2_msgs::srv::SetControlMode::Request> request, std::shared_ptr<as2_msgs::srv::SetControlMode::Response> response)

Service handler for platform/set_platform_control_mode.

Always succeeds; emulates the platform accepting the requested mode.

Parameters:

  • request – Mode the controller asks the platform to enter.

  • response – Service response with success set to true.

Private Members

std::vector<uint8_t> available_modes_
ControlModeRequest request_
std::shared_ptr<as2::SynchronousServiceClient<as2_msgs::srv::SetControlMode>> set_control_mode_client_
rclcpp::Service<as2_msgs::srv::ListControlModes>::SharedPtr list_control_modes_server_
rclcpp::Service<as2_msgs::srv::SetControlMode>::SharedPtr set_platform_control_mode_server_
rclcpp::TimerBase::SharedPtr init_timer_
rclcpp::CallbackGroup::SharedPtr list_modes_srv_callback_group_
rclcpp::CallbackGroup::SharedPtr set_mode_srv_callback_group_
struct ModeParametersRead
#include <pid_speed_controller.hpp>

Per-mode readiness for the optional gain groups.

The base already gates the essential groups (plugin / position_control / yaw_control) via essentialParamsReady(); these flags add the per-mode gating that the base does not know about (TRAJECTORY needs trajectory_control gains; SPEED / SPEED_IN_A_PLANE need speed_control gains when !use_bypass_).

Public Members

bool velocity = false
bool speed_in_a_plane = false
bool trajectory = false
class MulticopterINDIControl : public System, public ISystemConfigure, public ISystemPreUpdate, public System, public ISystemConfigure, public ISystemPreUpdate

Public Functions

MulticopterINDIControl() = default
void Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) override
void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override
MulticopterINDIControl() = default
void Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &_ecm, EventManager &_eventMgr) override
void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override

Private Types

using RotorConfiguration = std::vector<multicopter_control::Rotor>
using RotorConfiguration = std::vector<multicopter_control::Rotor>

Private Functions

void OnACRO(const msgs::Float_V &_msg)

Callback for ACRO messages The controller waits for the first ACRO message before publishing any rotor velocities.

Parameters:

_msg – [in] ACRO message

void OnEnable(const msgs::Boolean &_msg)

Callback for enable messages.

Parameters:

_msg – [in] Callback message. If false, the controller sends a zero rotor velocity command once and gets disabled. If the vehicle is in the air, disabling the controller will cause it to fall. If true, the controller becomes enabled and waits for an ACRO message.

void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels)

Publish provided rotor velocities.

Parameters:

  • _ecm – [in] Mutable reference to the EntityComponentManager

  • _vels – [in] Rotor velocities to be published

math::Inertiald VehicleInertial(const EntityComponentManager &_ecm, Entity _entity)

Get the vehicle inertial from child links and nested models.

Parameters:

  • _ecm – [in] Immutable reference to the EntityComponentManager

  • _entity – [in] Model entity to get inertial for

Eigen::Matrix<double, 4, 4> compute_mixer_matrix_4D(const RotorConfiguration &motors)
void OnACRO(const msgs::Float_V &_msg)

Callback for ACRO messages The controller waits for the first ACRO message before publishing any rotor velocities.

Parameters:

_msg – [in] ACRO message

void OnEnable(const msgs::Boolean &_msg)

Callback for enable messages.

Parameters:

_msg – [in] Callback message. If false, the controller sends a zero rotor velocity command once and gets disabled. If the vehicle is in the air, disabling the controller will cause it to fall. If true, the controller becomes enabled and waits for an ACRO message.

void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels)

Publish provided rotor velocities.

Parameters:

  • _ecm – [in] Mutable reference to the EntityComponentManager

  • _vels – [in] Rotor velocities to be published

math::Inertiald VehicleInertial(const EntityComponentManager &_ecm, Entity _entity)

Get the vehicle inertial from child links and nested models.

Parameters:

  • _ecm – [in] Immutable reference to the EntityComponentManager

  • _entity – [in] Model entity to get inertial for

Eigen::Matrix<double, 4, 4> compute_mixer_matrix_4D(const RotorConfiguration &motors)

Private Members

Model model = {kNullEntity}

Model interface.

std::string comLinkName

Link name.

Entity comLinkEntity

Link Entity.

std::string robotNamespace

Topic namespace.

std::string commandSubTopic = {"acro_vel"}

Topic for ACRO commands.

std::string enableSubTopic = {"enable"}

Topic for enabling commands.

transport::Node node

Gazebo communication node.

Eigen::VectorXd rotorVelocities = Eigen::VectorXd::Zero(4)

Holds the computed rotor angular velocities.

indi_controller::IndiController<double, 4> indiController

INDI controller, particularized for a quadrotor.

multicopter_control::NoiseParameters noiseParameters

Noise parameters read from SDF.

std::optional<msgs::Float_V> acroVelMsg

Current ACRO command. This is the reference the controller will try to maintain.

math::Vector3d maximumAngularVelocity

Maximum commanded angular velocity.

std::mutex acroVelMsgMutex

Mutex for acroVelMsg.

msgs::Actuators rotorVelocitiesMsg

Rotor velocities message.

bool initialized = {false}

Becomes true when the system is done initializing.

std::atomic<bool> controllerActive = {true}

True as long as the controller is active.

class MultirotorSimulatorPlatform : public as2::AerialPlatform

Public Functions

explicit MultirotorSimulatorPlatform(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~MultirotorSimulatorPlatform()
virtual void configureSensors() override

Configures the platform sensors.

virtual bool ownSetArmingState(bool state) override

Handles how arming state has to be settled in the concrete platform.

Parameters:

state – true for arming the platform, false to disarm.

Returns:

true Arming state is settled successfully.

Returns:

false Arming state is not settled.

virtual bool ownSetOffboardControl(bool offboard) override

Handles how offboard mode has to be settled in the concrete platform.

Parameters:

offboard – true if offboard mode is enabled.

Returns:

true Offboard mode is settled successfully.

Returns:

false Offboard mode is not settled.

virtual bool ownSetPlatformControlMode(const as2_msgs::msg::ControlMode &msg) override

Handles how the control mode has to be settled in the concrete platform.

Parameters:

control_mode – as2_msgs::msg::PlatformControlMode with the new control mode.

Returns:

true Control mode is settled successfully.

Returns:

false Control mode is not settled.

virtual bool ownSendCommand() override

Handles how a command must be sended in the concrete platform.

Returns:

true command is sended successfully.

Returns:

false command is not sended.

virtual void ownStopPlatform() override

Handles the platform emergency stop command. STOP means to hover as best as possible. This hover is different from the hover in the platform control mode. And when it is activated the platform will stop hearing commands from AS2. USE WITH CAUTION.

virtual void ownKillSwitch() override

Handles the platform emergency kill switch command. This means stop the motors inmediately, this cannot be reversed. USE WITH CAUTION.

virtual bool ownTakeoff() override

Handles the platform takeoff command.

Returns:

true Takeoff command is sended successfully.

Returns:

false Takeoff command is not sended.

virtual bool ownLand() override

Handles the platform landing command.

Returns:

true Landing command is sended successfully.

Returns:

false Landing command is not sended.

void gimbalControlCallback(const as2_msgs::msg::GimbalControl::SharedPtr msg)

Private Types

using Simulator = multirotor::Simulator<double, 4>
using SimulatorParams = multirotor::SimulatorParams<double, 4>
using Kinematics = multirotor::state::internal::Kinematics<double>

Private Functions

Eigen::Vector3d readVectorParams(const std::string &param_name)

Read given parameter of vector type.

Parameters:

param_name – Name of the parameter

Returns:

Eigen::Vector3d Vector parameter

inline void readParams(PlatformParams &platform_params)

Read platform parameters.

Parameters:

param_name – platform parameters

template<typename T>
inline void getParam(const std::string &param_name, T &param_value, bool use_default = false)

Get parameter from the parameter server.

Parameters:

  • param_name – Name of the parameter

  • param_value – Value of the parameter

  • use_default – Use default value if parameter is not found

void simulatorTimerCallback()

Simulator timer callback.

void simulatorControlTimerCallback()

Simulator control timer callback.

void simulatorInertialOdometryTimerCallback()

Simulator inertial odometry timer callback.

void simulatorStateTimerCallback()

Simulator state timer callback.

Private Members

As2MultirotorSimulatorInterface as2_interface_
as2::gps::GpsHandler gps_handler_
PlatformParams platform_params_
Simulator simulator_
SimulatorParams simulator_params_
geometry_msgs::msg::Point initial_position_
Kinematics control_state_
bool using_odom_for_control_ = false
rclcpp::TimerBase::SharedPtr simulator_timer_
rclcpp::TimerBase::SharedPtr simulator_control_timer_
rclcpp::TimerBase::SharedPtr simulator_inertial_odometry_timer_
rclcpp::TimerBase::SharedPtr simulator_state_pub_timer_
std::string frame_id_baselink_ = "base_link"
std::string frame_id_earth_ = "earth"
geometry_msgs::msg::QuaternionStamped gimbal_desired_orientation_
rclcpp::Subscription<as2_msgs::msg::GimbalControl>::SharedPtr gimbal_control_sub_
std::unique_ptr<as2::sensors::GroundTruth> sensor_ground_truth_ptr_
std::unique_ptr<as2::sensors::Sensor<nav_msgs::msg::Odometry>> sensor_odom_estimate_ptr_
std::unique_ptr<as2::sensors::Sensor<sensor_msgs::msg::Imu>> sensor_imu_ptr_
std::unique_ptr<as2::sensors::Sensor<sensor_msgs::msg::NavSatFix>> sensor_gps_ptr_
std::unique_ptr<as2::sensors::Gimbal> sensor_gimbal_ptr_
class Node : public AS2_NODE_FATHER_TYPE
#include <node.hpp>

Basic Aerostack2 Node, it heritages all the functionality of an rclcpp::Node.

Subclassed by as2_behavior::BehaviorServer< as2_msgs::action::MassEstimation >, as2_behavior::BehaviorServer< as2_msgs::action::SwarmFlocking >, as2_behavior::BehaviorServer< as2_msgs::action::SetArmingState >, as2_behavior::BehaviorServer< as2_msgs::action::NavigateToPoint >, as2_behavior::BehaviorServer< as2_msgs::action::ForceEstimation >, as2_behavior::BehaviorServer< as2_msgs::action::Takeoff >, as2_behavior::BehaviorServer< as2_msgs::action::GeneratePolynomialTrajectory >, as2_behavior::BehaviorServer< as2_msgs::action::FollowReference >, as2_behavior::BehaviorServer< as2_msgs::action::FollowPath >, as2_behavior::BehaviorServer< as2_msgs::action::SetOffboardMode >, as2_behavior::BehaviorServer< as2_msgs::action::GoToWaypoint >, as2_behavior::BehaviorServer< as2_msgs::action::PointGimbal >, as2_behavior::BehaviorServer< as2_msgs::action::DetectArucoMarkers >, as2_behavior::BehaviorServer< as2_msgs::action::GripperHandler >, as2_behavior::BehaviorServer< as2_msgs::action::Land >, AlphanumericViewer, As2ExternalObjectToTf, Geozones, as2::AerialPlatform, as2::BasicBehavior< MessageT >, as2::BasicBehavior< MessageT >, as2_behavior::BehaviorServer< actionT >, as2_map_server::MapServer, as2_motion_controller_test::MockPlatform, as2_state_estimator::StateEstimator, controller_manager::ControllerManager, real_sense_interface::RealsenseInterface, usb_camera_interface::UsbCameraInterface

Public Functions

inline Node(const std::string &name, const std::string &ns, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct a new Node object.

Parameters:

name – Node name

inline explicit Node(const std::string &name, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline void configure()
inline void activate()
inline void deactivate()
inline void cleanup()
inline void shutdown()
inline void error()
std::string generate_local_name(const std::string &name)

transform an string into local topic name inside drone namespace and node namespace

Parameters:

name – source string

Returns:

std::string result name

std::string generate_global_name(const std::string &name)

transform an string into global topic name inside drone namespace

Parameters:

name – source string

Returns:

std::string result name

template<typename DurationRepT, typename DurationT, typename CallbackT>
inline rclcpp::TimerBase::SharedPtr create_timer(std::chrono::duration<DurationRepT, DurationT> period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr)

create a timer with the node clock

Returns:

rclcpp::TimerBase::SharedPtr rclcpp timer using node clock

inline bool sleep()

sleeps the node to ensure node_frecuency desired

Returns:

true the node is sleeping

Returns:

false the node is not sleeping, this means that desired frequency is not reached

inline double get_loop_frequency()

Get the loop frequency object.

Returns:

double frequency in Hz

inline bool preset_loop_frequency(double frequency)

Protected Types

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn

Callback for the activate state.

Param state:

Return:

CallbackReturn

Protected Functions

inline virtual CallbackReturn on_activate(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())
inline virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())

Callback for the deactivate state.

Parameters:

state –

Returns:

CallbackReturn

inline virtual CallbackReturn on_configure(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())

Callback for the configure state.

Parameters:

state –

Returns:

CallbackReturn

inline virtual CallbackReturn on_cleanup(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())

Callback for the cleanup state.

Parameters:

state –

Returns:

CallbackReturn

inline virtual CallbackReturn on_shutdown(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())

Callback for the shutdown state.

Parameters:

state –

Returns:

CallbackReturn

inline virtual CallbackReturn on_error(const rclcpp_lifecycle::State& = rclcpp_lifecycle::State())

Callback for the error state.

Parameters:

state –

Returns:

CallbackReturn

Private Functions

inline void init()

Private Members

double loop_frequency_

frequency of the spin cycle of the node

std::shared_ptr<as2::Rate> loop_rate_ptr_
class Node

Public Functions

inline Node(const cv::Point2i &coords, const NodePtr &parent_ptr, const cv::Point2i goal)
inline cv::Point2i get_coordinates() const
inline NodePtr get_parent() const
inline double get_g_cost() const
inline double get_h_cost() const
inline double get_f_cost() const
inline operator int() const
inline double computeCosts(const cv::Point2i &goal)

Private Members

double g_cost_
double h_cost_
double f_cost_
cv::Point2i coordinates_
NodePtr parent_ptr_
struct NoiseParameters
#include <Parameters.hpp>

Noise parameters used when computing frame data. These are all assumed to be gaussian.

Public Members

Eigen::Vector3d linearVelocityMean
Eigen::Vector3d linearVelocityStdDev
Eigen::Vector3d angularVelocityMean
Eigen::Vector3d angularVelocityStdDev
class ObjectFramePublisher : public rclcpp::Node

Public Functions

ObjectFramePublisher()

Private Members

rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription
std::shared_ptr<gz::transport::Node> ign_node_ptr_

Private Static Functions

static void poseCallback(const gz::msgs::Pose_V &ign_msg, const gz::transport::MessageInfo &msg_info)

Private Static Attributes

static std::unique_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster = NULL
static std::shared_ptr<std::string> world_frame_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> model_name_ = std::make_shared<std::string>()
static std::shared_ptr<std::string> world_name = std::make_shared<std::string>()
static bool use_sim_time_ = false
class OffboardService : public as2_behavior_tree::BtServiceNode<std_srvs::srv::SetBool>

Public Functions

OffboardService(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion(std::shared_ptr<std_srvs::srv::SetBool::Response> response)
class OriginAlreadySet : public std::runtime_error

Public Functions

inline OriginAlreadySet()
class OriginNonSet : public std::runtime_error

Public Functions

inline OriginNonSet()
class ParamEstimation

Public Functions

ParamEstimation(double initial_mass, double threshold, double alpha, size_t n_samples)
inline ~ParamEstimation()
void computeMass(float &thrust, std::vector<double> &a_z)

Computes the mass based on the thrust and acceleration.

Parameters:

  • thrust – Thrust value (z axis)

  • a_z – Acceleration in z axis

void set_threshold(double threshold)
void set_alpha(double alpha)
void set_n_samples(size_t n_samples)
double getEstimatedMass()
double getInstantMass()
double getThreshold()
double getAlpha()
size_t getNSamples()

Private Functions

bool computeMassError(double &estimated_mass, double &last_estimated_mass)

Computes the mass error based on the real and estimated mass.

Parameters:

  • real_mass – Real mass of the drone

  • estimated_mass – Estimated mass of the drone

double computedMeanFromVector(std::vector<double> &vec)

Computes the mean of a vector.

Parameters:

vec – Vector to compute the mean

Returns:

Mean value of the vector

double lowPassFiltered(double &mass)

Compute a low pass filter for the mass data.

Parameters:

mass – Mass value to filter

Returns:

Filtered mass value

double computedMeanFromNSamples(const std::vector<double> &vec)

Computes the mean of the last n samples of a vector.

Parameters:

vec – Vector to compute the mean

Returns:

Mean value of the last n samples

Private Members

double estimated_mass_
std::vector<double> estimated_mass_vector_
double last_estimated_mass_
double last_filtered_mass_
double instant_mass_ = 0.0
double threshold_ = 0.0
double alpha_ = 1.0
size_t n_samples_ = 1
class PathPlannerBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::NavigateToPoint>

Public Functions

explicit PathPlannerBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~PathPlannerBehavior()

Private Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::NavigateToPoint::Goal> goal) override

As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::NavigateToPoint::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::NavigateToPoint::Goal> &goal, std::shared_ptr<as2_msgs::action::NavigateToPoint::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::NavigateToPoint::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
void drone_pose_cbk(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
void follow_path_response_cbk(const rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowPath>::SharedPtr &goal_handle)
void follow_path_feedback_cbk(rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowPath>::SharedPtr goal_handle, const std::shared_ptr<const as2_msgs::action::FollowPath::Feedback> feedback)
void follow_path_result_cbk(const rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowPath>::WrappedResult &result)

Private Members

as2_msgs::msg::YawMode yaw_mode_
as2_msgs::action::NavigateToPoint::Goal goal_
as2_msgs::action::NavigateToPoint::Feedback feedback_
as2_msgs::action::NavigateToPoint::Result result_
bool enable_visualization_ = false
bool enable_path_optimizer_ = false
geometry_msgs::msg::PoseStamped drone_pose_
double safety_distance_ = 1.0
std::vector<geometry_msgs::msg::Point> path_
bool navigation_aborted_ = false
std::shared_ptr<const as2_msgs::action::FollowPath::Feedback> follow_path_feedback_
bool follow_path_rejected_ = false
bool follow_path_succeeded_ = false
std::filesystem::path plugin_name_

Path Planner Behavior plugin

std::shared_ptr<pluginlib::ClassLoader<as2_behaviors_path_planning::PluginBase>> loader_
std::shared_ptr<as2_behaviors_path_planning::PluginBase> path_planner_plugin_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr drone_pose_sub_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr viz_pub_
rclcpp_action::Client<as2_msgs::action::FollowPath>::SharedPtr follow_path_client_
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr follow_path_pause_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr follow_path_resume_client_ = nullptr
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::shared_ptr<tf2_ros::TransformListener> tf_listener_
template<typename P = double, int dim = 3>
class PID
#include <PIDController.hpp>

PID controller.

Template Parameters:

  • P – Precision

  • dim – Dimension

Public Functions

inline explicit PID(const PIDParams<P, dim> &pid_params = PIDParams<P, dim>(), const bool &verbose = false)

Construct a new PID.

Parameters:

verbose – Verbosity flag. Default: false

inline ~PID()
inline void update_pid_params(const PIDParams<P, dim> &params)

Update the PID controller with pid params.

Parameters:

params – PIDParams struct

inline void reset_controller()

Reset the controller.

Reset the integral error

inline void set_output_saturation(const Vector &upper_saturation, const Vector &lower_saturation, bool proportional_saturation_flag = false)

Set the output saturation.

Parameters:

  • upper_saturation – Upper saturation

  • lower_saturation – Lower saturation

  • proportional_saturation_flag – Proportional saturation flag. Default: false

inline void disable_output_saturation()

Disable the output saturation.

Disable the output saturation. The output is not limited by the saturation limits. To enable the output saturation, use the set_output_saturation method.

Parameters:

saturation_flag – Saturation flag

inline Vector compute_control(const Scalar dt, const Vector &proportional_error)

Process the PID controller.

Parameters:

  • dt – Time step

  • proportional_error – Proportional error

Returns:

Vector PID output

inline Vector compute_control(const Scalar dt, const Vector &proportional_error, const Vector &derivative_error)

Process the PID controller with derivative feedback.

Derivative feedback is used to improve the controller performance.

Parameters:

  • dt – Time step (s)

  • state – Current state

  • reference – Reference state

  • state_dot – Current state derivative

  • reference_dot – Reference state derivative

Returns:

Vector

inline PIDParams<P, dim> get_params() const

Get the params.

Returns:

PIDParams<P, dim> PID parameters

inline void set_gains(const Vector &kp, const Vector &ki, const Vector &kd)

Set the Gains of the controller.

Parameters:

  • kp – Proportional gain

  • ki – Integral gain

  • kd – Derivative gain

inline void get_gains(Vector &kp, Vector &ki, Vector &kd) const

Get the gains.

Parameters:

  • kp – Proportional gain

  • ki – Integral gain

  • kd – Derivative gain

inline void set_gains_kp(const Vector &kp)

Set the gains kp.

Parameters:

kp – Proportional gain

inline Vector get_gains_kp() const

Get the gains kp.

Parameters:

kp – Proportional gain

inline void set_gains_ki(const Vector &ki)

Set the gains ki.

Parameters:

ki – Integral gain

inline Vector get_gains_ki() const

Get the gains ki.

Parameters:

ki – Integral gain

inline void set_gains_kd(const Vector &kd)

Set the gains kd.

Parameters:

kd – Derivative gain

inline Vector get_gains_kd() const

Get the gains kd.

Parameters:

kd – Derivative gain

inline void set_anti_windup(const Vector &anti_windup)

Set the anti windup.

Anti windup is a vector that limits the integral contribution of the controller. If the integral contribution is greater/lower than the anti windup, the integral contribution is set to anti windup value.

Parameters:

anti_windup – Anti windup

inline Vector get_anti_windup() const

Get the anti windup.

Anti windup is a value that limits the integral contribution of the controller. If the integral contribution is greater/lower than the anti windup, the integral contribution is set to anti windup value.

Parameters:

anti_windup – Anti windup

inline void set_alpha(const Vector alpha)

Set the alpha.

Alpha is a value that filters the derivative contribution of the controller. If alpha is 1, the derivative contribution is not filtered.

Parameters:

alpha – Alpha in (0, 1]

inline Vector get_alpha() const

Get the alpha.

Alpha is a value that filters the derivative contribution of the controller. If alpha is 1, the derivative contribution is not filtered.

Parameters:

alpha – Alpha in (0, 1]

inline void set_reset_integral_saturation_flag(bool reset_integral_flag)

Set the reset integral saturation flag.

If the flag is true, the integral contribution is reset to zero when the integral error is grater than the anti windup and the sign of the integral error is different from the sign of the proportional error.

Parameters:

reset_integral_flag – Reset integral saturation flag

inline bool get_reset_integral_saturation_flag() const

Get the reset integral saturation flag.

If the flag is true, the integral contribution is reset to zero when the integral error is grater than the anti windup and the sign of the integral error is different from the sign of the proportional error.

Parameters:

reset_integral_flag – Reset integral saturation flag

inline void set_proportional_saturation_flag(bool proportional_saturation_flag)

Set the proportional saturation flag.

If the flag is true, the output is saturated proportionally to the saturation limits. So, the output vector keeps the same direction but its norm is limited to the saturation

Parameters:

proportional_saturation_flag – Proportional saturation flag

inline bool get_proportional_saturation_flag() const

Get the proportional saturation flag.

If the flag is true, the output is saturated proportionally to the saturation limits. So, the output vector keeps the same direction but its norm is limited to the saturation

Parameters:

proportional_saturation_flag – Proportional saturation flag

inline void get_saturation_limits(Vector &saturation_upper_limit, Vector &saturation_lower_limit) const

Get the saturation limits.

Parameters:

saturation_limits – Saturation limits

inline bool get_output_saturation_flag() const

Get the output saturation flag.

Returns:

true Saturation is enabled

Returns:

false Saturation is disabled

inline const Vector &get_proportional_error() const

Get the proportional error.

Returns:

Vector Proportional error

inline const Vector &get_derivative_error() const

Get the derivative error.

Returns:

Vector Derivative error

inline const Vector &get_proportional_error_contribution() const

Get the proportional error contribution.

Returns:

Vector Proportional error contribution

inline const Vector &get_integral_error_contribution() const

Get the integral error contribution.

Returns:

Vector Integral error contribution

inline const Vector &get_derivative_error_contribution() const

Get the derivative error contribution.

Returns:

Vector Derivative error contribution

inline const Vector &get_output() const

Get the output.

Returns:

Vector Output

inline explicit PID(const PIDParams<P, dim> &pid_params = PIDParams<P, dim>(), const bool &verbose = false)

Construct a new PID.

Parameters:

verbose – Verbosity flag. Default: false

inline ~PID()
inline void update_pid_params(const PIDParams<P, dim> &params)

Update the PID controller with pid params.

Parameters:

params – PIDParams struct

inline void reset_controller()

Reset the controller.

Reset the integral error

inline void set_output_saturation(const Vector &upper_saturation, const Vector &lower_saturation, bool proportional_saturation_flag = false)

Set the output saturation.

Parameters:

  • upper_saturation – Upper saturation

  • lower_saturation – Lower saturation

  • proportional_saturation_flag – Proportional saturation flag. Default: false

inline void disable_output_saturation()

Disable the output saturation.

Disable the output saturation. The output is not limited by the saturation limits. To enable the output saturation, use the set_output_saturation method.

Parameters:

saturation_flag – Saturation flag

inline Vector compute_control(const Scalar dt, const Vector &proportional_error)

Process the PID controller.

Parameters:

  • dt – Time step

  • proportional_error – Proportional error

Returns:

Vector PID output

inline Vector compute_control(const Scalar dt, const Vector &proportional_error, const Vector &derivative_error)

Process the PID controller with derivative feedback.

Derivative feedback is used to improve the controller performance.

Parameters:

  • dt – Time step (s)

  • state – Current state

  • reference – Reference state

  • state_dot – Current state derivative

  • reference_dot – Reference state derivative

Returns:

Vector

inline PIDParams<P, dim> get_params() const

Get the params.

Returns:

PIDParams<P, dim> PID parameters

inline void set_gains(const Vector &kp, const Vector &ki, const Vector &kd)

Set the Gains of the controller.

Parameters:

  • kp – Proportional gain

  • ki – Integral gain

  • kd – Derivative gain

inline void get_gains(Vector &kp, Vector &ki, Vector &kd) const

Get the gains.

Parameters:

  • kp – Proportional gain

  • ki – Integral gain

  • kd – Derivative gain

inline void set_gains_kp(const Vector &kp)

Set the gains kp.

Parameters:

kp – Proportional gain

inline Vector get_gains_kp() const

Get the gains kp.

Parameters:

kp – Proportional gain

inline void set_gains_ki(const Vector &ki)

Set the gains ki.

Parameters:

ki – Integral gain

inline Vector get_gains_ki() const

Get the gains ki.

Parameters:

ki – Integral gain

inline void set_gains_kd(const Vector &kd)

Set the gains kd.

Parameters:

kd – Derivative gain

inline Vector get_gains_kd() const

Get the gains kd.

Parameters:

kd – Derivative gain

inline void set_anti_windup(const Vector &anti_windup)

Set the anti windup.

Anti windup is a vector that limits the integral contribution of the controller. If the integral contribution is greater/lower than the anti windup, the integral contribution is set to anti windup value.

Parameters:

anti_windup – Anti windup

inline Vector get_anti_windup() const

Get the anti windup.

Anti windup is a value that limits the integral contribution of the controller. If the integral contribution is greater/lower than the anti windup, the integral contribution is set to anti windup value.

Parameters:

anti_windup – Anti windup

inline void set_alpha(const Vector alpha)

Set the alpha.

Alpha is a value that filters the derivative contribution of the controller. If alpha is 1, the derivative contribution is not filtered.

Parameters:

alpha – Alpha in (0, 1]

inline Vector get_alpha() const

Get the alpha.

Alpha is a value that filters the derivative contribution of the controller. If alpha is 1, the derivative contribution is not filtered.

Parameters:

alpha – Alpha in (0, 1]

inline void set_reset_integral_saturation_flag(bool reset_integral_flag)

Set the reset integral saturation flag.

If the flag is true, the integral contribution is reset to zero when the integral error is grater than the anti windup and the sign of the integral error is different from the sign of the proportional error.

Parameters:

reset_integral_flag – Reset integral saturation flag

inline bool get_reset_integral_saturation_flag() const

Get the reset integral saturation flag.

If the flag is true, the integral contribution is reset to zero when the integral error is grater than the anti windup and the sign of the integral error is different from the sign of the proportional error.

Parameters:

reset_integral_flag – Reset integral saturation flag

inline void set_proportional_saturation_flag(bool proportional_saturation_flag)

Set the proportional saturation flag.

If the flag is true, the output is saturated proportionally to the saturation limits. So, the output vector keeps the same direction but its norm is limited to the saturation

Parameters:

proportional_saturation_flag – Proportional saturation flag

inline bool get_proportional_saturation_flag() const

Get the proportional saturation flag.

If the flag is true, the output is saturated proportionally to the saturation limits. So, the output vector keeps the same direction but its norm is limited to the saturation

Parameters:

proportional_saturation_flag – Proportional saturation flag

inline void get_saturation_limits(Vector &saturation_upper_limit, Vector &saturation_lower_limit) const

Get the saturation limits.

Parameters:

saturation_limits – Saturation limits

inline bool get_output_saturation_flag() const

Get the output saturation flag.

Returns:

true Saturation is enabled

Returns:

false Saturation is disabled

inline const Vector &get_proportional_error() const

Get the proportional error.

Returns:

Vector Proportional error

inline const Vector &get_derivative_error() const

Get the derivative error.

Returns:

Vector Derivative error

inline const Vector &get_proportional_error_contribution() const

Get the proportional error contribution.

Returns:

Vector Proportional error contribution

inline const Vector &get_integral_error_contribution() const

Get the integral error contribution.

Returns:

Vector Integral error contribution

inline const Vector &get_derivative_error_contribution() const

Get the derivative error contribution.

Returns:

Vector Derivative error contribution

inline const Vector &get_output() const

Get the output.

Returns:

Vector Output

Public Static Functions

static inline Vector get_error(const Vector &state, const Vector &reference)

Get the proportional error.

Parameters:

  • state – Current state

  • reference – Reference state

Returns:

Vector Error

static inline void get_error(const Vector &state, const Vector &reference, const Vector &state_dot, const Vector &reference_dot, Vector &proportional_error, Vector &derivative_error)

Get the proportional and derivative error.

Parameters:

  • state – State

  • reference – Reference

  • state_dot – State derivative

  • reference_dot – Reference derivative

  • proportional_error – Output proportional error

  • derivative_error – Output derivative error

static inline Vector saturate_output(const Vector &output, const Vector &upper_limits, const Vector &lower_limits, const bool proportional_saturation = false)

Saturation function.

If the output is greater than the upper limit, the output is saturated to the upper limit. If the output is lower than the lower limit, the output is saturated to the lower limit. If proportional_saturation is true, the output is saturated proportionally to the limits, keeping the vector direction.

Parameters:

  • output – Vector to saturate

  • upper_limits – Upper limits vector

  • lower_limits – Lower limits vector

  • proportional_saturation – Proportional saturation flag. Default: false

Returns:

Vector Saturated vector

static inline Vector get_error(const Vector &state, const Vector &reference)

Get the proportional error.

Parameters:

  • state – Current state

  • reference – Reference state

Returns:

Vector Error

static inline void get_error(const Vector &state, const Vector &reference, const Vector &state_dot, const Vector &reference_dot, Vector &proportional_error, Vector &derivative_error)

Get the proportional and derivative error.

Parameters:

  • state – State

  • reference – Reference

  • state_dot – State derivative

  • reference_dot – Reference derivative

  • proportional_error – Output proportional error

  • derivative_error – Output derivative error

static inline Vector saturate_output(const Vector &output, const Vector &upper_limits, const Vector &lower_limits, const bool proportional_saturation = false)

Saturation function.

If the output is greater than the upper limit, the output is saturated to the upper limit. If the output is lower than the lower limit, the output is saturated to the lower limit. If proportional_saturation is true, the output is saturated proportionally to the limits, keeping the vector direction.

Parameters:

  • output – Vector to saturate

  • upper_limits – Upper limits vector

  • lower_limits – Lower limits vector

  • proportional_saturation – Proportional saturation flag. Default: false

Returns:

Vector Saturated vector

Protected Functions

inline Vector compute_integral_contribution(const Scalar dt, const Vector &proportional_error)

Compute the integral contribution of the controller.

Parameters:

  • dt – Delta time (s)

  • proportional_error – Proportional error

Returns:

Vector Integral contribution

inline Vector compute_derivative_contribution_by_deriving(const Scalar dt, const Vector &proportional_error)

Compute the derivative contribution of the controller.

Parameters:

  • dt – Delta time (s)

  • proportional_error – Proportional error

Returns:

Vector Derivative contribution

inline Vector compute_derivative_contribution(const Vector &derivate_error)

Compute the derivative contribution of the controller.

For controllers with derivative feedback, the derivative contribution is computed using the state and reference derivatives.

Parameters:

  • state_dot – State derivative

  • reference_dot – Reference derivative

Returns:

Vector Derivative contribution

inline Vector compute_integral_contribution(const Scalar dt, const Vector &proportional_error)

Compute the integral contribution of the controller.

Parameters:

  • dt – Delta time (s)

  • proportional_error – Proportional error

Returns:

Vector Integral contribution

inline Vector compute_derivative_contribution_by_deriving(const Scalar dt, const Vector &proportional_error)

Compute the derivative contribution of the controller.

Parameters:

  • dt – Delta time (s)

  • proportional_error – Proportional error

Returns:

Vector Derivative contribution

inline Vector compute_derivative_contribution(const Vector &derivate_error)

Compute the derivative contribution of the controller.

For controllers with derivative feedback, the derivative contribution is computed using the state and reference derivatives.

Parameters:

  • state_dot – State derivative

  • reference_dot – Reference derivative

Returns:

Vector Derivative contribution

Protected Attributes

bool verbose_ = false
Matrix Kp_lin_mat_ = Matrix::Identity()
Matrix Ki_lin_mat_ = Matrix::Identity()
Matrix Kd_lin_mat_ = Matrix::Identity()
Vector antiwindup_cte_ = Vector::Zero()
Vector alpha_ = Vector::Zero()
bool reset_integral_flag_ = false
bool saturation_flag_ = false
bool proportional_saturation_flag_ = false
Vector upper_output_saturation_ = Vector::Zero()
Vector lower_output_saturation_ = Vector::Zero()
bool first_run_ = true
Vector integral_accum_error_ = Vector::Zero()
Vector filtered_derivate_error_ = Vector::Zero()
Vector proportional_error_ = Vector::Zero()
Vector derivative_error_ = Vector::Zero()
Vector proportional_error_contribution_ = Vector::Zero()
Vector integral_error_contribution_ = Vector::Zero()
Vector derivate_error_contribution_ = Vector::Zero()
Vector output_ = Vector::Zero()

Private Types

using Scalar = P
using Vector = Eigen::Matrix<P, dim, 1>
using Matrix = Eigen::Matrix<P, dim, dim>
using Scalar = P
using Vector = Eigen::Matrix<P, dim, 1>
using Matrix = Eigen::Matrix<P, dim, dim>
template<typename P = double, int dim = 3>
struct PIDParams

Public Types

using Vector = Eigen::Matrix<P, dim, 1>
using Matrix = Eigen::Matrix<P, dim, dim>
using Vector = Eigen::Matrix<P, dim, 1>
using Matrix = Eigen::Matrix<P, dim, dim>

Public Members

Vector Kp_gains = Vector::Zero()
Vector Ki_gains = Vector::Zero()
Vector Kd_gains = Vector::Zero()
Vector antiwindup_cte = Vector::Zero()
Vector alpha = Vector::Ones()
bool reset_integral_flag = false
bool proportional_saturation_flag = false
Vector upper_output_saturation = Vector::Zero()
Vector lower_output_saturation = Vector::Zero()
struct PlatformParams

Public Members

double update_freq = 1000.0
double control_freq = 100.0
double inertial_odometry_freq = 1000.0
double state_freq = 100.0
double imu_pub_freq = 100.0
double odometry_pub_freq = 100.0
double ground_truth_pub_freq = 100.0
double gps_pub_freq = 100.0
double gimbal_pub_freq = 100.0
double latitude
double longitude
double altitude
class PlatformStateMachine
#include <platform_state_machine.hpp>

This class implements the Platform State Machine, which is in charge of handling the state of the platform using a FSM (Finite State Machine). This state machine consist on 6 states:

  • DISARMED -> The platform is not armed.

  • LANDED -> The platform is armed and landed.

  • TAKING_OFF -> The platform is taking off.

  • FLYING -> The platform is on air.

  • LANDING -> The platform is landing.

  • EMERGENCY -> The platform is in emergency mode.

The events that can trigger the state machine are:

  • ARM

  • DISARM

  • TAKE_OFF

  • TOOK_OFF

  • LAND

  • LANDED

  • EMERGENCY TODO(miferco97): add figure of the state machine ../../../../../_images/test.jpg

Public Functions

explicit PlatformStateMachine(as2::Node *node)

Constructor of the Platform State Machine.

Parameters:

node_ptr – Pointer to an aerostack2 node.

~PlatformStateMachine()
bool processEvent(const int8_t &event)

This function is in charge of handling the state machine.

Parameters:

event – The event that triggers the state machine.

Returns:

true If the event is valid in current State.

bool processEvent(const Event &event)

This function is in charge of handling the state machine.

Parameters:

event – The event that triggers the state machine.

Returns:

true If the event is valid in current State.

StateMachineTransition getTransition(const int8_t &current_state, const int8_t &event)

Get the Transition object.

Parameters:

  • current_state –

  • event –

Returns:

StateMachineTransition

inline as2_msgs::msg::PlatformStatus getState()

This function returns the current state of the state machine.

Returns:

The current Platform Status of the state machine

inline void setState(as2_msgs::msg::PlatformStatus state)

Set the State of the FSM to the desired state. (THIS MAY BE USED ONLY FOR TESTING PURPOSES)

Parameters:

state –

inline void setState(const int8_t &state)

Private Functions

void defineTransitions()

in this function the state machine is created based on the transitions. its called in the constructor of the class.

void setStateMachineEventSrvCallback(const std::shared_ptr<as2_msgs::srv::SetPlatformStateMachineEvent::Request> request, std::shared_ptr<as2_msgs::srv::SetPlatformStateMachineEvent::Response> response)
std::string eventToString(int8_t event)
std::string stateToString(int8_t state)

Private Members

std::vector<StateMachineTransition> transitions_
as2_msgs::msg::PlatformStatus state_
as2::Node *node_ptr_
rclcpp::Service<as2_msgs::srv::SetPlatformStateMachineEvent>::SharedPtr state_machine_event_srv_
class Plugin : public as2_behaviors_path_planning::PluginBase

Public Functions

virtual void initialize(as2::Node *node_ptr, std::shared_ptr<tf2_ros::Buffer> tf_buffer) override
virtual bool on_activate(geometry_msgs::msg::PoseStamped drone_pose, as2_msgs::action::NavigateToPoint::Goal goal) override
virtual bool on_deactivate() override
virtual bool on_modify() override
virtual bool on_pause() override
virtual bool on_resume() override
virtual void on_execution_end() override
virtual as2_behavior::ExecutionStatus on_run() override

Private Functions

void occ_grid_cbk(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
visualization_msgs::msg::Marker get_path_marker(std::string frame_id, rclcpp::Time stamp, std::vector<Point2i> path, nav_msgs::msg::MapMetaData map_info, std_msgs::msg::Header map_header)

Private Members

AStarSearcher a_star_searcher_
nav_msgs::msg::OccupancyGrid last_occ_grid_
double safety_distance_
bool enable_path_optimizer_
bool enable_visualization_
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_grid_sub_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr viz_pub_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr viz_obstacle_grid_pub_
class Plugin : public gripper_behavior_plugin_base::GripperBase

Public Functions

virtual void ownInit() override
virtual bool own_activate(as2_msgs::action::GripperHandler::Goal &_goal) override
virtual bool own_modify(as2_msgs::action::GripperHandler::Goal &_goal) override
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool own_pause(const std::shared_ptr<std::string> &message) override
virtual bool own_resume(const std::shared_ptr<std::string> &message) override
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

void closeGripper()
void openGripper()
double angle_to_pwm(double angle)

Private Members

rclcpp::CallbackGroup::SharedPtr cbk_group_
rclcpp::PublisherOptions pub_options
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_pwm_
double angle_to_open_
double angle_to_close_
double max_angle_
double duty_min_
double duty_max_
std::string topic_pwm_
class Plugin : public as2_motion_controller_plugin_base::ControllerBase
#include <differential_flatness_controller.hpp>

Differential-flatness controller plugin.

Public Functions

inline Plugin()
inline ~Plugin()
virtual void ownInitialize() override

Plugin-specific initialization, called from initialize().

virtual std::vector<std::string> getEssentialParameters() const override

Names of the parameters required before the plugin can accept setMode.

Returns:

Vector of fully-qualified essential parameter names.

virtual void updateParameter(const rclcpp::Parameter &parameter) override

Apply a single parameter to the plugin.

Routes the value to the differential-flatness gain matrices and the mass/antiwindup scalars.

Parameters:

parameter – Parameter to apply.

virtual void reset() override

Reset the cached state, references and commands.

Calls ControllerBase::reset() to clear the base flags. The essentialParamsReady() latch is intentionally preserved.

virtual bool setMode(const as2_msgs::msg::ControlMode &mode_in, const as2_msgs::msg::ControlMode &mode_out) override

Update the control mode to be used by the controller plugin.

Only the TRAJECTORY input mode is accepted by the differential-flatness controller.

Parameters:

  • mode_in – Input control mode requested.

  • mode_out – Output control mode requested.

Returns:

true if the in-out control mode configuration is valid.

virtual void onUpdateState(const geometry_msgs::msg::PoseStamped &pose_msg, const geometry_msgs::msg::TwistStamped &twist_msg) override

Plugin hook called by the base after frame validation and hover latch.

Caches the position, velocity and attitude used by the controller.

Parameters:

  • pose_msg – Latest validated pose message.

  • twist_msg – Latest validated twist message.

virtual void onUpdateReference(const as2_msgs::msg::TrajectorySetpoints &ref) override

Plugin hook for trajectory reference.

Parameters:

ref – Latest trajectory reference message.

virtual bool computeOutput(double dt, geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::TwistStamped &twist, as2_msgs::msg::Thrust &thrust) override

Compute the output signal of the controller plugin.

Solves the differential-flatness law and packs the resulting ACRO command into the twist (body rates) and thrust output messages.

Parameters:

  • dt – Time elapsed since the last call to computeOutput().

  • pose – Output pose (unused by this plugin).

  • twist – Output twist with the body rates in base_link.

  • thrust – Output collective thrust.

Returns:

true if the output is valid.

Private Functions

void updateDFParameter(const std::string &_parameter_name, const rclcpp::Parameter &_param)

Apply a parameter change to the differential-flatness gains.

Parameters:

  • _parameter_name – Tail name of the parameter (without plugin namespace).

  • _param – New parameter value.

inline void resetState()

Reset the cached UAV state.

void resetReferences()

Reset the cached references.

void resetCommands()

Reset the cached commands.

Eigen::Vector3d getForce(const double &_dt, const Eigen::Vector3d &_pos_state, const Eigen::Vector3d &_vel_state, const Eigen::Vector3d &_pos_reference, const Eigen::Vector3d &_vel_reference, const Eigen::Vector3d &_acc_reference)

Compute the desired force from position/velocity tracking errors.

Parameters:

  • _dt – Time elapsed since the previous call, in seconds.

  • _pos_state – Current position.

  • _vel_state – Current linear velocity.

  • _pos_reference – Reference position.

  • _vel_reference – Reference linear velocity.

  • _acc_reference – Reference linear acceleration (feed-forward).

Returns:

Desired force expressed in the inertial frame.

Acro_command computeTrajectoryControl(const double &_dt, const Eigen::Vector3d &_pos_state, const Eigen::Vector3d &_vel_state, const tf2::Quaternion &_attitude_state, const Eigen::Vector3d &_pos_reference, const Eigen::Vector3d &_vel_reference, const Eigen::Vector3d &_acc_reference, const double &_yaw_angle_reference)

Compute the ACRO command for trajectory tracking.

Parameters:

  • _dt – Time elapsed since the previous call, in seconds.

  • _pos_state – Current position.

  • _vel_state – Current linear velocity.

  • _attitude_state – Current attitude quaternion.

  • _pos_reference – Reference position.

  • _vel_reference – Reference linear velocity.

  • _acc_reference – Reference linear acceleration (feed-forward).

  • _yaw_angle_reference – Reference yaw angle.

Returns:

ACRO command (body rates + thrust).

bool getOutput(geometry_msgs::msg::TwistStamped &twist_msg, as2_msgs::msg::Thrust &thrust_msg)

Pack the latest ACRO command into the output messages.

Parameters:

  • twist_msg – Output twist message (body rates).

  • thrust_msg – Output thrust message.

Returns:

true if the command was successfully packed.

Private Members

UAV_state uav_state_
UAV_reference control_ref_
Acro_command control_command_
as2_msgs::msg::ControlMode control_mode_in_
as2_msgs::msg::ControlMode control_mode_out_
Eigen::Matrix3d Kp_ = {Eigen::Matrix3d::Zero()}
Eigen::Matrix3d Kd_ = {Eigen::Matrix3d::Zero()}
Eigen::Matrix3d Ki_ = {Eigen::Matrix3d::Zero()}
Eigen::Matrix3d Kp_ang_mat_ = {Eigen::Matrix3d::Zero()}
Eigen::Vector3d accum_pos_error_ = {Eigen::Vector3d::Zero()}
double mass_
double antiwindup_cte_ = 0.0
const Eigen::Vector3d gravitational_accel_ = Eigen::Vector3d(0, 0, -9.81)
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr debug_desired_velocity_pub_
const std::vector<std::string> parameters_tail_ = {"mass", "trajectory_control.antiwindup_cte", "trajectory_control.alpha", "trajectory_control.kp.x", "trajectory_control.kp.y", "trajectory_control.kp.z", "trajectory_control.ki.x", "trajectory_control.ki.y", "trajectory_control.ki.z", "trajectory_control.kd.x", "trajectory_control.kd.y", "trajectory_control.kd.z", "trajectory_control.roll_control.kp", "trajectory_control.pitch_control.kp", "trajectory_control.yaw_control.kp",}
class Plugin : public generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase
#include <dynamic_mav_trajectory_generator.hpp>

Plugin wrapper for DynamicTrajectory backend.

Public Functions

Plugin()

Construct plugin instance.

~Plugin() override = default

Virtual destructor.

bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) override

Generate trajectory from waypoint list.

The starting position is read from vehicle_pose_ (refreshed by the host) and pushed into the backend before scheduling the new trajectory. The DynamicTrajectory backend forces zero velocity at the starting waypoint when generating from scratch, so the live vehicle_twist_ is intentionally ignored on this path; smooth velocity matching is only available through updateWaypoints() after a previous trajectory has been generated.

Anchors the internal time-axis offset so that the host’s t_trajectory_now maps to the backend’s getMinTime().

Parameters:

  • waypoints – Mission waypoints (no synthetic “current” entry).

  • max_speed – Maximum speed in m/s.

  • t_trajectory_now – Current host trajectory time (seconds).

Returns:

true when generation succeeds.

bool evaluate(double t_trajectory, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample) override

Evaluate references at host trajectory time t_trajectory.

Reads the live vehicle pose from the protected base member (vehicle_pose_) so the dynamic backend can close the loop on the actual vehicle state. The injection is gated on is_horizon_sample being false: for horizon predictions and debug sampling the call is non-mutating (for_plotting=true).

Parameters:

  • t_trajectory – Evaluation time in the host’s trajectory axis.

  • out – Output trajectory point.

  • is_horizon_sample – True for horizon predictions and debug sampling; false for the live control setpoint.

Returns:

true when evaluation succeeds.

bool isFinished(double t_trajectory) const override

Whether the trajectory is exhausted at the given t_trajectory.

Parameters:

t_trajectory – Current host trajectory time in seconds.

Returns:

true when the trajectory has finished.

double getDuration() const override
bool isTrajectoryGenerated() override

Check if a trajectory is currently available.

Returns:

true when trajectory was generated.

void reset() override

Reset trajectory generator state.

bool updateWaypoints(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) override

Update the active trajectory using the backend stitching logic.

Overrides the base default (regenerate) to leverage DynamicTrajectory’s automatic stitching, which evaluates the existing trajectory at the current time and uses position + velocity + acceleration of that sample as boundary conditions for the new trajectory. The backend keeps its internal time origin so the plugin’s internal_offset_ is preserved unchanged and the host’s t_trajectory keeps mapping to the same backend trajectory.

Parameters:

  • waypoints – Updated pending waypoints (mission only).

  • max_speed – Maximum cruise speed in m/s.

  • t_trajectory_now – Current host trajectory time (unused on the stitching path, but kept for API symmetry).

Returns:

true when the backend accepts the update request.

bool consumeRegeneratedFlag() override

Consume and return the backend’s “trajectory regenerated” flag.

Delegates to DynamicTrajectory::getWasTrajectoryRegenerated(), which is set to true by swapTrajectory() (the deferred event triggered by an asynchronous updateWaypoints()) and consumed (reset to false) on read.

Returns:

true at most once per asynchronous regeneration event.

std::string getNextWaypointId() override

Get the id of the next pending mission waypoint.

Auxiliary backend waypoints (e.g. stitching points) are filtered out.

Returns:

Empty string when no pending waypoint, else its id.

Protected Functions

void ownInitialize() override

Optional plugin-specific initialization hook.

Private Functions

void pushVehiclePositionToGenerator(const geometry_msgs::msg::PoseStamped &pose)

Push vehicle position into generator state.

Parameters:

pose – Current vehicle pose.

std::shared_ptr<dynamic_traj_generator::DynamicTrajectory> makeTrajectoryGenerator() const

Build a fresh DynamicTrajectory.

Returns:

A new generator with the low-speed factors set.

Private Members

std::shared_ptr<dynamic_traj_generator::DynamicTrajectory> trajectory_generator_
double ls_velocity_factor_ = {1.0}
double ls_acceleration_factor_ = {1.0}
double internal_offset_ = {0.0}

Private Static Functions

static dynamic_traj_generator::DynamicWaypoint::Deque toDynamicDeque(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints)

Convert AS2 waypoints to dynamic generator deque.

Parameters:

waypoints – Input waypoint list.

Returns:

Converted dynamic waypoint deque.

class Plugin : public follow_path_base::FollowPathBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::FollowPath::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::FollowPath::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline virtual Eigen::Vector3d getTargetPosition() override
inline geometry_msgs::msg::Quaternion processYaw(as2_msgs::action::FollowPath::Goal &_goal, const std::string &id)
inline void updateDesiredPose(as2_msgs::action::FollowPath::Goal &_goal, const std::string &waypoint_id)

Private Functions

inline bool checkGoalCondition()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr
std::vector<std::string> path_ids_
std::vector<std::string> path_ids_remaining_
double initial_yaw_
geometry_msgs::msg::PoseStamped desired_pose_
geometry_msgs::msg::TwistStamped desired_twist_
class Plugin : public follow_path_base::FollowPathBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::FollowPath::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::FollowPath::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline virtual Eigen::Vector3d getTargetPosition() override
inline void feedback_callback(GoalHandleTrajectoryGenerator::SharedPtr, const std::shared_ptr<const TrajectoryGeneratorAction::Feedback> feedback)
inline void result_callback(const GoalHandleTrajectoryGenerator::WrappedResult &result)

Private Types

using TrajectoryGeneratorAction = as2_msgs::action::GeneratePolynomialTrajectory
using GoalHandleTrajectoryGenerator = rclcpp_action::ClientGoalHandle<TrajectoryGeneratorAction>

Private Functions

inline as2_msgs::action::GeneratePolynomialTrajectory::Goal followPathGoalToTrajectoryGeneratorGoal(const as2_msgs::action::FollowPath::Goal &_goal)

Private Members

std::shared_ptr<rclcpp_action::Client<TrajectoryGeneratorAction>> traj_gen_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_pause_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_resume_client_ = nullptr
rclcpp_action::Client<TrajectoryGeneratorAction>::SendGoalOptions traj_gen_goal_options_
std::shared_future<GoalHandleTrajectoryGenerator::SharedPtr> traj_gen_goal_handle_future_
bool traj_gen_goal_accepted_ = false
bool traj_gen_result_received_ = false
bool traj_gen_result_ = false
geometry_msgs::msg::Pose desired_pose_
class Plugin : public follow_reference_base::FollowReferenceBase

Public Functions

inline virtual void ownInit() override
inline virtual bool own_activate(as2_msgs::action::FollowReference::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::FollowReference::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline float getActualYaw()
inline bool computeYaw(const uint8_t yaw_mode, const geometry_msgs::msg::Point &target, const geometry_msgs::msg::Point &actual, float &yaw)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr
class Plugin : public follow_reference_base::FollowReferenceBase

Public Functions

inline virtual void ownInit() override
inline virtual bool own_activate(as2_msgs::action::FollowReference::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::FollowReference::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline void feedback_callback(GoalHandleTrajectoryGenerator::SharedPtr, const std::shared_ptr<const TrajectoryGeneratorAction::Feedback> feedback)
inline void result_callback(const GoalHandleTrajectoryGenerator::WrappedResult &result)

Private Types

using TrajectoryGeneratorAction = as2_msgs::action::GeneratePolynomialTrajectory
using GoalHandleTrajectoryGenerator = rclcpp_action::ClientGoalHandle<TrajectoryGeneratorAction>

Private Functions

inline double declareAndGetDouble(const std::string &param_name, double default_value)
inline bool tryConvertTargetToEarth(const geometry_msgs::msg::PointStamped &target, geometry_msgs::msg::PointStamped &out)
inline void publishModifyInEarth(const geometry_msgs::msg::PointStamped &target_in_earth)
inline void reactiveModifyTick()
inline TrajectoryGeneratorAction::Goal followReferenceGoalToTrajectoryGoal(const as2_msgs::action::FollowReference::Goal &_goal, const geometry_msgs::msg::PointStamped &target_in_earth)

Private Members

std::shared_ptr<rclcpp_action::Client<TrajectoryGeneratorAction>> traj_gen_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_pause_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_resume_client_ = nullptr
rclcpp::Publisher<as2_msgs::msg::PoseStampedWithIDArray>::SharedPtr modify_pub_ = nullptr
rclcpp_action::Client<TrajectoryGeneratorAction>::SendGoalOptions traj_gen_goal_options_
std::shared_future<GoalHandleTrajectoryGenerator::SharedPtr> traj_gen_goal_handle_future_
TrajectoryGeneratorAction::Feedback traj_gen_feedback_
bool traj_gen_goal_accepted_ = false
bool traj_gen_result_received_ = false
bool traj_gen_result_ = false
bool cancel_requested_ = false
double modify_threshold_ = 0.0
double modify_frequency_ = 0.0
geometry_msgs::msg::PointStamped last_target_in_earth_
rclcpp::Time last_modify_time_
class Plugin : public generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase
#include <gcopter_trajectory_generator.hpp>

Plugin wrapper for gcopter_lib::TrajectoryGenerator backend.

The optimizer is offline and produces a piecewise polynomial spline that can be evaluated continuously. The plugin does not override updateWaypoints(): each modify request goes through the base class default, which calls reset() + generateTrajectory() with the live vehicle pose and twist as boundary conditions, so the resulting trajectory is C1-continuous with the drone state at the time of the re-plan.

Public Functions

Plugin()
~Plugin() override = default
bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) override
bool evaluate(double t_trajectory, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample) override
bool isFinished(double t_trajectory) const override
double getDuration() const override
bool isTrajectoryGenerated() override
void reset() override
std::string getNextWaypointId() override

Public Static Functions

static ExpandedPath expandPathWithAnchors(const std::vector<Eigen::Vector3d> &waypoints, double loose_margin, double tight_margin, double anchor_radius)

Insert anchor points around intermediate waypoints.

For N >= 3 input waypoints, places one anchor at distance min(anchor_radius, 0.45 * leg_length) before and after each intermediate waypoint along the entry/exit directions. The two pinch segments that touch each waypoint receive tight_margin while the rest of the corridor uses loose_margin. For coincident consecutive waypoints the anchor is skipped (no useful direction). For N < 3 or when anchoring is disabled, returns the original waypoints with a uniform loose_margin per segment.

Exposed publicly to keep it unit-testable without instantiating the full ROS plugin.

Parameters:

  • waypoints – Ordered waypoint positions [m].

  • loose_margin – Default AABB half-margin per face [m].

  • tight_margin – Half-margin for the pinch segments [m]. <=0 disables anchoring.

  • anchor_radius – Offset of each anchor along the path [m]. <=0 disables anchoring.

Returns:

Expanded path with per-segment margins and a mapping back to the original waypoint indices.

Protected Functions

void ownInitialize() override

Private Functions

void readConfigParameters()

Private Members

gcopter_lib::GeneratorConfig generator_config_
std::unique_ptr<gcopter_lib::TrajectoryGenerator> trajectory_generator_
double waypoint_margin_ = {0.1}
double waypoint_anchor_radius_ = {0.5}
std::vector<std::pair<std::string, double>> waypoint_arrival_times_
double last_eval_backend_time_ = {0.0}
double internal_offset_ = {0.0}

Private Static Functions

static gcopter_lib::Waypoint toGcopterWaypoint(const as2_msgs::msg::PoseStampedWithID &wp)
class Plugin : public go_to_base::GoToBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::GoToWaypoint::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::GoToWaypoint::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline bool checkGoalCondition()
inline float getActualYaw()
inline bool computeYaw(const uint8_t yaw_mode, const geometry_msgs::msg::Point &target, const geometry_msgs::msg::Point &actual, float &yaw)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr
class Plugin : public go_to_base::GoToBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::GoToWaypoint::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::GoToWaypoint::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline void feedback_callback(GoalHandleTrajectoryGenerator::SharedPtr, const std::shared_ptr<const TrajectoryGeneratorAction::Feedback> feedback)
inline void result_callback(const GoalHandleTrajectoryGenerator::WrappedResult &result)

Private Types

using TrajectoryGeneratorAction = as2_msgs::action::GeneratePolynomialTrajectory
using GoalHandleTrajectoryGenerator = rclcpp_action::ClientGoalHandle<TrajectoryGeneratorAction>

Private Functions

inline as2_msgs::action::GeneratePolynomialTrajectory::Goal goToGoalToTrajectoryGeneratorGoal(const as2_msgs::action::GoToWaypoint::Goal &_goal)

Private Members

std::shared_ptr<rclcpp_action::Client<TrajectoryGeneratorAction>> traj_gen_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_pause_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_resume_client_ = nullptr
rclcpp_action::Client<TrajectoryGeneratorAction>::SendGoalOptions traj_gen_goal_options_
std::shared_future<GoalHandleTrajectoryGenerator::SharedPtr> traj_gen_goal_handle_future_
TrajectoryGeneratorAction::Feedback traj_gen_feedback_
bool traj_gen_goal_accepted_ = false
bool traj_gen_result_received_ = false
bool traj_gen_result_ = false
class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase

Public Functions

inline Plugin()
inline virtual void on_setup() override

Private Functions

inline void generate_map_frame_from_gps(const geographic_msgs::msg::GeoPoint &origin, const sensor_msgs::msg::NavSatFix &gps_pose)
inline void generate_map_frame_from_ground_truth_pose(const geometry_msgs::msg::PoseStamped &pose)
inline void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
inline void twist_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
inline void getOriginCallback(const as2_msgs::srv::GetOrigin::Request::SharedPtr request, as2_msgs::srv::GetOrigin::Response::SharedPtr response)
inline void setOriginCallback(const as2_msgs::srv::SetOrigin::Request::SharedPtr request, as2_msgs::srv::SetOrigin::Response::SharedPtr response)
inline void gps_callback(sensor_msgs::msg::NavSatFix::UniquePtr msg)

Private Members

rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_
rclcpp::Service<as2_msgs::srv::SetOrigin>::SharedPtr set_origin_srv_
rclcpp::Service<as2_msgs::srv::GetOrigin>::SharedPtr get_origin_srv_
bool use_gps_ = false
bool set_origin_on_start_ = false
bool earth_to_map_set_ = false
double origin_lat_ = 0.0
double origin_lon_ = 0.0
double origin_alt_ = 0.0
geometry_msgs::msg::TransformStamped earth_to_map_
geographic_msgs::msg::GeoPoint::UniquePtr origin_
sensor_msgs::msg::NavSatFix::UniquePtr gps_pose_
bool using_gazebo_tf_ = false
class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase

Public Functions

inline Plugin()
inline virtual void on_setup() override

Private Functions

inline geometry_msgs::msg::TransformStamped getTransform(const nav_msgs::msg::Odometry &odom, const geometry_msgs::msg::PoseStamped &ground_truth)

Computes the transformation from the map frame to the odometry frame using Eigen::Isometry3d.

Parameters:

  • odom – The odometry data.

  • ground_truth – The ground truth pose data.

Returns:

geometry_msgs::msg::TransformStamped The transformation from the map to odom frame.

inline void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)

Callback for the odometry topic.

Parameters:

msg – Odometry message

inline void groundTruthCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)

Callback for the ground truth topic.

Parameters:

msg – PoseStamped message

inline void rigidBodiesCallback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr msg)

Callback for the rigid bodies topic.

Parameters:

msg – RigidBodies message

Private Members

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr ground_truth_sub_
rclcpp::Subscription<mocap4r2_msgs::msg::RigidBodies>::SharedPtr rigid_bodies_sub_
geometry_msgs::msg::PoseStamped::SharedPtr ground_truth_ = nullptr
nav_msgs::msg::Odometry::SharedPtr odom_ = nullptr
std::string rigid_body_name_ = ""
geometry_msgs::msg::TransformStamped earth_to_map_
geometry_msgs::msg::TransformStamped map_to_odom_
geometry_msgs::msg::TransformStamped odom_to_baselink_
bool map_to_earth_set_ = false
class Plugin : public generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase
#include <jerk_limited_trajectory_generator.hpp>

Plugin wrapper for trajectory_generator_jerk_limited backend.

The optimizer is a one-shot jerk-limited S-curve simulator that emits a piecewise polynomial spline plus an exact in-memory sample sequence. The plugin does not override updateWaypoints(): each modify request goes through the base class default, which calls reset() + generateTrajectory() with the live vehicle pose and twist as boundary conditions, so the resulting trajectory is C^1-continuous with the drone state at the time of the re-plan.

The initial acceleration boundary condition is left at zero because the host behavior does not expose a vehicle acceleration estimate; in practice this only relaxes the smoothness of the splice from C^2 to C^1 and the controller absorbs the residual jerk transient.

Public Functions

Plugin()
~Plugin() override = default
bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) override
bool evaluate(double t_trajectory, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample) override
bool isFinished(double t_trajectory) const override
double getDuration() const override
bool isTrajectoryGenerated() override
void reset() override
std::string getNextWaypointId() override

Protected Functions

void ownInitialize() override

Private Functions

void readConfigParameters()

Private Members

trajectory_generator_jerk_limited::GeneratorConfig generator_config_
std::unique_ptr<trajectory_generator_jerk_limited::TrajectoryGenerator> trajectory_generator_
std::vector<std::pair<std::string, double>> waypoint_arrival_times_
double last_eval_backend_time_ = {0.0}
double internal_offset_ = {0.0}

Private Static Functions

static trajectory_generator_jerk_limited::Waypoint toJlWaypoint(const as2_msgs::msg::PoseStampedWithID &wp)
class Plugin : public land_base::LandBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Members

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr platform_land_cli_
std_srvs::srv::SetBool::Request::SharedPtr platform_land_request_
rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture platform_land_future_
class Plugin : public land_base::LandBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline bool checkGoalCondition()

Decide whether the land action should be considered finished.

The plugin commands a position reference well below the ground; the platform stops descending in practice when it touches the floor and the position controller can no longer reduce the altitude error. Two conditions must hold simultaneously and continuously during land_position_condition_time_ seconds for the action to succeed:

  1. Slow enough: |vz| (vertical velocity in earth frame, taken from feedback_.actual_land_speed) is below velocity_threshold_, computed at activation as |land_speed| * land_speed_condition_percentage_. This detects that the descent has effectively stopped.

  2. Descended enough: the drone has descended more than land_condition_height_ meters from its pose at activation (initial_height_ - z > land_condition_height_). This guards against false positives right after activation, when |vz| has not yet built up but the drone has barely moved.

The “continuous” requirement is implemented by resetting condition_start_time_ to “now” any time either condition fails. Only after both conditions have remained true uninterruptedly for longer than land_position_condition_time_ is true returned.

If localization is not yet available (localization_flag_ == false), the function returns false without touching the timer.

Returns:

true if land must be considered finished; false otherwise.

Private Members

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr
double land_height_ = -10.0
double land_speed_condition_percentage_ = 0.2
double land_condition_height_ = 0.2
double land_position_condition_time_ = 1.0
geometry_msgs::msg::PoseStamped target_pose_
geometry_msgs::msg::TwistStamped target_twist_
double velocity_threshold_ = 0.0
double initial_height_ = 0.0
rclcpp::Time condition_start_time_
class Plugin : public land_base::LandBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline bool checkGoalCondition()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::SpeedMotion> speed_motion_handler_ = nullptr
rclcpp::Time time_
double land_speed_condition_percentage_
double land_condition_height_
double land_position_condition_time_
float speed_condition_
float initial_height_
class Plugin : public land_base::LandBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Land::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool own_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool own_resume(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline void feedback_callback(GoalHandleTrajectoryGenerator::SharedPtr, const std::shared_ptr<const TrajectoryGeneratorAction::Feedback> feedback)
inline void result_callback(const GoalHandleTrajectoryGenerator::WrappedResult &result)

Private Types

using TrajectoryGeneratorAction = as2_msgs::action::GeneratePolynomialTrajectory
using GoalHandleTrajectoryGenerator = rclcpp_action::ClientGoalHandle<TrajectoryGeneratorAction>

Private Functions

inline bool checkGoalCondition()
inline as2_msgs::action::GeneratePolynomialTrajectory::Goal landGoalToTrajectoryGeneratorGoal(const as2_msgs::action::Land::Goal &_goal)

Private Members

std::shared_ptr<rclcpp_action::Client<TrajectoryGeneratorAction>> traj_gen_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_pause_client_ = nullptr
as2::SynchronousServiceClient<std_srvs::srv::Trigger>::SharedPtr traj_gen_resume_client_ = nullptr
rclcpp_action::Client<TrajectoryGeneratorAction>::SendGoalOptions traj_gen_goal_options_
std::shared_future<GoalHandleTrajectoryGenerator::SharedPtr> traj_gen_goal_handle_future_
TrajectoryGeneratorAction::Feedback traj_gen_feedback_
bool traj_gen_goal_accepted_ = false
bool traj_gen_result_received_ = false
bool traj_gen_result_ = false
rclcpp::Time time_
double land_speed_condition_percentage_
double land_condition_height_
double land_position_condition_time_
float speed_condition_
float initial_height_
float land_height_ = -10.0f
class Plugin : public generate_polynomial_trajectory_behavior_plugin_base::GeneratePolynomialTrajectoryBase
#include <mav_trajectory_generator.hpp>

Plugin wrapper for the static (offline) ETH-ASL mav_trajectory_generation backend.

The plugin does not override updateWaypoints(): each modify request goes through the base class default, which calls reset() + generateTrajectory() with the live vehicle pose and twist as boundary conditions, so the resulting trajectory is C1-continuous with the drone state at the time of the re-plan.

Public Functions

Plugin()
~Plugin() override = default
bool generateTrajectory(const std::vector<as2_msgs::msg::PoseStampedWithID> &waypoints, double max_speed, double t_trajectory_now) override
bool evaluate(double t_trajectory, as2_msgs::msg::TrajectoryPoint &out, bool is_horizon_sample) override
bool isFinished(double t_trajectory) const override
double getDuration() const override
bool isTrajectoryGenerated() override
void reset() override
std::string getNextWaypointId() override

Protected Functions

void ownInitialize() override

Private Functions

void readConfigParameters()

Private Members

mav_trajectory_generation_cpp::GeneratorConfig generator_config_
std::unique_ptr<mav_trajectory_generation_cpp::TrajectoryGenerator> trajectory_generator_
std::vector<std::pair<std::string, double>> waypoint_arrival_times_
double last_eval_backend_time_ = {0.0}
double internal_offset_ = {0.0}

Private Static Functions

static mav_trajectory_generation_cpp::Waypoint toMavWaypoint(const as2_msgs::msg::PoseStampedWithID &wp)
class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase

Public Functions

inline Plugin()
inline virtual void on_setup() override
inline const geometry_msgs::msg::TwistStamped &twist_from_pose(const geometry_msgs::msg::PoseStamped &pose, std::vector<tf2::Transform> *data = nullptr)

Public Members

geometry_msgs::msg::TwistStamped twist_msg_

Private Functions

inline void rigid_bodies_callback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr msg)
inline void process_mocap_pose(const geometry_msgs::msg::PoseStamped &msg)

Private Members

rclcpp::Subscription<mocap4r2_msgs::msg::RigidBodies>::SharedPtr rigid_bodies_sub_
tf2::Transform earth_to_map_ = tf2::Transform::getIdentity()
const tf2::Transform map_to_odom_ = tf2::Transform::getIdentity()
tf2::Transform odom_to_base_ = tf2::Transform::getIdentity()
bool has_earth_to_map_ = false
std::string mocap_topic_
std::string rigid_body_name_
double twist_alpha_ = 1.0
double orientation_alpha_ = 1.0
geometry_msgs::msg::PoseStamped last_pose_msg_
class Plugin : public as2_motion_controller_plugin_base::ControllerBase
#include <pid_speed_controller.hpp>

PID-based speed controller plugin.

Public Functions

inline Plugin()
inline ~Plugin()
virtual void ownInitialize() override

Plugin-specific initialization, called from initialize().

virtual std::vector<std::string> getEssentialParameters() const override

Names of the parameters required before the plugin can accept setMode.

Returns the fully-namespaced names of the essential PID gain groups (plugin / position_control / yaw_control). Optional groups are tracked separately via params_read_.

Returns:

Vector of fully-qualified essential parameter names.

virtual void updateParameter(const rclcpp::Parameter &parameter) override

Apply a single parameter to the plugin.

Routes the value to the corresponding PID handler and toggles the plugin-side flags (use_bypass_, proportional_limitation_). Tracks the optional gain groups in params_read_ so setMode can refuse modes whose gains have not been delivered yet.

Parameters:

parameter – Parameter to apply.

virtual void reset() override

Reset the cached state, references and commands.

Calls ControllerBase::reset() to clear the base flags. The essentialParamsReady() latch is intentionally preserved.

virtual bool setMode(const as2_msgs::msg::ControlMode &mode_in, const as2_msgs::msg::ControlMode &mode_out) override

Update the control mode to be used by the controller plugin.

Validates that the requested mode is supported by the active gain groups, configures the output twist frame id, and resets the integrators of the affected PID handlers.

Parameters:

  • mode_in – Input control mode requested.

  • mode_out – Output control mode requested.

Returns:

true if the in-out control mode configuration is valid.

virtual void onUpdateState(const geometry_msgs::msg::PoseStamped &pose_msg, const geometry_msgs::msg::TwistStamped &twist_msg) override

Plugin hook called by the base after frame validation and hover latch.

Caches the position, velocity and yaw used by the PID handlers.

Parameters:

  • pose_msg – Latest validated pose message.

  • twist_msg – Latest validated twist message.

virtual void onUpdateReference(const geometry_msgs::msg::PoseStamped &ref) override

Plugin hook for pose reference.

Parameters:

ref – Latest pose reference message.

virtual void onUpdateReference(const geometry_msgs::msg::TwistStamped &ref) override

Plugin hook for twist reference.

Parameters:

ref – Latest twist reference message.

virtual void onUpdateReference(const as2_msgs::msg::TrajectorySetpoints &ref) override

Plugin hook for trajectory reference.

Parameters:

ref – Latest trajectory reference message.

virtual void latchHoverReference(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist) override

Hover latch override.

Synthesizes the reference directly into control_ref_ (zero velocity at the cached pose) because onUpdateReference(TrajectorySetpoints) is gated to TRAJECTORY mode and would reject the default base-class latch.

Parameters:

  • pose – Cached state pose used as the hover anchor.

  • twist – Cached state twist (unused).

virtual bool computeOutput(double dt, geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::TwistStamped &twist, as2_msgs::msg::Thrust &thrust) override

Compute the output signal of the controller plugin.

Runs the active PID handler on the cached state/reference and packs the twist command in the output_twist_frame_id_.

Parameters:

  • dt – Time elapsed since the last call to computeOutput().

  • pose – Output pose (unused by this plugin).

  • twist – Output twist in the output reference frame.

  • thrust – Output thrust (unused by this plugin).

Returns:

true if the output is valid.

Private Types

using PID = pid_controller::PID<double>
using PID_1D = pid_1d_controller::PID<double>

Private Functions

void checkParamList(const std::string &param, std::vector<std::string> &_params_list, bool &_all_params_read)

Mark a parameter as read inside an optional-group tail list.

Parameters:

  • param – Parameter name (already namespaced) to remove.

  • _params_list – In/out tail list still pending; entries are erased on hit.

  • _all_params_read – Out: flipped to true once _params_list is empty.

void updateControllerParameter(PID_1D &_pid_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

Apply a parameter change to a 1-D PID handler.

Parameters:

  • _pid_handler – PID handler to configure.

  • _parameter_name – Tail name of the parameter (without plugin namespace).

  • _param – New parameter value.

void updateController3DParameter(PID &_pid_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

Apply a parameter change to a 3-D PID handler.

Parameters:

  • _pid_handler – PID handler to configure.

  • _parameter_name – Tail name of the parameter (without plugin namespace).

  • _param – New parameter value.

void updateSpeedInAPlaneParameter(PID_1D &_pid_1d_handler, PID &_pid_3d_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

Apply a parameter change to the speed-in-a-plane PID pair.

Parameters:

  • _pid_1d_handler – 1-D handler used for the height PID.

  • _pid_3d_handler – 3-D handler used for the planar PIDs.

  • _parameter_name – Tail name of the parameter (without plugin namespace).

  • _param – New parameter value.

void resetState()

Reset the cached UAV state.

void resetReferences()

Reset the cached references.

void resetCommands()

Reset the cached commands.

bool getOutput(geometry_msgs::msg::TwistStamped &twist_msg)

Compute the twist command from the current state and reference.

Parameters:

twist_msg – Output twist message filled by the controller.

Returns:

true if the command was successfully computed.

Private Members

as2_msgs::msg::ControlMode control_mode_in_
as2_msgs::msg::ControlMode control_mode_out_
ModeParametersRead params_read_
PID_1D pid_yaw_handler_
PID pid_3D_position_handler_
PID pid_3D_velocity_handler_
PID_1D pid_1D_speed_in_a_plane_handler_
PID pid_3D_speed_in_a_plane_handler_
PID pid_3D_trajectory_handler_
const std::vector<std::string> plugin_parameters_tail_ = {"proportional_limitation", "use_bypass"}
const std::vector<std::string> position_control_parameters_tail_ = {"position_control.reset_integral", "position_control.antiwindup_cte", "position_control.alpha", "position_control.kp.x", "position_control.kp.y", "position_control.kp.z", "position_control.ki.x", "position_control.ki.y", "position_control.ki.z", "position_control.kd.x", "position_control.kd.y", "position_control.kd.z"}
const std::vector<std::string> velocity_control_parameters_tail_ = {"speed_control.reset_integral", "speed_control.antiwindup_cte", "speed_control.alpha", "speed_control.kp.x", "speed_control.kp.y", "speed_control.kp.z", "speed_control.ki.x", "speed_control.ki.y", "speed_control.ki.z", "speed_control.kd.x", "speed_control.kd.y", "speed_control.kd.z"}
const std::vector<std::string> speed_in_a_plane_control_parameters_tail_ = {"speed_in_a_plane_control.reset_integral", "speed_in_a_plane_control.antiwindup_cte", "speed_in_a_plane_control.alpha", "speed_in_a_plane_control.height.kp", "speed_in_a_plane_control.height.ki", "speed_in_a_plane_control.height.kd", "speed_in_a_plane_control.speed.kp.x", "speed_in_a_plane_control.speed.kp.y", "speed_in_a_plane_control.speed.ki.x", "speed_in_a_plane_control.speed.ki.y", "speed_in_a_plane_control.speed.kd.x", "speed_in_a_plane_control.speed.kd.y"}
const std::vector<std::string> trajectory_control_parameters_tail_ = {"trajectory_control.reset_integral", "trajectory_control.antiwindup_cte", "trajectory_control.alpha", "trajectory_control.kp.x", "trajectory_control.kp.y", "trajectory_control.kp.z", "trajectory_control.ki.x", "trajectory_control.ki.y", "trajectory_control.ki.z", "trajectory_control.kd.x", "trajectory_control.kd.y", "trajectory_control.kd.z"}
const std::vector<std::string> yaw_control_parameters_tail_ = {"yaw_control.reset_integral", "yaw_control.antiwindup_cte", "yaw_control.alpha", "yaw_control.kp", "yaw_control.ki", "yaw_control.kd"}
std::vector<std::string> velocity_control_parameters_to_read_
std::vector<std::string> speed_in_a_plane_control_parameters_to_read_
std::vector<std::string> trajectory_control_parameters_to_read_
UAV_state uav_state_
UAV_state control_ref_
UAV_command control_command_
Eigen::Vector3d speed_limits_
double yaw_speed_limit_
bool use_bypass_ = true
bool proportional_limitation_ = false
std::string output_twist_frame_id_
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr debug_desired_velocity_pub_
class Plugin : public as2_state_estimator_plugin_base::StateEstimatorBase

Public Functions

inline Plugin()
inline virtual void on_setup() override

Private Functions

inline void generate_map_frame_from_gps(const geographic_msgs::msg::GeoPoint &origin, const sensor_msgs::msg::NavSatFix &gps_pose)
inline void odom_callback(const nav_msgs::msg::Odometry::UniquePtr msg)
inline void getOriginCallback(const as2_msgs::srv::GetOrigin::Request::SharedPtr request, as2_msgs::srv::GetOrigin::Response::SharedPtr response)
inline void setOriginCallback(const as2_msgs::srv::SetOrigin::Request::SharedPtr request, as2_msgs::srv::SetOrigin::Response::SharedPtr response)
inline void gps_callback(sensor_msgs::msg::NavSatFix::UniquePtr msg)

Private Members

rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_
rclcpp::Service<as2_msgs::srv::SetOrigin>::SharedPtr set_origin_srv_
rclcpp::Service<as2_msgs::srv::GetOrigin>::SharedPtr get_origin_srv_
bool use_gps_ = false
bool set_origin_on_start_ = false
double earth_to_map_height_ = 0.0
double origin_lat_ = 0.0
double origin_lon_ = 0.0
double origin_alt_ = 0.0
bool set_map_to_odom_ = true
geometry_msgs::msg::TransformStamped earth_to_map_
geographic_msgs::msg::GeoPoint::UniquePtr origin_
sensor_msgs::msg::NavSatFix::UniquePtr gps_pose_
class Plugin : public as2_map_server_plugin_base::MapServerBase

Public Functions

inline Plugin()
virtual void on_setup() override

Private Functions

void on_laser_scan(const sensor_msgs::msg::LaserScan::SharedPtr msg)
void publish_map(const nav_msgs::msg::OccupancyGrid &map_update)
std::vector<std::vector<int>> bresenham_line(int x0, int y0, int x1, int y1)
bool is_cell_index_valid(std::vector<int> cell)
std::vector<int8_t> add_occ_grid_update(const std::vector<int8_t> &update, const std::vector<int8_t> &occ_grid_data)
nav_msgs::msg::OccupancyGrid filter_occ_grid(const nav_msgs::msg::OccupancyGrid &occ_grid)
std::vector<int> point_to_cell(geometry_msgs::msg::PointStamped point, nav_msgs::msg::MapMetaData map_info, std::string target_frame_id, std::shared_ptr<tf2_ros::Buffer> tf_buffer)
cv::Mat grid_to_img(nav_msgs::msg::OccupancyGrid occ_grid, double thresh = 30, bool unknown_as_free = false)
nav_msgs::msg::OccupancyGrid img_to_grid(const cv::Mat img, const std_msgs::msg::Header &header, double grid_resolution)

Private Members

double scan_range_max_
double map_resolution_
int hit_confidence_
int miss_confidence_
int map_width_
int map_height_
nav_msgs::msg::OccupancyGrid::SharedPtr occ_grid_ = std::make_shared<nav_msgs::msg::OccupancyGrid>()
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_filtered_pub_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::shared_ptr<tf2_ros::TransformListener> tf_listener_
class Plugin : public takeoff_base::TakeoffBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Members

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr platform_takeoff_cli_
std_srvs::srv::SetBool::Request::SharedPtr platform_takeoff_request_
rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture platform_takeoff_future_
class Plugin : public takeoff_base::TakeoffBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline bool checkGoalCondition()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr
geometry_msgs::msg::Point takeoff_position_
double takeoff_angle_
class Plugin : public takeoff_base::TakeoffBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

inline bool checkGoalCondition()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::SpeedMotion> speed_motion_handler_ = nullptr
class Plugin : public takeoff_base::TakeoffBase

Public Functions

inline virtual void ownInit()
inline virtual bool own_activate(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_modify(as2_msgs::action::Takeoff::Goal &_goal) override
inline virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
inline virtual as2_behavior::ExecutionStatus own_run() override
inline void feedback_callback(GoalHandleTrajectoryGenerator::SharedPtr, const std::shared_ptr<const TrajectoryGeneratorAction::Feedback> feedback)
inline void result_callback(const GoalHandleTrajectoryGenerator::WrappedResult &result)

Private Types

using TrajectoryGeneratorAction = as2_msgs::action::GeneratePolynomialTrajectory
using GoalHandleTrajectoryGenerator = rclcpp_action::ClientGoalHandle<TrajectoryGeneratorAction>

Private Functions

inline as2_msgs::action::GeneratePolynomialTrajectory::Goal takeoffGoalToTrajectoryGeneratorGoal(const as2_msgs::action::Takeoff::Goal &_goal)

Private Members

std::shared_ptr<rclcpp_action::Client<TrajectoryGeneratorAction>> traj_gen_client_ = nullptr
rclcpp_action::Client<TrajectoryGeneratorAction>::SendGoalOptions traj_gen_goal_options_
std::shared_future<GoalHandleTrajectoryGenerator::SharedPtr> traj_gen_goal_handle_future_
TrajectoryGeneratorAction::Feedback traj_gen_feedback_
bool traj_gen_goal_accepted_ = false
bool traj_gen_result_received_ = false
bool traj_gen_result_ = false
class Plugin : public gripper_behavior_plugin_base::GripperBase

Public Functions

virtual void ownInit() override
virtual bool own_activate(as2_msgs::action::GripperHandler::Goal &_goal) override
virtual bool own_modify(as2_msgs::action::GripperHandler::Goal &_goal) override
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool own_pause(const std::shared_ptr<std::string> &message) override
virtual bool own_resume(const std::shared_ptr<std::string> &message) override
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) override
virtual as2_behavior::ExecutionStatus own_run() override

Private Functions

void closeGripper()
void openGripper()

Private Members

rclcpp::CallbackGroup::SharedPtr cbk_group_
rclcpp::PublisherOptions pub_options
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_l_finger_
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_r_finger_
double l_finger_open_
double r_finger_open_
double l_finger_close_
double r_finger_close_
std::string topic_l_finger_
std::string topic_r_finger_
class Plugin : public as2_behaviors_path_planning::PluginBase

Public Functions

virtual void initialize(as2::Node *node_ptr, std::shared_ptr<tf2_ros::Buffer> tf_buffer) override
virtual bool on_activate(geometry_msgs::msg::PoseStamped drone_pose, as2_msgs::action::NavigateToPoint::Goal goal) override
virtual bool on_deactivate() override
virtual bool on_modify() override
virtual bool on_pause() override
virtual bool on_resume() override
virtual void on_execution_end() override
virtual as2_behavior::ExecutionStatus on_run() override

Private Functions

void occ_grid_cbk(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
bool outline_map(nav_msgs::msg::OccupancyGrid &occ_grid, uint8_t value)
void update_dynamic_voronoi(nav_msgs::msg::OccupancyGrid &occ_grid)
void update_costs(nav_msgs::msg::OccupancyGrid &occ_grid)
void viz_voronoi_grid()
void viz_dist_field_grid()
visualization_msgs::msg::Marker get_path_marker(std::string frame_id, rclcpp::Time stamp, std::vector<Point2i> path, nav_msgs::msg::MapMetaData map_info, std_msgs::msg::Header map_header)

Private Members

DynamicVoronoi dynamic_voronoi_
unsigned int last_size_x_ = 0
unsigned int last_size_y_ = 0
std::mutex mutex_
VoronoiSearcher graph_searcher_
nav_msgs::msg::OccupancyGrid last_occ_grid_
nav_msgs::msg::OccupancyGrid last_dist_field_grid_
bool enable_visualization_
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_grid_sub_
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr viz_pub_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr viz_voronoi_grid_pub_
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr viz_dist_field_grid_pub_
class PluginBase

Subclassed by a_star::Plugin, voronoi::Plugin

Public Functions

virtual void initialize(as2::Node *node_ptr, std::shared_ptr<tf2_ros::Buffer> tf_buffer) = 0
virtual bool on_activate(geometry_msgs::msg::PoseStamped drone_pose, as2_msgs::action::NavigateToPoint::Goal goal) = 0
virtual bool on_deactivate() = 0
virtual bool on_modify() = 0
virtual bool on_pause() = 0
virtual bool on_resume() = 0
virtual void on_execution_end() = 0
virtual as2_behavior::ExecutionStatus on_run() = 0
inline virtual ~PluginBase()

Public Members

std::vector<geometry_msgs::msg::Point> path_

Protected Functions

inline PluginBase()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
class Point2i
#include <cell_node.hpp>

Point2i class, an integer 2d point.

Public Functions

inline Point2i()

Constructor for Point2i class.

inline Point2i(int _x, int _y)

Constructor for Point2i class.

Parameters:

  • _x – x value

  • _y – y value

inline bool operator==(const Point2i &other)
inline bool operator!=(const Point2i &other)

Public Members

int x
int y
class PointGimbalBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::PointGimbal>

Public Functions

explicit PointGimbalBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
virtual ~PointGimbalBehavior() = default

Protected Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::PointGimbal::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::PointGimbal::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::PointGimbal::Goal> &goal, std::shared_ptr<as2_msgs::action::PointGimbal::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::PointGimbal::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
bool check_gimbal_limits(const double roll, const double pitch, const double yaw)
bool update_gimbal_state()
bool point_check_finished()
bool move_check_finished(as2_msgs::msg::GimbalControl target, geometry_msgs::msg::Vector3Stamped current)

Private Members

as2::tf::TfHandler tf_handler_
std::string base_link_frame_id_
std::string gimbal_name_
std::string gimbal_base_frame_id_
std::string gimbal_frame_id_
double gimbal_threshold_
rclcpp::Time goal_init_time_
rclcpp::Duration behavior_timeout_ = rclcpp::Duration(0, 0)
double gimbal_roll_min_
double gimbal_roll_max_
double gimbal_pitch_min_
double gimbal_pitch_max_
double gimbal_yaw_min_
double gimbal_yaw_max_
geometry_msgs::msg::PointStamped desired_goal_position_
geometry_msgs::msg::PointStamped current_goal_position_
geometry_msgs::msg::Vector3Stamped gimbal_angles_current_
geometry_msgs::msg::Vector3Stamped gimbal_angles_offset_
as2_msgs::msg::GimbalControl gimbal_control_msg_
rclcpp::Publisher<as2_msgs::msg::GimbalControl>::SharedPtr gimbal_control_pub_
class PolynomialThrustMap
#include <polynomial_thrust_map.hpp>

Polynomial thrust map class.

Computes throttle command from thrust and voltage by evaluating: throttle = a + b * T + c * V + d * T^2 + e * T * V + f * V^2

Public Functions

inline explicit PolynomialThrustMap(unsigned int n_motors)

Construct a PolynomialThrustMap with zero-initialized coefficients.

Parameters:

n_motors – Number of motors of the platform.

inline explicit PolynomialThrustMap(unsigned int n_motors, double max_throttle, double min_throttle, double a, double b, double c, double d, double e, double f, double gamma2, double gamma1, double gamma0, bool use_correction_factor, as2::AerialPlatform *platform_node_ptr)

Construct a PolynomialThrustMap with given coefficients.

Parameters:

  • n_motors – Number of motors of the platform.

  • max_throttle – Maximum throttle value for normalized output.

  • min_throttle – Minimum throttle value for normalized output.

  • a – Constant coefficient of the throttle polynomial.

  • b – Linear thrust coefficient of the throttle polynomial.

  • c – Linear voltage coefficient of the throttle polynomial.

  • d – Quadratic thrust coefficient of the throttle polynomial.

  • e – Cross (thrust * voltage) coefficient of the throttle polynomial.

  • f – Quadratic voltage coefficient of the throttle polynomial.

  • gamma2 – Quadratic coefficient of the voltage correction factor polynomial.

  • gamma1 – Linear coefficient of the voltage correction factor polynomial.

  • gamma0 – Constant coefficient of the voltage correction factor polynomial.

  • use_correction_factor – Whether to apply the voltage correction factor.

  • platform_node_ptr – Pointer to the aerial platform node.

std::string to_string() const

Return string with thrust map parameters for logging.

Returns:

String with thrust map parameters.

template<typename T>
T getParameter(std::string param_name) const

Read a ROS 2 parameter from platform config file, declaring it if not yet set.

Template Parameters:

T – Type to cast the parameter value to.

Parameters:

param_name – Name of the ROS 2 parameter to read.

Returns:

The parameter value cast to type T.

void set_parameters(double max_throttle, double min_throttle, double a, double b, double c, double d, double e, double f, bool use_correction_factor, double gamma2, double gamma1, double gamma0)

Set all thrust map parameters.

void readParameters()

Read and load all polynomial coefficients from config file.

void initialize(as2::AerialPlatform *platform_node_ptr)

Initialize the thrust map with a pointer to the aerial platform node.

Parameters:

platform_node_ptr – Pointer to the aerial platform node.

double mapThrust(double thrust, double voltage)

Compute a throttle command from thrust and voltage.

Parameters:

  • thrust – Desired thrust in Newtons.

  • voltage – Current battery voltage in Volts.

Returns:

Throttle command in the calibrated range.

uint16_t getThrottle_useconds(double thrust, double voltage)

Compute the throttle command in microseconds from thrust and voltage.

Parameters:

  • thrust – Desired thrust in Newtons.

  • voltage – Current battery voltage in Volts.

Returns:

Throttle command in microseconds (us).

double getThrottle_normalized(double thrust, double voltage)

Compute the normalized throttle command from thrust and voltage.

Parameters:

  • thrust – Desired thrust in Newtons.

  • voltage – Current battery voltage in Volts.

Returns:

Normalized throttle command in the specified throttle range.

Protected Attributes

as2::AerialPlatform *platform_node_ptr_

Private Members

double max_throttle_ = 2000.0
double min_throttle_ = 1000.0
double a
double b
double c
double d
double e
double f
bool use_correction_factor_
double gamma2
double gamma1
double gamma0
uint n_motors

Friends

inline friend std::ostream &operator<<(std::ostream &os, const PolynomialThrustMap &tm)
class PositionMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <position_motion.hpp>

The PositionMotion class is a motion reference handler that moves the robot to a given position.

Public Functions

explicit PositionMotion(as2::Node *node_ptr, const std::string &ns = "")

PositionMotion Constructor.

Parameters:

node – as2::Node pointer.

inline ~PositionMotion()

PositionMotion Destructor.

bool ownSendCommand()

ownSendCommand sends the pose and twist messages.

Returns:

true if commands was sent successfully, false otherwise.

bool sendPositionCommandWithYawAngle(const std::string &frame_id_pose, const float &x, const float &y, const float &z, const float &yaw_angle, const std::string &frame_id_twist, const float &vx, const float &vy, const float &vz)

sendPositionCommandWithYawAngle sends a position command to the robot. The yaw angle is given in radians. The linear velocity limitation is given in m/s. The position command and the velocity limitation are sent in the input frame id.

Parameters:

  • frame_id_pose – frame id of the position command.

  • x – x coordinate of the position command.

  • y – y coordinate of the position command.

  • z – z coordinate of the position command.

  • yaw_angle – yaw angle of the position command.

  • frame_id_twist – frame id of the velocity limitation.

  • vx – linear velocity limitation in x direction.

  • vy – linear velocity limitation in y direction.

  • vz – linear velocity limitation in z direction.

Returns:

true if the command was sent successfully, false otherwise.

bool sendPositionCommandWithYawAngle(const std::string &frame_id_pose, const float &x, const float &y, const float &z, const geometry_msgs::msg::Quaternion &q, const std::string &frame_id_twist, const float &vx, const float &vy, const float &vz)

sendPositionCommandWithYawAngle sends a position command to the robot. The yaw angle is given in a quaternion. The linear velocity is given in m/s. The position command and the velocity limitation are sent in the input frame id.

Parameters:

  • frame_id_pose – frame id of the position command.

  • x – x coordinate of the position command.

  • y – y coordinate of the position command.

  • z – z coordinate of the position command.

  • q – quaternion of the position command. (with the desired yaw angle).

  • frame_id_twist – frame id of the velocity limitation.

  • vx – linear velocity limitation in x direction.

  • vy – linear velocity limitation in y direction.

  • vz – linear velocity limitation in z direction.

Returns:

true if the command was sent successfully, false otherwise.

bool sendPositionCommandWithYawAngle(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

sendPositionCommandWithYawAngle sends a position command to the robot. The position command is sent in the frame id frame.

Parameters:

  • pose – geometry_msgs::msg::PoseStamped with the desired position and yaw angle.

  • twist – geometry_msgs::msg::TwistStamped with the desired linear velocity.

Returns:

true if the command was sent successfully, false otherwise.

bool sendPositionCommandWithYawSpeed(const std::string &frame_id_pose, const float &x, const float &y, const float &z, const float &yaw_speed, const std::string &frame_id_twist, const float &vx, const float &vy, const float &vz)

sendPositionCommandWithYawSpeed sends a position command to the robot. The yaw speed is given in rad/s. The linear velocity is given in m/s. The position command and the velocity limitation are sent in the input frame id.

Parameters:

  • frame_id_pose – frame id of the position command.

  • x – x coordinate of the position command.

  • y – y coordinate of the position command.

  • z – z coordinate of the position command.

  • yaw_speed – yaw speed of the position command.

  • frame_id_twist – frame id of the velocity limitation.

  • vx – linear velocity limitation in x direction.

  • vy – linear velocity limitation in y direction.

  • vz – linear velocity limitation in z direction.

Returns:

true if the command was sent successfully, false otherwise.

bool sendPositionCommandWithYawSpeed(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

sendPositionCommandWithYawSpeed sends a position command to the robot. The position command is sent in the frame id frame.

Parameters:

  • x – pose geometry_msgs::msg::PoseStamped with the desired position.

  • twist – geometry_msgs::msg::TwistStamped with the desired linear velocity and yaw speed.

Returns:

true if the command was sent successfully, false otherwise.

struct Quaternion

Public Members

float w
float x
float y
float z
class RateBase

Subclassed by as2::rate::GenericRate< Clock >

Public Functions

RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE (RateBase) virtual ~RateBase()=default
virtual bool sleep() = 0
virtual bool is_steady() const = 0
virtual void reset() = 0
virtual void set_period(std::chrono::nanoseconds period) = 0
class RealsenseInterface : public as2::Node

Public Functions

explicit RealsenseInterface(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Constructor of the RealsenseInterface object.

void stop()

Stop rutine for odometry node. This function stops the pipeline.

void run()

Funtionality during node lifetime. This function gets the sensor measurements from realsense device and publish them.

Private Functions

bool setup()
void setupCamera(const std::shared_ptr<as2::sensors::Camera> &_camera, const rs2_stream _rs2_stream, const std::string _encoding, const std::string _camera_model)
void runImu(const rs2::motion_frame &accel_frame, const rs2::motion_frame &gyro_frame)
void runPose(const rs2::pose_frame &pose_frame)
void runOdom(const rs2::pose_frame &pose_frame)
void runColor(const rs2::video_frame &color_frame)
void runDepth(const rs2::video_frame &depth_frame)
bool identifyDevice()
bool identifySensors(const rs2::device &dev)
void setStaticTransform(const std::string _rs_link, const std::string _ref_frame, const std::array<double, 3> &_t, const std::array<double, 3> &_r)
void setupPoseTransforms(const std::array<double, 3> &device_t, const std::array<double, 3> &device_r)

Private Members

std::string realsense_name_
bool verbose_
bool device_not_found_
bool imu_available_
bool depth_available_
bool color_available_
bool fisheye_available_
bool pose_available_
std::string serial_
rs2::pipeline pipe_
std::shared_ptr<as2::sensors::Sensor<nav_msgs::msg::Odometry>> pose_sensor_
std::shared_ptr<as2::sensors::Imu> imu_sensor_
std::shared_ptr<as2::sensors::Camera> color_sensor_
std::shared_ptr<rs2::motion_frame> accel_frame_
std::shared_ptr<rs2::motion_frame> gyro_frame_
std::shared_ptr<rs2::pose_frame> pose_frame_
std::shared_ptr<rs2::video_frame> color_frame_
std::shared_ptr<as2::sensors::Camera> depth_sensor_
std::shared_ptr<rs2::video_frame> depth_frame_
std::string depth_sensor_frame_
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_
std::vector<geometry_msgs::msg::TransformStamped> tf2_fix_transforms_
geometry_msgs::msg::TransformStamped rs_odom2rs_link_tf_
std::string base_link_frame_
std::string odom_frame_
std::string realsense_link_frame_
std::string color_sensor_frame_
std::string imu_sensor_frame_
tf2::Transform base_link_to_realsense_link_
tf2::Transform base_link_to_realsense_pose_odom_
tf2::Transform realsense_link_to_realsense_pose_
tf2::Transform realsense_link_to_realsense_pose_odom_
tf2::Transform realsense_pose_
struct Rotor
#include <Parameters.hpp>

A struct that holds various properties of a rotor.

Public Members

double angle

The angle formed on the xy plane by the vector going from the the center of mass to the rotor. The angle is measured from the +x axis of the body frame.

double armLength

The length of the vector going from the center of mass to the rotor.

double forceConstant

A constant that multiplies with the square of the rotor’s velocity to compute its thrust.

double momentConstant

A constant the multiplies with the rotor’s thrust to compute its moment.

int direction

Direction of rotation of the rotor. +1 is counterclockwise and -1 is clockwise.

class SendEvent : public BT::SyncActionNode

Public Functions

SendEvent(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
BT::NodeStatus tick() override

Public Static Functions

static inline BT::PortsList providedPorts()

Private Members

rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_
std::string topic_name_
template<typename T>
class Sensor : public as2::sensors::TFStatic, protected as2::sensors::GenericSensor, public as2::sensors::SensorData<T>
#include <sensor.hpp>

Sensor handler to publish sensor data at a given frequency.

Public Functions

inline Sensor(const std::string &id, as2::Node *node_ptr, float pub_freq = -1.0f, bool add_sensor_measurements_base = true)

Construct a new Sensor object.

Parameters:

  • id – Sensor ID

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

inline Sensor(const std::string &id, as2::Node *node_ptr, int pub_freq, bool add_sensor_measurements_base = true)

Construct a new Sensor object.

Parameters:

  • id – Sensor ID

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

inline Sensor(const std::string &id, as2::Node *node_ptr, double pub_freq, bool add_sensor_measurements_base = true)

Construct a new Sensor object.

Parameters:

  • id – Sensor ID

  • node_ptr – Pointer to the node

  • pub_freq – Frequency to publish the data (-1 to publish every time updateData is called)

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

inline virtual ~Sensor()

Destroy the Sensor object.

inline void updateData(const T &msg)

Update the data of the sensor.

If the frequency is set to -1, the data is published immediately Otherwise, the data is stored and published at the given frequency

Parameters:

msg – Message

Protected Functions

inline virtual void publishData() override

Publish the data in a topic.

template<typename T>
class SensorData
#include <sensor.hpp>

SensorData object to publish data in a topic.

Template Parameters:

T – Type of the message

Subclassed by as2::sensors::Sensor< T >

Public Functions

inline explicit SensorData(const std::string &topic_name, rclcpp::Node *node_ptr, bool add_sensor_measurements_base = true)

Construct a new GenericSensor object.

Parameters:

node_ptr – Pointer to the node

inline virtual ~SensorData()

Destroy the GenericSensor object.

inline void setData(const T &msg)

Update the message value.

Parameters:

msg – Message

inline void publish()

Publish the data stored in the message.

inline void updateAndPublish(const T &msg)

Update the message value and publish it.

Parameters:

msg – Message

inline const std::string &getTopicName() const

Get the Topic Name object.

Returns:

const std::string& Topic name

inline const T &getData() const

Get the data stored in the message.

Returns:

const T& Message

inline T &getDataRef()

Get the data stored in the message.

Returns:

T& Message

Public Static Functions

static inline std::string processTopicName(const std::string &topic_name, bool add_sensor_measurements_base = true)

Process the topic name with the sensor measurements base.

Parameters:

  • topic_name – Topic name

  • add_sensor_measurements_base – Add “sensor_measurements” to the topic name

Returns:

std::string Processed topic name

Private Members

rclcpp::Publisher<T>::SharedPtr sensor_publisher_
T msg_data_
std::string topic_name_
class SetArmingStateBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::SetArmingState>

Public Functions

inline SetArmingStateBehavior()
inline bool on_activate(std::shared_ptr<const as2_msgs::action::SetArmingState::Goal> goal) override
inline bool on_modify(std::shared_ptr<const as2_msgs::action::SetArmingState::Goal> goal) override
inline virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool on_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool on_resume(const std::shared_ptr<std::string> &message) override
inline virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
inline as2_behavior::ExecutionStatus on_run(const typename std::shared_ptr<const as2_msgs::action::SetArmingState::Goal> &goal, typename std::shared_ptr<as2_msgs::action::SetArmingState::Feedback> &feedback_msg, typename std::shared_ptr<as2_msgs::action::SetArmingState::Result> &result_msg) override

Public Members

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_
rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future_
class SetEntityPoseBridge : public rclcpp::Node

Public Functions

SetEntityPoseBridge()

Private Functions

void setEntityPoseServiceCallback(const ros_gz_interfaces::srv::SetEntityPose::Request::SharedPtr request, ros_gz_interfaces::srv::SetEntityPose::Response::SharedPtr result)

Private Members

std::shared_ptr<gz::transport::Node> gz_node_ptr_
std::string world_name_
std::string set_entity_pose_service
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr ps_sub_
rclcpp::Service<ros_gz_interfaces::srv::SetEntityPose>::SharedPtr ps_srv_sub_
class SetOffboardModeBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::SetOffboardMode>

Public Functions

inline SetOffboardModeBehavior()
inline bool on_activate(std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal> goal) override
inline bool on_modify(std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal> goal) override
inline virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
inline virtual bool on_pause(const std::shared_ptr<std::string> &message) override
inline virtual bool on_resume(const std::shared_ptr<std::string> &message) override
inline virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override
inline as2_behavior::ExecutionStatus on_run(const typename std::shared_ptr<const as2_msgs::action::SetOffboardMode::Goal> &goal, typename std::shared_ptr<as2_msgs::action::SetOffboardMode::Feedback> &feedback_msg, typename std::shared_ptr<as2_msgs::action::SetOffboardMode::Result> &result_msg) override

Public Members

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client_
rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future_
class SetOrigin : public as2_behavior_tree::BtServiceNode<as2_msgs::srv::SetOrigin>

Public Functions

SetOrigin(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Fill in service request with information if necessary.

BT::NodeStatus on_completion()

Public Static Functions

static inline BT::PortsList providedPorts()
class SimClockPublisher : public rclcpp::Node

Public Functions

inline SimClockPublisher()

Private Functions

inline void tick()

Private Members

double frequency_hz_ = {1000.0}
double rtf_ = {1.0}
double sim_dt_s_ = {0.001}
double sim_time_s_ = {0.0}
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr publisher_
rclcpp::TimerBase::SharedPtr timer_
class SpeedInAPlaneMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <speed_in_a_plane_motion.hpp>

The SpeedInAPlaneMotion class is a motion reference handler that moves the robot at a given speed.

Public Functions

explicit SpeedInAPlaneMotion(as2::Node *node_ptr, const std::string &ns = "")

PositionMotion Constructor.

Parameters:

node – as2::Node pointer.

inline ~SpeedInAPlaneMotion()

PositionMotion Destructor.

bool ownSendCommand()

ownSendCommand sends pose and twist messages.

Returns:

true if commands was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawSpeed(const std::string &frame_id_speed, const float vx, const float vy, const std::string &frame_id_pose, const float hz, const float yaw_speed)

sendSpeedInAPlaneCommandWithYawAngle sends a speed in a plane command to the robot with a yaw speed. The speed plane command is sent in the frame id frame. The linear velocity is given in m/s. The height command is sent in the frame id frame. The height is given in m.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • frame_id_yaw – frame id of the height and yaw angle command.

  • hz – Height in the z axis.

  • yaw_speed – Yaw speed in radians/s.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawSpeed(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

sendSpeedInAPlaneCommandWithYawAngle sends a speed in a plane command to the robot with a yaw speed. The speed plane command is sent in the frame id frame. The linear velocity is given in m/s. The height command is sent in the frame id frame. The height is given in m.

Parameters:

  • pose – PoseStamped message that represents the height.

  • twist – TwistStamped message that represents the linear velocity and yaw speed.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawAngle(const std::string &frame_id_speed, const float vx, const float vy, const std::string &frame_id_pose, const float hz, const float yaw_angle)

sendSpeedInAPlaneCommandWithYawAngle sends a speed in a plane command to the robot with a yaw angle. The speed plane command is sent in the frame id frame. The linear velocity is given in m/s. The height command is sent in the frame id frame. The height is given in m.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • frame_id_yaw – frame id of the height and yaw angle command.

  • hz – Height in the z axis.

  • yaw_angle – Yaw angle in radians.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawAngle(const std::string &frame_id_speed, const float vx, const float vy, const std::string &frame_id_pose, const float hz, const geometry_msgs::msg::Quaternion &q)

sendSpeedInAPlaneCommandWithYawAngle sends a speed in a plane command to the robot with a yaw angle. The speed plane command is sent in the frame id frame. The linear velocity is given in m/s. The height command is sent in the frame id frame. The height is given in m.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • frame_id_yaw – frame id of the height and yaw angle command.

  • hz – Height in the z axis.

  • q – Quaternion that represents the yaw angle.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawAngle(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

sendSpeedInAPlaneCommandWithYawAngle sends a speed in a plane command to the robot with a yaw angle. The speed plane command is sent in the frame id frame. The linear velocity is given in m/s. The height command is sent in the frame id frame. The height is given in m.

Parameters:

  • pose – PoseStamped message that represents the height and yaw angle.

  • twist – TwistStamped message that represents the linear velocity.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawAngle(const std::string &frame_id_speed, const float &vx, const float &vy, const std::string &frame_id_pose, const float &hz, const float &yaw_angle)

sendSpeedInAPlaneCommandWithYawAngle sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • hz – Height in the z axis.

  • frame_id_yaw – frame id of the yaw angle command.

  • yaw_angle – Yaw angle in radians.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawAngle(const std::string &frame_id_speed, const float &vx, const float &vy, const std::string &frame_id_pose, const float &hz, const geometry_msgs::msg::Quaternion &q)

sendSpeedInAPlaneCommandWithYawAngle sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • hz – Height in the z axis.

  • frame_id_yaw – frame id of the yaw angle command.

  • q – Quaternion that represents the yaw angle.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedInAPlaneCommandWithYawSpeed(const std::string &frame_id, const float &vx, const float &vy, const std::string &frame_id_pose, const float &hz, const float &yaw_speed)

sendSpeedInAPlaneCommandWithYawSpeed sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s. The yaw speed is given in rad/s.

Parameters:

  • frame_id – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • vz – Linear velocity in the z axis.

  • yaw_speed – Yaw speed in rad/s.

Returns:

true if the command was sent successfully, false otherwise.

class SpeedMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <speed_motion.hpp>

The SpeedMotion class is a motion reference handler that moves the robot at a given speed.

Public Functions

explicit SpeedMotion(as2::Node *node_ptr, const std::string &ns = "")

PositionMotion Constructor.

Parameters:

node – as2::Node pointer.

inline ~SpeedMotion()

SpeedMotion Destructor.

bool ownSendCommand()

ownSendCommand sends pose and twist messages.

Returns:

true if commands was sent successfully, false otherwise.

bool sendSpeedCommandWithYawAngle(const std::string &frame_id_speed, const float &vx, const float &vy, const float &vz, const std::string &frame_id_yaw, const float &yaw_angle)

sendSpeedCommandWithYawAngle sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • vz – Linear velocity in the z axis.

  • frame_id_yaw – frame id of the yaw angle command.

  • yaw_angle – Yaw angle in radians.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedCommandWithYawAngle(const std::string &frame_id_speed, const float &vx, const float &vy, const float &vz, const std::string &frame_id_yaw, const geometry_msgs::msg::Quaternion &q)

sendSpeedCommandWithYawAngle sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s.

Parameters:

  • frame_id_speed – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • vz – Linear velocity in the z axis.

  • frame_id_yaw – frame id of the yaw angle command.

  • q – Quaternion that represents the yaw angle.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedCommandWithYawAngle(const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist)

sendSpeedCommandWithYawAngle sends a speed command to the robot. The speed command is sent in the frame id frame. The linear velocity is given in m/s.

Parameters:

  • pose – PoseStamped message that represents the rotation .

  • twist – TwistStamped message that represents the linear velocity.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedCommandWithYawSpeed(const std::string &frame_id, const float &vx, const float &vy, const float &vz, const float &yaw_speed)

sendSpeedCommandWithYawSpeed sends a speed command to the robot. The speed command is sent in the input frame id. The linear velocity is given in m/s. The yaw speed is given in rad/s.

Parameters:

  • frame_id – frame id of the velocity command.

  • vx – Linear velocity in the x axis.

  • vy – Linear velocity in the y axis.

  • vz – Linear velocity in the z axis.

  • yaw_speed – Yaw speed in rad/s.

Returns:

true if the command was sent successfully, false otherwise.

bool sendSpeedCommandWithYawSpeed(const geometry_msgs::msg::TwistStamped &twist)

sendSpeedCommandWithYawSpeed sends a speed command to the robot. The speed command is sent in the frame id frame. The linear velocity is given in m/s. The yaw speed is given in rad/s.

Parameters:

twist – TwistStamped message that represents the linear velocity and the angular yaw speed

Returns:

true if the command was sent successfully, false otherwise.

class StateEstimator : public as2::Node

Public Functions

explicit StateEstimator(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
inline ~StateEstimator()

Private Members

std::filesystem::path plugin_name_
std::shared_ptr<pluginlib::ClassLoader<as2_state_estimator_plugin_base::StateEstimatorBase>> loader_
std::shared_ptr<as2_state_estimator_plugin_base::StateEstimatorBase> plugin_ptr_
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfstatic_broadcaster_
std::shared_ptr<as2::tf::TfHandler> tf_handler_

Private Static Functions

static rclcpp::NodeOptions get_modified_options(const rclcpp::NodeOptions &options)

Modify the node options to allow undeclared parameters.

class StateEstimatorBase

Subclassed by ground_truth::Plugin, ground_truth_odometry_fuse::Plugin, mocap_pose::Plugin, raw_odometry::Plugin

Public Functions

inline StateEstimatorBase()
inline void setup(as2::Node *node, std::shared_ptr<as2::tf::TfHandler> tf_handler, std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster)
virtual void on_setup() = 0
inline virtual bool get_earth_to_map_transform(geometry_msgs::msg::TransformStamped &transform)

Protected Functions

inline void check_standard_transform(const geometry_msgs::msg::TransformStamped &transform)
inline void publish_transform(const geometry_msgs::msg::TransformStamped &transform)
inline void publish_static_transform(const geometry_msgs::msg::TransformStamped &transform)
inline void publish_twist(const geometry_msgs::msg::TwistStamped &twist)
inline void publish_pose(const geometry_msgs::msg::PoseStamped &pose)
inline const std::string &get_earth_frame() const
inline const std::string &get_map_frame() const
inline const std::string &get_odom_frame() const
inline const std::string &get_base_frame() const
inline void set_earth_frame(const std::string &frame)
inline void set_map_frame(const std::string &frame)
inline void set_odom_frame(const std::string &frame)
inline void set_base_frame(const std::string &frame)
inline bool get_earth_to_map_transform(tf2::Transform &earth_to_map)
inline bool convert_earth_to_baselink_2_odom_to_baselink_transform(const tf2::Transform &earth_to_baselink, tf2::Transform &odom_to_baselink, const tf2::Transform &earth_to_map, const tf2::Transform &map_to_odom = tf2::Transform::getIdentity())
inline bool convert_odom_to_baselink_2_earth_to_baselink_transform(const tf2::Transform &odom_to_baselink, tf2::Transform &earth_to_baselink, const tf2::Transform &earth_to_map, const tf2::Transform &map_to_odom = tf2::Transform::getIdentity())

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_
tf2::Transform odom_to_baselink
tf2::Transform earth_to_map
tf2::Transform map_to_odom
tf2::Transform earth_to_baselink
bool static_transforms_published_ = false
rclcpp::TimerBase::SharedPtr static_transforms_timer_

Private Members

std::string earth_frame_id_
std::string base_frame_id_
std::string odom_frame_id_
std::string map_frame_id_
tf2::Transform earth_to_map_ = tf2::Transform::getIdentity()
tf2::Transform map_to_odom_ = tf2::Transform::getIdentity()
tf2::Transform odom_to_base_ = tf2::Transform::getIdentity()
struct StateMachineTransition
#include <platform_state_machine.hpp>

Data Structure for defining the state machine transitions.

Public Members

std::string transition_name
int8_t from_state_id
int8_t transition_id
int8_t to_state_id
class SuctionGripperPlugin : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPreUpdate

Public Functions

SuctionGripperPlugin()
~SuctionGripperPlugin()
void Configure(const gz::sim::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventMgr) override
void PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override

Public Members

std::unique_ptr<SuctionGripperPrivate> dataPtr
class SuctionGripperPrivate

Public Functions

inline void OnContact(int idx0, int idx1, const gz::msgs::Contacts &_msg)

Callback for when contact is made.

inline void OnCmd(const gz::msgs::Boolean &_suctionOn)

Command callback.

Public Members

gz::sim::Entity childItem = {gz::sim::v8::kNullEntity}

The item being moved.

std::string linkName

The gripper link name.

gz::sim::Entity joint = {gz::sim::v8::kNullEntity}

Used to store the joint when we attach to an object.

gz::sim::Entity gripperEntity = {gz::sim::v8::kNullEntity}

The gripper link entity.

gz::transport::Node node

The transport node.

bool suctionOn = {false}

Used for determining when the suction is on.

bool pendingJointCreation = {false}

Set to true when we detect the suction gripper is in contact.

bool jointCreated = {false}

True when we are holding an object.

std::mutex mtx

mutex for accessing member variables

std::array<std::array<gz::sim::Entity, 3>, 3> contacts

Two-dimensional array of contact points.

gz::transport::Node::Publisher contactPublisherCenter

Publisher for contact points.

gz::transport::Node::Publisher contactPublisherLeft
gz::transport::Node::Publisher contactPublisherRight
gz::transport::Node::Publisher contactPublisherTop
gz::transport::Node::Publisher contactPublisherBottom
class SwarmFlockingBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::SwarmFlocking>

Public Functions

SwarmFlockingBehavior()
inline ~SwarmFlockingBehavior()
bool on_activate(std::shared_ptr<const as2_msgs::action::SwarmFlocking::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::SwarmFlocking::Goal> goal) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::SwarmFlocking::Goal> &goal, std::shared_ptr<as2_msgs::action::SwarmFlocking::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::SwarmFlocking::Result> &result_msg) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Public Members

std::vector<std::shared_ptr<rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowReference>>> goal_future_handles_

Private Functions

bool setUpVirtualCentroid(const geometry_msgs::msg::PoseStamped &virtual_centroid)

Set up the virtual centroid of the swarm with offset in the desired frame.

Parameters:

virtual_centroid – The virtual centroid of the swarm in the desired frame

Returns:

bool Return true if the frame is not empty

bool setUpDronesFormation(geometry_msgs::msg::PoseStamped centroid, std::vector<std::string> drones_namespace, std::vector<as2_msgs::msg::PoseWithID> formation)

Set the drones reference in the swarm.

Parameters:

  • centroid – The centroid of the swarm

  • drones_namespace – The namespaced of the drones

  • formation – The position of the drones in the swarm

Returns:

bool Always true at the end of the function

bool initDroneReferences()

Active the followReference of the drones and check if the drones are ready to execute the action.

Returns:

bool Return true if the drones are ready to execute

as2_behavior::ExecutionStatus monitoring(const std::vector<std::shared_ptr<rclcpp_action::ClientGoalHandle<as2_msgs::action::FollowReference>>> goal_future_handles)

Check the followReference status of the drones.

Parameters:

goal_future_handles – The goal future handles of the drones

Returns:

as2_behavior::ExecutionStatus Return the status of the monitoring

void dynamicSwarmFormationCallback(as2_msgs::msg::PoseWithIDArray new_formation)

Callback to update the refrences of the drones inside the swarm.

Parameters:

new_formation – The new formation of the swarm

void modifySwarmSrv(const std::shared_ptr<as2_msgs::action::SwarmFlocking::Impl::SendGoalService::Request> request, const std::shared_ptr<as2_msgs::action::SwarmFlocking::Impl::SendGoalService::Response> response)

Service to modify the virtual_centroid, add or detach a new drone reference within the swarm.

Parameters:

  • request – The request of the service

  • response – The response of the service

Private Members

as2_msgs::action::SwarmFlocking::Goal goal_
as2_msgs::action::SwarmFlocking::Result result_
as2_msgs::action::SwarmFlocking::Feedback feedback_
std::shared_ptr<rclcpp::Service<as2_msgs::action::SwarmFlocking::Impl::SendGoalService>> modify_srv_ = nullptr
rclcpp::CallbackGroup::SharedPtr cbk_group_
std::unordered_map<std::string, std::shared_ptr<DroneSwarm>> drones_
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tfstatic_swarm_broadcaster_
std::shared_ptr<geometry_msgs::msg::TransformStamped> transform_
std::string swarm_base_link_frame_id_
rclcpp::Subscription<as2_msgs::msg::PoseWithIDArray>::SharedPtr dynamic_swarm_formation_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::shared_ptr<tf2_ros::TransformListener> tf_listener_
template<class ServiceT>
class SynchronousServiceClient
#include <synchronous_service_client.hpp>

Class for handling synchronous service clients in ROS2 without taking care about the spin() method.

Public Types

using SharedPtr = std::shared_ptr<SynchronousServiceClient<ServiceT>>

Public Functions

inline SynchronousServiceClient(std::string service_name, as2::Node *node)

Constructor.

Parameters:

service_name – Name of the service

inline bool wait_for_service(std::chrono::nanoseconds timeout)

Block until the underlying service is discoverable.

Parameters:

timeout – Maximum time to wait for the service to appear.

Returns:

True if the service is discoverable before the timeout.

inline bool sendRequest(const RequestT &req, ResponseT &resp, int wait_time = 0)

Send Request synchronously to the service.

Parameters:

  • request – Request to be sent to the service

  • respone – Response received from the service

  • wait_time – Time to wait for the service to be available in seconds

Returns:

True if the service was called successfully, false otherwise

inline bool sendRequest(const std::shared_ptr<RequestT> &req, std::shared_ptr<ResponseT> &resp, int wait_time = 0)

Send Request synchronously to the service.

Parameters:

  • request – Request to be sent to the service

  • respone – Response received from the service

  • wait_time – Time to wait for the service to be available in seconds

Returns:

True if the service was called successfully, false otherwise

Private Types

typedef ServiceT::Request RequestT
typedef ServiceT::Response ResponseT

Private Members

std::string service_name_
as2::Node *node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
std::shared_ptr<rclcpp::Client<ServiceT>> service_client_
struct takeoff_plugin_params

Public Members

double takeoff_height = 0.0
double takeoff_speed = 0.0
double takeoff_threshold = 0.0
class TakeoffAction : public as2_behavior_tree::BtActionNode<as2_msgs::action::Takeoff>

Public Functions

TakeoffAction(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)
virtual void on_tick() override

Function to perform some user-defined operation on tick Could do dynamic checks, such as getting updates to values on the blackboard.

inline virtual BT::NodeStatus on_success()

Function to perform some user-defined operation upon successful completion of the action. Could put a value on the blackboard.

Returns:

BT::NodeStatus Returns SUCCESS by default, user may override return another value

void on_wait_for_result(std::shared_ptr<const as2_msgs::action::Takeoff::Feedback> feedback)

Public Members

std::string action_name_

Public Static Functions

static inline BT::PortsList providedPorts()
class TakeoffBase

Subclassed by takeoff_plugin_platform::Plugin, takeoff_plugin_position::Plugin, takeoff_plugin_speed::Plugin, takeoff_plugin_trajectory::Plugin

Public Types

using GoalHandleTakeoff = rclcpp_action::ServerGoalHandle<as2_msgs::action::Takeoff>

Public Functions

inline TakeoffBase()
inline virtual ~TakeoffBase()
inline void initialize(as2::Node *node_ptr, std::shared_ptr<as2::tf::TfHandler> tf_handler, takeoff_plugin_params &params)
inline virtual void state_callback(geometry_msgs::msg::PoseStamped &pose_msg, geometry_msgs::msg::TwistStamped &twist_msg)
inline bool on_activate(std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal)
inline bool on_modify(std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal)
inline bool on_deactivate(const std::shared_ptr<std::string> &message)
inline bool on_pause(const std::shared_ptr<std::string> &message)
inline bool on_resume(const std::shared_ptr<std::string> &message)
inline void on_execution_end(const as2_behavior::ExecutionStatus &state)
inline as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal, std::shared_ptr<as2_msgs::action::Takeoff::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::Takeoff::Result> &result_msg)

Protected Functions

inline virtual void ownInit()
virtual bool own_activate(as2_msgs::action::Takeoff::Goal &goal) = 0
inline virtual bool own_modify(as2_msgs::action::Takeoff::Goal &goal)
virtual bool own_deactivate(const std::shared_ptr<std::string> &message) = 0
inline virtual bool own_pause(const std::shared_ptr<std::string> &message)
inline virtual bool own_resume(const std::shared_ptr<std::string> &message)
virtual void own_execution_end(const as2_behavior::ExecutionStatus &state) = 0
virtual as2_behavior::ExecutionStatus own_run() = 0
inline void sendHover()

Protected Attributes

as2::Node *node_ptr_
std::shared_ptr<as2::tf::TfHandler> tf_handler = nullptr
as2_msgs::action::Takeoff::Goal goal_
as2_msgs::action::Takeoff::Feedback feedback_
as2_msgs::action::Takeoff::Result result_
takeoff_plugin_params params_
geometry_msgs::msg::PoseStamped actual_pose_
bool localization_flag_

Private Functions

inline bool processGoal(as2_msgs::action::Takeoff::Goal &_goal)

Private Members

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr
class TakeoffBehavior : public as2_behavior::BehaviorServer<as2_msgs::action::Takeoff>

Public Types

using GoalHandleTakeoff = rclcpp_action::ServerGoalHandle<as2_msgs::action::Takeoff>
using PSME = as2_msgs::msg::PlatformStateMachineEvent

Public Functions

explicit TakeoffBehavior(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
~TakeoffBehavior()
void state_callback(const geometry_msgs::msg::TwistStamped::SharedPtr _twist_msg)
bool sendEventFSME(const int8_t _event)
bool process_goal(std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal, as2_msgs::action::Takeoff::Goal &new_goal)
bool on_activate(std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal) override
bool on_modify(std::shared_ptr<const as2_msgs::action::Takeoff::Goal> goal) override
virtual bool on_deactivate(const std::shared_ptr<std::string> &message) override
virtual bool on_pause(const std::shared_ptr<std::string> &message) override
virtual bool on_resume(const std::shared_ptr<std::string> &message) override
as2_behavior::ExecutionStatus on_run(const std::shared_ptr<const as2_msgs::action::Takeoff::Goal> &goal, std::shared_ptr<as2_msgs::action::Takeoff::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::Takeoff::Result> &result_msg) override
virtual void on_execution_end(const as2_behavior::ExecutionStatus &state) override

Private Members

std::string base_link_frame_id_
std::shared_ptr<pluginlib::ClassLoader<takeoff_base::TakeoffBase>> loader_
std::shared_ptr<takeoff_base::TakeoffBase> takeoff_plugin_
std::shared_ptr<as2::tf::TfHandler> tf_handler_
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_
as2::SynchronousServiceClient<as2_msgs::srv::SetPlatformStateMachineEvent>::SharedPtr platform_cli_
class TeleopPanel : public rviz_common::Panel

Public Functions

explicit TeleopPanel(QWidget *parent = 0)
virtual void load(const rviz_common::Config &config)
virtual void save(rviz_common::Config config) const

Public Slots

void takeoff()
void land()
void disarm(const rclcpp_action::ClientGoalHandle<as2_msgs::action::Land>::WrappedResult &result)
void hover()
void kill()
void updateDroneNs()

Protected Attributes

QLineEdit *drone_editor_
QPushButton *takeoff_button_
QPushButton *land_button_
QPushButton *hover_button_
QPushButton *kill_button_
QString drone_namespace_
std::shared_ptr<as2::Node> node_
std::shared_ptr<rclcpp::Node> node2_
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr arming_client_
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr offboard_client_
rclcpp_action::Client<as2_msgs::action::Takeoff>::SharedPtr takeoff_client_
rclcpp_action::Client<as2_msgs::action::Land>::SharedPtr land_client_
rclcpp::Publisher<as2_msgs::msg::AlertEvent>::SharedPtr alert_pub_
std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_handler_
class TFDynamic
#include <sensor.hpp>

TFDynamic object to publish dynamic transforms in TF.

Subclassed by as2::sensors::Gimbal

Public Functions

explicit TFDynamic(rclcpp::Node *node_ptr)

Construct a new TFDynamic object.

Parameters:

node_ptr – Pointer to the node

virtual ~TFDynamic()

Destroy the TFDynamic object.

virtual void setDynamicTransform(const geometry_msgs::msg::TransformStamped &transform)

Publish the transform.

Parameters:

transform_msg – Transform message

virtual void setDynamicTransform(const std::string &frame_id, const std::string &parent_frame_id, float x, float y, float z, float qx, float qy, float qz, float qw)

Add a dynamic transform.

Parameters:

  • frame_id – Frame ID

  • parent_frame_id – Parent Frame ID

  • x – X position (m)

  • y – Y position (m)

  • z – Z position (m)

  • qx – Quaternion X

  • qy – Quaternion Y

  • qz – Quaternion Z

  • qw – Quaternion W

virtual void setDynamicTransform(const std::string &frame_id, const std::string &parent_frame_id, float x, float y, float z, float roll, float pitch, float yaw)

Add a dynamic transform.

Parameters:

  • frame_id – Frame ID

  • parent_frame_id – Parent Frame ID

  • x – X position (m)

  • y – Y position (m)

  • z – Z position (m)

  • roll – Roll (rad)

  • pitch – Pitch (rad)

  • yaw – Yaw (rad)

std::shared_ptr<tf2_ros::TransformBroadcaster> getTransformBroadcaster() const

Get the Transform Broadcaster object.

Returns:

std::shared_ptr<tf2_ros::TransformBroadcaster>

const rclcpp::Node *getNode() const

Get the Node Pointer object.

Returns:

rclcpp::Node* Node pointer

Protected Attributes

rclcpp::Node *node_ptr_ = nullptr
std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_tf_broadcaster_ptr_
class TfHandler
#include <tf_utils.hpp>

Helper that wraps tf2_ros::Buffer and tf2_ros::TransformListener for use inside an as2::Node.

On construction, the handler declares (if not already declared) and reads the ROS parameter tf_timeout_threshold (in seconds, default 0.05) on the owning node. This timeout is used by all overloads that do not take an explicit timeout argument.

The convert/getter overloads that perform a lookupTransform use "earth" as the fixed frame, which is the inertial root used in aerostack2.

Public Functions

explicit TfHandler(as2::Node *_node)

Construct a new TfHandler bound to the given as2::Node.

Parameters:

_node – Owning node used for clock, parameters and logging.

void setTfTimeoutThreshold(double tf_timeout_threshold)

Set the TF lookup timeout threshold.

Parameters:

tf_timeout_threshold – Timeout, in seconds.

void setTfTimeoutThreshold(const std::chrono::nanoseconds &tf_timeout_threshold)

Set the TF lookup timeout threshold.

Parameters:

tf_timeout_threshold – Timeout, as std::chrono::nanoseconds.

double getTfTimeoutThreshold() const

Get the TF lookup timeout threshold.

Returns:

Timeout, in seconds.

std::shared_ptr<tf2_ros::Buffer> getTfBuffer() const

Get the underlying TF buffer.

Returns:

Shared pointer to the tf2_ros::Buffer owned by this handler.

template<typename T>
inline T convert(const T &input, const std::string &target_frame, const std::chrono::nanoseconds timeout)

Convert a stamped message from one frame to another.

Uses lookupTransform(target, now, source, input.header.stamp, "earth", timeout) when timeout > 0, and the latest available transform (tf2::TimePointZero) otherwise.

Parameters:

  • input – Input stamped message; must expose header.frame_id and header.stamp.

  • target_frame – Target frame id.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Message expressed in target_frame.

template<typename T>
inline T convert(const T &input, const std::string &target_frame)

Convert a stamped message from one frame to another, using the configured tf_timeout_threshold_.

Uses lookupTransform(target, now, source, input.header.stamp, "earth", timeout) when timeout > 0, and the latest available transform (tf2::TimePointZero) otherwise.

Parameters:

  • input – Input stamped message; must expose header.frame_id and header.stamp.

  • target_frame – Target frame id.

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Message expressed in target_frame.

geometry_msgs::msg::TwistStamped convert(const geometry_msgs::msg::TwistStamped &_twist, const std::string &target_frame, const std::chrono::nanoseconds timeout)

Convert a TwistStamped to a target frame.

Only the linear component is transformed (as a Vector3Stamped). The angular component is copied through unchanged: this is correct when both the source and target frames are inertial (or share the same orientation), which is the typical aerostack2 case (e.g. earth ↔ <ns>/odom).

Parameters:

  • _twist – Input twist with valid header.frame_id and stamp.

  • target_frame – Target frame id.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Twist expressed in target_frame.

inline geometry_msgs::msg::TwistStamped convert(const geometry_msgs::msg::TwistStamped &twist, const std::string &target_frame)

Convert a TwistStamped to a target frame, using the configured tf_timeout_threshold_.

Only the linear component is transformed (as a Vector3Stamped). The angular component is copied through unchanged: this is correct when both the source and target frames are inertial (or share the same orientation), which is the typical aerostack2 case (e.g. earth ↔ <ns>/odom).

Parameters:

  • twist – Input twist with valid header.frame_id and stamp.

  • target_frame – Target frame id.

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Twist expressed in target_frame.

nav_msgs::msg::Path convert(const nav_msgs::msg::Path &_path, const std::string &target_frame, const std::chrono::nanoseconds timeout)

Convert a nav_msgs::msg::Path to a target frame.

Each pose in _path.poses is transformed individually; the resulting path has header.frame_id = target_frame and keeps the original header.stamp.

Parameters:

  • _path – Input path. Each pose must carry a valid frame_id.

  • target_frame – Target frame id.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if any per-pose lookup fails.

Returns:

Path with all poses expressed in target_frame.

inline nav_msgs::msg::Path convert(const nav_msgs::msg::Path &path, const std::string &target_frame)

Convert a nav_msgs::msg::Path to a target frame, using the configured tf_timeout_threshold_.

Each pose in path.poses is transformed individually; the resulting path has header.frame_id = target_frame and keeps the original header.stamp.

Parameters:

  • path – Input path. Each pose must carry a valid frame_id.

  • target_frame – Target frame id.

Throws:

tf2::TransformException – if any per-pose lookup fails.

Returns:

Path with all poses expressed in target_frame.

as2_msgs::msg::TrajectorySetpoints convert(const as2_msgs::msg::TrajectorySetpoints &traj, const std::string &target_frame, const std::chrono::nanoseconds timeout)

Convert an as2_msgs::msg::TrajectorySetpoints to a target frame.

The trajectory header.frame_id is the frame in which both the position and the linear quantities (twist, acceleration) of every TrajectoryPoint are expressed. The transform is therefore applied as follows:

  • position: full transform (rotation + translation).

  • twist and acceleration: rotation only (they are vector quantities, not points, so the translation does not apply).

  • yaw_angle: the yaw component of the transform is added.

The yaw_angle handling is geometrically meaningful only when the transform between source and target frames does not introduce significant roll or pitch — which holds for transforms between inertial frames such as earth ↔ <ns>/odom in aerostack2 systems.

Parameters:

  • traj – Input trajectory with valid header.frame_id and stamp.

  • target_frame – Target frame id.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Trajectory expressed in target_frame.

inline as2_msgs::msg::TrajectorySetpoints convert(const as2_msgs::msg::TrajectorySetpoints &traj, const std::string &target_frame)

Convert an as2_msgs::msg::TrajectorySetpoints to a target frame, using the configured tf_timeout_threshold_.

The trajectory header.frame_id is the frame in which both the position and the linear quantities (twist, acceleration) of every TrajectoryPoint are expressed. The transform is therefore applied as follows:

  • position: full transform (rotation + translation).

  • twist and acceleration: rotation only (they are vector quantities, not points, so the translation does not apply).

  • yaw_angle: the yaw component of the transform is added.

The yaw_angle handling is geometrically meaningful only when the transform between source and target frames does not introduce significant roll or pitch — which holds for transforms between inertial frames such as earth ↔ <ns>/odom in aerostack2 systems.

Parameters:

  • traj – Input trajectory with valid header.frame_id and stamp.

  • target_frame – Target frame id.

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Trajectory expressed in target_frame.

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const std::chrono::nanoseconds timeout)

Obtain a PoseStamped from the TF buffer.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Pose of source_frame expressed in target_frame.

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const std::chrono::nanoseconds timeout)

Obtain a PoseStamped from the TF buffer.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as rclcpp::Time.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Pose of source_frame expressed in target_frame.

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time = tf2::TimePointZero)

Obtain a PoseStamped from the TF buffer using the configured tf_timeout_threshold_.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint. Defaults to tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Pose of source_frame expressed in target_frame.

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time)

Obtain a PoseStamped from the TF buffer using the configured tf_timeout_threshold_.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as rclcpp::Time.

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Pose of source_frame expressed in target_frame.

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const std::chrono::nanoseconds timeout)

Obtain a QuaternionStamped (orientation only) from the TF buffer.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Orientation of source_frame expressed in target_frame.

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const std::chrono::nanoseconds timeout)

Obtain a QuaternionStamped (orientation only) from the TF buffer.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as rclcpp::Time.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Orientation of source_frame expressed in target_frame.

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time = tf2::TimePointZero)

Obtain a QuaternionStamped from the TF buffer using the configured tf_timeout_threshold_.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint. Defaults to tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Orientation of source_frame expressed in target_frame.

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time)

Obtain a QuaternionStamped from the TF buffer using the configured tf_timeout_threshold_.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as rclcpp::Time.

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Orientation of source_frame expressed in target_frame.

geometry_msgs::msg::TransformStamped getTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time = tf2::TimePointZero)

Obtain a TransformStamped from the TF buffer.

Note

Unlike the other getters in this class, this overload uses the simple two-frame lookupTransform(target, source, time) without a fixed frame and without an explicit timeout. For a timeout-bearing variant see the 4-argument overload below.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint. Defaults to tf2::TimePointZero (latest).

Throws:

tf2::TransformException – if the lookup fails.

Returns:

Transform from source_frame to target_frame.

geometry_msgs::msg::TransformStamped getTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const std::chrono::nanoseconds timeout)

Obtain a TransformStamped from the TF buffer, blocking up to timeout for the transform to become available.

Uses the timeout-bearing two-frame lookupTransform(target, source, time, timeout) overload of tf2_ros::Buffer, without a fixed frame.

Parameters:

  • target_frame – Target frame id.

  • source_frame – Source frame id.

  • time – Lookup time, as tf2::TimePoint.

  • timeout – Lookup timeout. Zero returns immediately if the transform is not yet available.

Throws:

tf2::TransformException – if the lookup fails or the timeout expires.

Returns:

Transform from source_frame to target_frame.

template<typename T>
inline bool tryConvert(T &input, const std::string &target_frame, const std::chrono::nanoseconds timeout)

Try to convert input in place to a target frame.

On failure, the tf2::TransformException is caught internally, a warning is logged via the node logger, and false is returned. input is left unmodified in that case.

Parameters:

  • input – Variable to convert (modified in place on success).

  • target_frame – Target frame id.

  • timeout – Lookup timeout. Zero uses tf2::TimePointZero (latest).

Returns:

true if the conversion was successful, false otherwise.

template<typename T>
inline bool tryConvert(T &input, const std::string &target_frame)

Try to convert input in place to a target frame, using the configured tf_timeout_threshold_.

On failure, the tf2::TransformException is caught internally, a warning is logged via the node logger, and false is returned. input is left unmodified in that case.

Parameters:

  • input – Variable to convert (modified in place on success).

  • target_frame – Target frame id.

Returns:

true if the conversion was successful, false otherwise.

std::pair<geometry_msgs::msg::PoseStamped, geometry_msgs::msg::TwistStamped> getState(const geometry_msgs::msg::TwistStamped &_twist, const std::string &_twist_target_frame, const std::string &_pose_target_frame, const std::string &_pose_source_frame, const std::chrono::nanoseconds timeout)

Build a (pose, twist) pair expressed in the requested frames.

Steps:

  1. Transform the input twist to _twist_target_frame (linear part is rotated; angular part is kept — see convert(TwistStamped, ...)).

  2. Look up the pose of _pose_source_frame in _pose_target_frame, stamped at the converted twist’s header.stamp.

Parameters:

  • _twist – Input twist with valid header.frame_id and stamp.

  • _twist_target_frame – Frame in which the output twist is expressed.

  • _pose_target_frame – Target frame for the pose lookup.

  • _pose_source_frame – Source frame for the pose lookup.

  • timeout – Timeout used for both TF lookups.

Throws:

tf2::TransformException – if any of the two lookups fails.

Returns:

std::pair{ pose, twist } with the pose in _pose_target_frame and the twist in _twist_target_frame.

std::pair<geometry_msgs::msg::PoseStamped, geometry_msgs::msg::TwistStamped> getState(const geometry_msgs::msg::TwistStamped &_twist, const std::string &_twist_target_frame, const std::string &_pose_target_frame, const std::string &_pose_source_frame)

Build a (pose, twist) pair expressed in the requested frames, using the configured tf_timeout_threshold_.

Steps:

  1. Transform the input twist to _twist_target_frame (linear part is rotated; angular part is kept — see convert(TwistStamped, ...)).

  2. Look up the pose of _pose_source_frame in _pose_target_frame, stamped at the converted twist’s header.stamp.

Parameters:

  • _twist – Input twist with valid header.frame_id and stamp.

  • _twist_target_frame – Frame in which the output twist is expressed.

  • _pose_target_frame – Target frame for the pose lookup.

  • _pose_source_frame – Source frame for the pose lookup.

Throws:

tf2::TransformException – if any of the two lookups fails.

Returns:

std::pair{ pose, twist } with the pose in _pose_target_frame and the twist in _twist_target_frame.

Private Members

std::shared_ptr<tf2_ros::Buffer> tf_buffer_
std::shared_ptr<tf2_ros::TransformListener> tf_listener_
as2::Node *node_
std::chrono::nanoseconds tf_timeout_threshold_ = std::chrono::nanoseconds::zero()
class TFStatic
#include <sensor.hpp>

TFStatic object to publish static transforms in TF.

Subclassed by as2::sensors::Camera, as2::sensors::Gimbal, as2::sensors::Sensor< T >

Public Functions

explicit TFStatic(rclcpp::Node *node_ptr)

Construct a new TFStatic object.

virtual ~TFStatic()

Destroy the TFStatic object.

virtual void setStaticTransform(const geometry_msgs::msg::TransformStamped &transformStamped)

Set the Static Transform in TF.

Parameters:

transformStamped – TransformStamped message

virtual void setStaticTransform(const std::string &frame_id, const std::string &parent_frame_id, float x, float y, float z, float qx, float qy, float qz, float qw)

Set the Static Transform in TF.

Parameters:

  • frame_id – Frame ID

  • parent_frame_id – Parent Frame ID

  • x – X position (m)

  • y – Y position (m)

  • z – Z position (m)

  • qx – Quaternion X

  • qy – Quaternion Y

  • qz – Quaternion Z

  • qw – Quaternion W

virtual void setStaticTransform(const std::string &frame_id, const std::string &parent_frame_id, float x, float y, float z, float roll, float pitch, float yaw)

Set the Static Transform in TF.

Parameters:

  • frame_id – Frame ID

  • parent_frame_id – Parent Frame ID

  • x – X position (m)

  • y – Y position (m)

  • z – Z position (m)

  • roll – Roll (rad)

  • pitch – Pitch (rad)

  • yaw – Yaw (rad)

const rclcpp::Node *getNode() const

Get the Node Pointer object.

Returns:

rclcpp::Node* Node pointer

Private Functions

virtual void setStaticTransform_(const geometry_msgs::msg::TransformStamped &transformStamped)

Set the Static Transform in TF.

Parameters:

transformStamped – TransformStamped message

Private Members

rclcpp::Node *node_ptr_ = nullptr
class TrajectoryMotion : public as2::motionReferenceHandlers::BasicMotionReferenceHandler
#include <trajectory_motion.hpp>

The TrajectoryMotion class is a motion reference handler that sends a puntual trajectory reference to the robot. The trajectory point is given by a position, a yaw angle, a velocity and a acceleration.

Public Functions

explicit TrajectoryMotion(as2::Node *node_ptr, const std::string &ns = "")

TrajectoryMotion constructor.

Parameters:

node – as2::Node pointer.

inline ~TrajectoryMotion()

TrajectoryMotion destructor.

bool sendTrajectoryCommandWithYawAngle(const std::string &frame_id, const double x, const double y, const double z, const double yaw_angle, const double vx, const double vy, const double vz, const double ax, const double ay, const double az)

sendTrajectoryCommandWithYawAngle sends a trajectory command to the robot.

Parameters:

  • frame_id – frame id of trayectory point.

  • x – x coordinate of the trajectory point.

  • y – y coordinate of the trajectory point.

  • z – z coordinate of the trajectory point.

  • yaw_angle – yaw angle of the trajectory point.

  • vx – x velocity of the trajectory point.

  • vy – y velocity of the trajectory point.

  • vz – z velocity of the trajectory point.

  • ax – x acceleration of the trajectory point.

  • ay – y acceleration of the trajectory point.

  • az – z acceleration of the trajectory point.

Returns:

true if the command was sent successfully, false otherwise.

bool sendTrajectoryCommandWithYawAngle(const std::string &frame_id, const double yaw_angle, const std::vector<double> &positions, const std::vector<double> &velocities, const std::vector<double> &accelerations)

sendTrajectoryCommandWithYawAngle sends a trajectory command to the robot.

Parameters:

  • frame_id – frame id of trayectory point.

  • yaw_angle – yaw angle of the trajectory point.

  • positions – vector of positions of the trajectory point (x,y,z).

  • velocities – vector of velocities of the trajectory point (vx,vy,vz).

  • accelerations – vector of accelerations of the trajectory point (ax,ay,az).

Returns:

true if the command was sent successfully, false otherwise.

bool sendTrajectoryCommandWithYawAngle(const std::string &frame_id, const double yaw_angle, const Eigen::Vector3d &positions, const Eigen::Vector3d &velocities, const Eigen::Vector3d &accelerations)

sendTrajectoryCommandWithYawAngle sends a trajectory command to the robot.

Parameters:

  • frame_id – frame id of trayectory point.

  • yaw_angle – yaw angle of the trajectory point.

  • positions – vector of positions of the trajectory point (x,y,z).

  • velocities – vector of velocities of the trajectory point (vx,vy,vz).

  • accelerations – vector of accelerations of the trajectory point (ax,ay,az).

Returns:

true if the command was sent successfully, false otherwise.

bool sendTrajectorySetpoints(const as2_msgs::msg::TrajectorySetpoints &trajectory_setpoints)

sendTrajectorySetpoints sends a trajectory setpoints message to the robot.

Parameters:

trajectory_setpoints – trajectory setpoints message.

Returns:

true if the command was sent successfully, false otherwise.

struct UAV_command
#include <pid_speed_controller.hpp>

Output command produced by the PID handlers.

Public Members

Eigen::Vector3d velocity = Eigen::Vector3d::Zero()
double yaw_speed = 0.0
struct UAV_reference
#include <differential_flatness_controller.hpp>

Trajectory reference fed to the differential-flatness controller.

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()
Eigen::Vector3d velocity = Eigen::Vector3d::Zero()
Eigen::Vector3d acceleration = Eigen::Vector3d::Zero()
double yaw = 0.0
struct UAV_state
#include <differential_flatness_controller.hpp>

Cached UAV state used by the differential-flatness controller.

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()
Eigen::Vector3d velocity = Eigen::Vector3d::Zero()
tf2::Quaternion attitude_state = tf2::Quaternion::getIdentity()
struct UAV_state
#include <pid_speed_controller.hpp>

Cached UAV state used by the PID handlers.

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()
Eigen::Vector3d velocity = Eigen::Vector3d::Zero()
Eigen::Vector3d yaw = Eigen::Vector3d::Zero()
class UsbCameraInterface : public as2::Node

Public Functions

explicit UsbCameraInterface(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Construct a new UsbCameraInterface object.

inline ~UsbCameraInterface()

Destroy the UsbCameraInterface object.

Private Functions

void captureImage()
void setupCamera()

Private Members

std::shared_ptr<as2::sensors::Camera> camera_
cv::VideoCapture cap_
struct VehicleParameters
#include <Parameters.hpp>

A struct that holds properties of the vehicle such as mass, inertia and rotor configuration. Gravity is also included even though it’s not a parameter unique to the vehicle.

Public Members

double mass

Total mass of the vehicle.

Eigen::Matrix3d inertia

Moment of inertia matrix of the vehicle.

Eigen::Vector3d gravity

Gravity vector.

RotorConfiguration rotorConfiguration

A collection of Rotor objects that specifiy various properties of the rotors in the vehicle.

class VoronoiSearcher : public GraphSearcher<DynamicVoronoi>

Public Functions

void update_voronoi(const DynamicVoronoi &voronoi)

Protected Functions

virtual double calc_h_cost(Point2i current, Point2i end) override
virtual double calc_g_cost(Point2i current) override
virtual int hash_key(Point2i point) override
virtual bool cell_in_limits(Point2i point) override
virtual bool cell_occuppied(Point2i point) override
class WaitForAlert : public BT::DecoratorNode

Public Functions

WaitForAlert(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)

Public Static Functions

static inline BT::PortsList providedPorts()

Private Functions

BT::NodeStatus tick() override
void callback(as2_msgs::msg::AlertEvent::SharedPtr msg)

Private Members

rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
rclcpp::Subscription<as2_msgs::msg::AlertEvent>::SharedPtr sub_
std::string topic_name_
bool flag_ = false
class WaitForEvent : public BT::DecoratorNode

Public Functions

WaitForEvent(const std::string &xml_tag_name, const BT::NodeConfiguration &conf)

Public Static Functions

static inline BT::PortsList providedPorts()

Private Functions

BT::NodeStatus tick() override
void callback(std_msgs::msg::String::SharedPtr msg)

Private Members

rclcpp::Node::SharedPtr node_
rclcpp::CallbackGroup::SharedPtr callback_group_
rclcpp::executors::SingleThreadedExecutor callback_group_executor_
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_
std::string topic_name_
bool flag_ = false
namespace a_star
namespace as2

Typedefs

using Event = as2_msgs::msg::PlatformStateMachineEvent

Event type.

using Rate = as2::rate::GenericRate<std::chrono::system_clock>
using WallRate = as2::rate::GenericRate<std::chrono::steady_clock>

Functions

void spinLoop(std::shared_ptr<as2::Node> node, std::function<void()> run_function = nullptr)

Executes the main loop of the node.

Parameters:

  • node – node to execute the main loop

  • run_function – function to be executed in the main loop. Node frequency must be higher than 0

namespace control_mode

Functions

uint8_t convertAS2ControlModeToUint8t(const as2_msgs::msg::ControlMode &mode)
as2_msgs::msg::ControlMode convertUint8tToAS2ControlMode(uint8_t control_mode_uint8t)
std::string controlModeToString(const uint8_t control_mode_uint8t)
std::string controlModeToString(const as2_msgs::msg::ControlMode &mode)
constexpr uint8_t convertToUint8t(const as2_msgs::msg::ControlMode &mode)
constexpr uint8_t convertToUint8t(uint8_t control_mode_uint8t, uint8_t yaw_mode_uint8t, uint8_t reference_frame_uint8t)
inline bool compareModes(const uint8_t mode1, const uint8_t mode2, const uint8_t mask = MATCH_ALL)
inline bool compareModes(const as2_msgs::msg::ControlMode &mode1, const as2_msgs::msg::ControlMode &mode2, const uint8_t mask = MATCH_ALL)
inline bool isUnsetMode(const uint8_t control_mode_uint8t)
inline bool isUnsetMode(const as2_msgs::msg::ControlMode &mode)
inline bool isHoverMode(const uint8_t control_mode_uint8t)
inline bool isHoverMode(const as2_msgs::msg::ControlMode &mode)
void printControlMode(const as2_msgs::msg::ControlMode &mode)
void printControlMode(uint8_t control_mode_uint8t)
namespace frame

Functions

Eigen::Vector3d transform(const tf2::Quaternion &quaternion, const Eigen::Vector3d &vector)

Apply a quaternion rotation to a vector.

Parameters:

  • quaternion – tf2::Quaternion to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transform(const float roll_angle, const float pitch_angle, const float yaw_angle, const Eigen::Vector3d &vector)

Apply a quaternion rotation to a vector.

Parameters:

  • roll_angle – Roll angle to apply.

  • pitch_angle – Pitch angle to apply.

  • yaw_angle – Yaw angle to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transform(const geometry_msgs::msg::Quaternion &quaternion, const Eigen::Vector3d &vector)

Apply a quaternion rotation to a vector.

Parameters:

  • quaternion – geometry_msgs::Quaternion to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transform(const Eigen::Quaterniond &quaternion, const Eigen::Vector3d &vector)

Apply a quaternion rotation to a vector.

Parameters:

  • quaternion – Eigen::Quaterniond to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transformInverse(const tf2::Quaternion &quaternion, const Eigen::Vector3d &vector)

Apply a inverse quaternion rotation to a vector.

Parameters:

  • quaternion – tf2::Quaternion to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transformInverse(const float roll_angle, const float pitch_angle, const float yaw_angle, const Eigen::Vector3d &vector)

Apply a inverse quaternion rotation to a vector.

Parameters:

  • roll_angle – Roll angle to apply.

  • pitch_angle – Pitch angle to apply.

  • yaw_angle – Yaw angle to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transformInverse(const geometry_msgs::msg::Quaternion &quaternion, const Eigen::Vector3d &vector)

Apply a inverse quaternion rotation to a vector.

Parameters:

  • quaternion – geometry_msgs::Quaternion to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Eigen::Vector3d transformInverse(const Eigen::Quaterniond &quaternion, const Eigen::Vector3d &vector)

Apply a inverse quaternion rotation to a vector.

Parameters:

  • quaternion – Eigen::Quaterniond to apply.

  • vector – Eigen::Vector3d Vector to rotate.

  • quaternion – geometry_msgs::msg::Pose with the quaternion to apply.

  • vector – Eigen::Vector3d Vector to rotate.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

Returns:

Returns:

Eigen::Vector3d Rotated vector.

void quaternionToEuler(const tf2::Quaternion &quaternion, double &roll, double &pitch, double &yaw)

Convert quaternion to euler angles.

Parameters:

  • quaternion – tf2::Quaternion to convert.

  • roll – double pointer to store roll angle.

  • pitch – double pointer to store pitch angle.

  • yaw – double pointer to store yaw angle.

void quaternionToEuler(const geometry_msgs::msg::Quaternion &quaternion, double &roll, double &pitch, double &yaw)

Convert quaternion to euler angles.

Parameters:

  • quaternion – geometry_msgs::msg::Quaternion to convert.

  • roll – double pointer to store roll angle.

  • pitch – double pointer to store pitch angle.

  • yaw – double pointer to store yaw angle.

void quaternionToEuler(const Eigen::Quaterniond &quaternion, double &roll, double &pitch, double &yaw)

Convert quaternion to euler angles.

Parameters:

  • quaternion – Eigen::Quaternion to convert.

  • yaw – double pointer to store yaw angle.

void eulerToQuaternion(const double roll, const double pitch, const double yaw, tf2::Quaternion &quaternion)

Convert euler angles to quaternion.

Parameters:

  • roll – double roll angle.

  • pitch – double pitch angle.

  • yaw – double yaw angle.

  • quaternion – tf2::Quaternion pointer to store quaternion.

void eulerToQuaternion(const double roll, const double pitch, const double yaw, geometry_msgs::msg::Quaternion &quaternion)

Convert euler angles to quaternion.

Parameters:

  • roll – double roll angle.

  • pitch – double pitch angle.

  • yaw – double yaw angle.

  • quaternion – geometry_msgs::msg::Quaternion pointer to store quaternion.

void eulerToQuaternion(const double roll, const double pitch, const double yaw, Eigen::Quaterniond &quaternion)

Convert euler angles to quaternion.

Parameters:

  • roll – double roll angle.

  • pitch – double pitch angle.

  • yaw – double yaw angle.

  • quaternion – Eigen::Quaterniond pointer to store quaternion.

double getYawFromQuaternion(const tf2::Quaternion &quaternion)

Convert quaternion to euler angles.

Parameters:

  • quaternion – tf2::Quaternion to convert.

  • yaw – double pointer to store yaw angle.

double getYawFromQuaternion(const geometry_msgs::msg::Quaternion &quaternion)

Convert quaternion to euler angles.

Parameters:

  • quaternion – geometry_msgs::msg::Quaternion to convert.

  • yaw – double pointer to store yaw angle.

Returns:

Double yaw angle.

double getYawFromQuaternion(const Eigen::Quaterniond &quaternion)

Convert quaternion to euler angles.

Parameters:

  • quaternion – Eigen::Quaternion to convert.

  • yaw – double pointer to store yaw angle.

Returns:

Double yaw angle.

double getVector2DAngle(const double x, const double y)

Compute the angle between of a given vector in 2D and the unitary vector (1,0).

Parameters:

  • x – double x coordinate of the vector.

  • y – double y coordinate of the vector.

Returns:

Double yaw angle.

double wrapAngle0To2Pi(const double theta)

Wrap angle to [0, 2*pi].

Parameters:

theta – double angle.

Returns:

Double wrapped angle.

double wrapAnglePiToPi(const double theta)

Wrap angle to [-pi, pi].

Parameters:

theta – double angle.

Returns:

Double wrapped angle.

double angleMinError(const double theta1, const double theta2)

Compute the minumun angle between two angles. Maximun error is pi.

Parameters:

  • theta1 – double first angle.

  • theta2 – double second angle.

Returns:

Double yaw difference.

namespace gps

Functions

void Ecef2LatLon(const geometry_msgs::msg::PoseStamped &ps, geographic_msgs::msg::GeoPoseStamped &gps)

Variables

static const GeographicLib::Geocentric &earth = GeographicLib::Geocentric::WGS84()
static const char global_frame[] = "earth"
namespace motionReferenceHandlers
namespace rate
namespace sensors

Typedefs

using Odometry = Sensor<nav_msgs::msg::Odometry>
using Imu = Sensor<sensor_msgs::msg::Imu>
using GPS = Sensor<sensor_msgs::msg::NavSatFix>
using Lidar = Sensor<sensor_msgs::msg::LaserScan>
using Battery = Sensor<sensor_msgs::msg::BatteryState>
using Barometer = Sensor<sensor_msgs::msg::FluidPressure>
using Compass = Sensor<sensor_msgs::msg::MagneticField>
using RangeFinder = Sensor<sensor_msgs::msg::Range>
namespace tf

Functions

std::string generateTfName(const std::string &_namespace, const std::string &_frame_name)

Prefix a TF frame name with a namespace, following aerostack2 conventions.

Rules applied:

  • If _frame_name is empty, throws std::runtime_error.

  • If _frame_name starts with ‘/’, it is treated as absolute and returned without the leading ‘/’ (no namespace prefix is added).

  • If _frame_name already starts with <_namespace>/, it is returned as is.

  • If _namespace is empty, _frame_name is returned unchanged and a warning is logged (potential frame conflict).

  • Otherwise, returns “<namespace>/<frame_name>”, stripping any leading ‘/’ in _namespace.

Parameters:

  • _namespace – Robot/node namespace. May be empty or start with ‘/’.

  • _frame_name – Frame name, possibly absolute (leading ‘/’) or already prefixed.

Throws:

std::runtime_error – if _frame_name is empty.

Returns:

Fully-qualified TF frame id.

std::string generateTfName(rclcpp::Node *node, std::string _frame_name)

Convenience overload that uses node->get_namespace() as namespace.

Parameters:

  • node – ROS 2 node whose namespace will be used as prefix.

  • _frame_name – Frame name (see generateTfName(namespace, frame_name) for rules).

Throws:

std::runtime_error – if _frame_name is empty.

Returns:

Fully-qualified TF frame id.

geometry_msgs::msg::TransformStamped getTransformation(const std::string &_frame_id, const std::string &_child_frame_id, double _translation_x, double _translation_y, double _translation_z, double _roll, double _pitch, double _yaw)

Build a TransformStamped from a translation and Euler-angle rotation.

The rotation is built with tf2::Quaternion::setRPY(roll, pitch, yaw), i.e. the standard ROS convention (intrinsic rotations applied in roll-pitch-yaw order around the body axes).

Note

The returned message has its header.stamp left default-initialized; the caller is responsible for stamping it before broadcasting.

Parameters:

  • _frame_id – Parent frame id (header.frame_id).

  • _child_frame_id – Child frame id (child_frame_id).

  • _translation_x – Translation along X, in meters.

  • _translation_y – Translation along Y, in meters.

  • _translation_z – Translation along Z, in meters.

  • _roll – Roll angle, in radians.

  • _pitch – Pitch angle, in radians.

  • _yaw – Yaw angle, in radians.

Returns:

TransformStamped describing parent → child.

namespace yaml

Functions

std::filesystem::path get_project_export_path_from_xml_path(const std::filesystem::path &xml_path)
std::vector<std::filesystem::path> find_yaml_files_inside(const std::filesystem::path &dir)
YAML::Node find_tag_across_multiple_yaml_files(const std::vector<std::filesystem::path> &yaml_files, const std::string &tag)
uint8_t parse_uint_from_string(const std::string &str)
std::vector<uint8_t> parse_uint_from_string(const std::vector<std::string> &str_v)
YAML::Node find_tag_in_yaml_node(const YAML::Node &node, const std::string &tag)
template<typename T = std::string>
std::vector<T> find_tag_in_yaml_file(const std::filesystem::path &yaml_file, const std::string &tag)
template<typename T = std::string>
std::vector<T> find_tag_from_project_exports_path(const std::filesystem::path &project_exports_path, const std::string &tag)
YAML::Node search_tag_across_multiple_yaml_files(const std::vector<std::filesystem::path> &yaml_files, const std::string &tag)
namespace as2_behavior

Enums

enum class ExecutionStatus

Values:

enumerator SUCCESS
enumerator RUNNING
enumerator FAILURE
enumerator ABORTED
enumerator PAUSED
namespace as2_behavior_tree
namespace as2_behaviors_path_planning
namespace as2_map_server

Typedefs

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
namespace as2_map_server_plugin_base
namespace as2_motion_controller_param_utils

Functions

template<typename T>
T readParam(rclcpp::Node *node, const std::string &name)

Read a scalar parameter of type T from the node.

Specializations are provided for bool, int64_t, double and std::string. Throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter was not declared, or rclcpp::ParameterTypeException on type mismatch.

Parameters:

  • node – Pointer to the ROS 2 node holding the parameters.

  • name – Fully-qualified parameter name.

Returns:

Parameter value of type T.

template<>
inline bool readParam<bool>(rclcpp::Node *node, const std::string &name)
template<>
inline int64_t readParam<int64_t>(rclcpp::Node *node, const std::string &name)
template<>
inline double readParam<double>(rclcpp::Node *node, const std::string &name)
template<>
inline std::string readParam<std::string>(rclcpp::Node *node, const std::string &name)
template<std::size_t N>
std::array<double, N> readArray(rclcpp::Node *node, const std::string &name)

Read a fixed-size double array parameter into std::array<double, N>.

The runtime size is checked against the compile-time N. A size mismatch is treated as fatal: the function logs RCLCPP_FATAL and throws rclcpp::exceptions::InvalidParameterValueException so the controller does not silently run with a half-configured solver.

Template Parameters:

N – Expected number of elements in the array.

Parameters:

  • node – Pointer to the ROS 2 node.

  • name – Fully-qualified parameter name.

Returns:

Fixed-size std::array<double, N> with the values.

std::vector<double> readDoubleArray(rclcpp::Node *node, const std::string &name, std::size_t expected_size = 0)

Read a variable-size double array parameter.

If expected_size != 0, the size is validated and a mismatch is fatal (RCLCPP_FATAL + throw). When expected_size == 0, any size is accepted.

Parameters:

  • node – Pointer to the ROS 2 node.

  • name – Fully-qualified parameter name.

  • expected_size – Expected number of elements, or 0 to skip the check.

Returns:

Vector with the parameter values.

inline Eigen::Vector3d readVector3(rclcpp::Node *node, const std::string &name)

Read a 3-component double array parameter as Eigen::Vector3d.

Parameters:

  • node – Pointer to the ROS 2 node.

  • name – Fully-qualified parameter name.

Returns:

Eigen::Vector3d with the values.

bool isNanSentinel(const std::vector<double> &values)

True if every element of values is NaN.

Used as a sentinel for “intentionally empty” optional double-array parameters (e.g. unconstrained limits in MPC plugins). The YAML keeps a non-empty array because ROS 2 forbids empty array overrides.

Parameters:

values – Array values to test.

Returns:

true if values is non-empty and every element is NaN.

namespace as2_motion_controller_plugin_base
namespace as2_motion_controller_test
namespace as2_names
namespace actions
namespace behaviors

Variables

const char takeoff[] = "TakeoffBehavior"
const char gotowaypoint[] = "GoToBehavior"
const char followreference[] = "FollowReferenceBehavior"
const char followpath[] = "FollowPathBehavior"
const char land[] = "LandBehavior"
const char trajectorygenerator[] = "TrajectoryGeneratorBehavior"
namespace services

Variables

const char set_speed[] = ""
namespace behavior

Variables

const char package_pickup[] = "behavior/package_pickup"
const char package_unpick[] = "behavior/package_unpick"
const char dynamic_land[] = "behavior/dynamic_land"
const char dynamic_follower[] = "behavior/dynamic_follower"
namespace controller

Variables

const char set_control_mode[] = "controller/set_control_mode"
const char list_control_modes[] = "controller/list_control_modes"
namespace gps

Variables

const char get_origin[] = "get_origin"
const char set_origin[] = "set_origin"
const char path_to_geopath[] = ""
const char geopath_to_path[] = ""
namespace motion_reference

Variables

const char send_traj_wayp[] = "traj_gen/send_traj_wayp"
const char add_traj_wayp[] = "traj_gen/add_traj_wayp"
const char set_traj_speed[] = "traj_gen/set_traj_speed"
namespace platform

Variables

const char set_arming_state[] = "set_arming_state"
const char set_offboard_mode[] = "set_offboard_mode"
const char set_platform_control_mode[] = "set_platform_control_mode"
const char takeoff[] = "platform_takeoff"
const char land[] = "platform_land"
const char set_platform_state_machine_event[] = "platform/state_machine_event"
const char list_control_modes[] = "platform/list_control_modes"
namespace topics
namespace actuator_command

Variables

const rclcpp::QoS qos = rclcpp::SensorDataQoS()
const char pose[] = "actuator_command/pose"
const char twist[] = "actuator_command/twist"
const char thrust[] = "actuator_command/thrust"
const char trajectory[] = "actuator_command/trajectory"
namespace controller

Variables

const rclcpp::QoS qos_info = rclcpp::QoS(10)
const char info[] = "controller/info"
namespace follow_target

Variables

const rclcpp::QoS qos_info = rclcpp::QoS(10)
const char info[] = "follow_target/info"
namespace global

Variables

const rclcpp::QoS qos = rclcpp::QoS(10)
const char alert_event[] = "alert_event"
namespace ground_truth

Variables

const rclcpp::QoS qos = rclcpp::SensorDataQoS()
const char pose[] = "ground_truth/pose"
const char twist[] = "ground_truth/twist"
namespace motion_reference

Variables

const rclcpp::QoS qos = rclcpp::SensorDataQoS()
const rclcpp::QoS qos_waypoint = rclcpp::QoS(10)
const rclcpp::QoS qos_trajectory = rclcpp::QoS(10)
const char thrust[] = "motion_reference/thrust"
const char pose[] = "motion_reference/pose"
const char twist[] = "motion_reference/twist"
const char trajectory[] = "motion_reference/trajectory"
const char modify_waypoint[] = "motion_reference/modify_waypoint"
const char traj_gen_info[] = "motion_reference/traj_gen_info"
namespace platform

Variables

const rclcpp::QoS qos = rclcpp::QoS(10)
const char info[] = "platform/info"
namespace self_localization

Variables

const rclcpp::QoS qos = rclcpp::SensorDataQoS()
const char odom[] = "self_localization/odom"
const char pose[] = "self_localization/pose"
const char twist[] = "self_localization/twist"
namespace sensor_measurements

Variables

const rclcpp::QoS qos = rclcpp::SensorDataQoS()
const char base[] = "sensor_measurements/"
const char imu[] = "sensor_measurements/imu"
const char lidar[] = "sensor_measurements/lidar"
const char gps[] = "sensor_measurements/gps"
const char camera[] = "sensor_measurements/camera"
const char battery[] = "sensor_measurements/battery"
const char odom[] = "sensor_measurements/odom"
namespace as2_platform_multirotor_simulator
namespace as2_rviz_plugins
namespace as2_state_estimator
namespace as2_state_estimator_plugin_base
namespace BT

Functions

template<>
inline geometry_msgs::msg::Pose convertFromString(BT::StringView str)
namespace controller_handler

Functions

static inline bool checkMatchWithMask(const uint8_t mode1, const uint8_t mode2, const uint8_t mask)
static uint8_t findBestMatchWithMask(const uint8_t mode, const std::vector<uint8_t> &mode_list, const uint8_t mask)
namespace controller_manager
namespace dc_servo
namespace differential_flatness_controller
namespace [anonymous]
namespace dynamic_mav_trajectory_generator
namespace [anonymous]
namespace follow_path_base
namespace follow_path_plugin_position
namespace follow_path_plugin_trajectory
namespace follow_reference_base
namespace follow_reference_plugin_position
namespace follow_reference_plugin_trajectory

Variables

constexpr char kFollowReferenceWaypointId[] = "follow_reference_target"
namespace force_estimation_behavior
namespace gazebo_platform
namespace gcopter_trajectory_generator
namespace generate_polynomial_trajectory_behavior_plugin_base

Functions

template<typename T>
inline void getParameter(as2::Node *node_ptr, const std::string &param_name, T &param_value, bool use_default = false)

Declare (if needed) and read a ROS 2 parameter on the hosting node.

Template Parameters:

T – Parameter type.

Parameters:

  • node_ptr – Node pointer.

  • param_name – Fully-qualified parameter name.

  • param_value – [in] default value used when use_default is true, [out] read value on success.

  • use_default – Whether param_value is taken as a fallback default.

Variables

constexpr double kDegenerateDistanceM = 0.05

Distance threshold (in metres) under which the host behaviour short-circuits the plugin and publishes a degenerate-hold trajectory: the last waypoint is sent as a static reference with zero velocity and zero acceleration.

namespace GeographicLib
namespace go_to_base
namespace go_to_plugin_position
namespace go_to_plugin_trajectory
namespace gripper_behavior
namespace gripper_behavior_plugin_base
namespace ground_truth
namespace ground_truth_odometry_fuse
namespace gz
namespace sim
namespace GZ_SIM_VERSION_NAMESPACE
namespace systems
namespace multicopter_control

Typedefs

typedef std::vector<Rotor> RotorConfiguration

A collection of Rotor objects.

Functions

RotorConfiguration loadRotorConfiguration(const EntityComponentManager &_ecm, const sdf::ElementPtr &_sdf, const Model &_model, const Entity &_comLink)

Loads rotor configuration from SDF.

Parameters:

  • _ecm – [in] Immutable reference to the entity component manager

  • _sdf – [in] Pointer to the SDF element of the system

  • _model – [in] Model to which the system is attached

  • _comLink – [in] Link associated with the center of mass.

void createFrameDataComponents(EntityComponentManager &_ecm, const Entity &_link)

Creates components necessary for obtaining the frame data of the given link.

Parameters:

  • _ecm – [in] Mutable reference to the entity component manager

  • _link – [in] Link on which the components will be created.

std::optional<FrameData> getFrameData(const EntityComponentManager &_ecm, const Entity &_link, const NoiseParameters &_noise)

Retrieves the frame data of the given link and applies noise.

Parameters:

  • _ecm – [in] Imutable reference to the entity component manager

  • _link – [in] Link on which the components will be created.

  • _noise – [in] Noise parameters

namespace systems
namespace indi_controller

Functions

template<typename Vector>
Vector sqrt_keep_sign(const Vector &vector)

Compute element-wise square root and keep the sign.

This function computes the square root of each element of the input vector and retains the original sign of each element.

Template Parameters:

Vector – The type of the input vector (Eigen vector type)

Parameters:

vector – The input vector

Returns:

Vector The resulting vector with square root of elements and original sign

namespace jerk_limited_trajectory_generator
namespace land_base
namespace land_plugin_platform
namespace land_plugin_position
namespace land_plugin_speed
namespace land_plugin_trajectory
namespace mass_estimation_behavior
namespace mav_trajectory_generator
namespace [anonymous]
namespace mbzirc
namespace mocap_pose
namespace pid_controller
namespace pid_speed_controller
namespace [anonymous]
namespace Pnpoly

Functions

template<typename T>
inline bool isEqual(T a, T b)
Parameters:

  • a –

  • b –

Returns:

true if a and b are (almost - in case of floating points) equal

template<typename T>
inline std::vector<std::array<T, 2>> getConvexHull(const std::vector<std::array<T, 2>> &polygon)

Compute convex hull using Andrew’s monotone chain algorithm.

Parameters:

polygon –

Returns:

convex hull

template<typename T>
inline bool isIn(std::vector<std::array<T, 2>> &polygon, std::array<T, 2> &p)
Parameters:

  • polygon – describing a geofenced area

  • p – point to test whether inside or not

Returns:

true if p is inside the polygon OR when p is any vertex OR on an edge of the convex hull

namespace point_gimbal_behavior
namespace raw_odometry
namespace rclcpp
namespace real_sense_interface
namespace rviz_common
namespace scan2occ_grid
namespace std

STL namespace.

namespace chrono_literals
namespace takeoff_base
namespace takeoff_plugin_platform
namespace takeoff_plugin_position
namespace takeoff_plugin_speed
namespace takeoff_plugin_trajectory
namespace two_fingers
namespace usb_camera_interface
namespace utils

Functions

Point2i pointToCell(geometry_msgs::msg::PointStamped point, nav_msgs::msg::MapMetaData map_info, std::string target_frame_id, std::shared_ptr<tf2_ros::Buffer> tf_buffer)

Convert a point to a cell in the occupancy grid.

Parameters:

  • point – point to convert

  • map_info – occupancy grid metadata

  • target_frame_id – target frame id

  • tf_buffer – tf buffer

Point2i poseToCell(geometry_msgs::msg::PoseStamped pose, nav_msgs::msg::MapMetaData map_info, std::string target_frame_id, std::shared_ptr<tf2_ros::Buffer> tf_buffer)

Convert a pose to a cell in the occupancy grid.

Parameters:

  • pose – pose to convert

  • map_info – occupancy grid metadata

  • target_frame_id – target frame id

  • tf_buffer – tf buffer

geometry_msgs::msg::PointStamped cellToPoint(int cell_x, int cell_y, nav_msgs::msg::MapMetaData map_info, std_msgs::msg::Header map_header)

Convert a cell to a point in the map frame.

Parameters:

  • cell_x – cell x

  • cell_y – cell y

  • map_info – occupancy grid metadata

  • map_header – occupancy grid header

geometry_msgs::msg::PointStamped cellToPoint(Point2i cell, nav_msgs::msg::MapMetaData map_info, std_msgs::msg::Header map_header)

Convert a cell to a point in the map frame.

Parameters:

  • cell – cell to convert

  • map_info – occupancy grid metadata

  • map_header – occupancy grid header

namespace voronoi
file as2_platform_gazebo.hpp
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include “as2_core/aerial_platform.hpp”
#include “as2_core/core_functions.hpp”
#include “as2_core/names/topics.hpp”
#include “as2_core/utils/control_mode_utils.hpp”
#include “as2_core/utils/tf_utils.hpp”
#include “as2_msgs/msg/thrust.hpp”
#include “as2_msgs/msg/acro.hpp”

Implementation of an Gazebo UAV platform

Authors

Rafael Pérez Seguí

file as2_platform_gazebo.cpp
#include “as2_platform_gazebo.hpp”

Implementation of an Gazebo UAV platform

ROS2 node for gazebo platform

Authors

Rafael Pérez Seguí

Authors

Rafael Pérez Seguí

file as2_platform_gazebo_node.cpp
#include “as2_platform_gazebo.hpp”

Functions

int main(int argc, char *argv[])
file as2_interface.hpp
#include <cassert>
#include <string>
#include <vector>
#include “multirotor_simulator.hpp”
#include “as2_core/aerial_platform.hpp”
#include “as2_core/utils/tf_utils.hpp”
#include “as2_core/utils/frame_utils.hpp”
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <as2_msgs/msg/trajectory_setpoints.hpp>
#include <as2_msgs/msg/trajectory_point.hpp>

As2MultirotorSimulatorInterface class definition

Authors

Rafael Pérez Seguí

file as2_platform_multirotor_simulator.hpp
#include <string>
#include <memory>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include “as2_core/aerial_platform.hpp”
#include “as2_core/core_functions.hpp”
#include “as2_core/names/topics.hpp”
#include “as2_core/sensor.hpp”
#include “as2_core/utils/control_mode_utils.hpp”
#include “as2_core/utils/frame_utils.hpp”
#include “as2_core/utils/tf_utils.hpp”
#include “as2_core/utils/gps_utils.hpp”
#include “as2_msgs/msg/control_mode.hpp”
#include “as2_msgs/msg/gimbal_control.hpp”
#include “as2_interface.hpp”
#include “multirotor_simulator.hpp”

MultirotorSimulatorPlatform class definition

Author

Rafael Perez-Segui r.psegui@upm.es

file as2_interface.cpp
#include “as2_platform_multirotor_simulator/as2_interface.hpp”
#include “as2_core/core_functions.hpp”
#include “as2_core/utils/frame_utils.hpp”
#include “as2_core/utils/tf_utils.hpp”
#include “as2_core/utils/control_mode_utils.hpp”

As2MultirotorSimulatorInterface class implementation

Author

Rafael Perez-Segui r.psegui@upm.es

file as2_platform_multirotor_simulator.cpp
#include “as2_platform_multirotor_simulator/as2_platform_multirotor_simulator.hpp”
#include “as2_core/utils/frame_utils.hpp”
#include “as2_core/utils/tf_utils.hpp”
#include “as2_core/utils/control_mode_utils.hpp”

MultirotorSimulatorPlatform class implementation

Author

Rafael Perez-Segui r.psegui@upm.es

file as2_platform_multirotor_simulator_node.cpp
#include “as2_platform_multirotor_simulator/as2_platform_multirotor_simulator.hpp”
#include “as2_core/core_functions.hpp”

MultirotorSimulatorPlatform node implementation

Author

Rafael Perez-Segui r.psegui@upm.es

Functions

int main(int argc, char *argv[])
file sim_clock_publisher_node.cpp
#include <chrono>
#include <cstdint>
#include <rclcpp/rclcpp.hpp>
#include <rosgraph_msgs/msg/clock.hpp>

External /clock authority for faster-than-real-time simulation of the multirotor platform.

Author

Rafael Perez-Segui r.psegui@upm.es

Functions

int main(int argc, char **argv)
file arm_service.hpp
#include <memory>
#include <string>
#include “as2_behavior_tree/bt_service_node.hpp”
#include <std_srvs/srv/set_bool.hpp>

Arm and disarm services implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file echo.hpp
#include <string>
#include “as2_behavior_tree/bt_action_node.hpp”
#include “rclcpp/rclcpp.hpp”

Echo implementation as behavior tree node. Just for testing purpouses

Echo implementation as behavior tree node. Just for testing

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file follow_path.hpp
#include <string>
#include <memory>
#include <vector>
#include “as2_behavior_tree/bt_action_node.hpp”
#include “as2_core/names/actions.hpp”
#include “as2_msgs/action/follow_path.hpp”
#include “as2_msgs/msg/pose_with_id.hpp”
#include “as2_msgs/msg/yaw_mode.hpp”

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file get_origin.hpp
#include <string>
#include “as2_behavior_tree/bt_service_node.hpp”
#include “as2_msgs/srv/get_origin.hpp”

Get origin implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file go_to_action.hpp
#include <string>
#include <memory>
#include “as2_core/names/actions.hpp”
#include “as2_msgs/action/go_to_waypoint.hpp”
#include “as2_behavior_tree/bt_action_node.hpp”
#include “as2_behavior_tree/port_specialization.hpp”
#include “geometry_msgs/msg/point_stamped.hpp”

Go to action implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file go_to_gps_action.hpp
#include <iterator>
#include <string>
#include <memory>
#include “as2_behavior_tree/bt_action_node.hpp”
#include “as2_core/names/actions.hpp”
#include “as2_msgs/action/go_to_waypoint.hpp”
#include “as2_msgs/srv/geopath_to_path.hpp”
#include “geometry_msgs/msg/point.hpp”

Go to Gps action implementation as behavior tree node

Go to gps action implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file gps_to_cartesian.hpp
#include <iterator>
#include <string>
#include “as2_behavior_tree/bt_service_node.hpp”
#include “as2_behavior_tree/port_specialization.hpp”
#include “as2_msgs/srv/geopath_to_path.hpp”
#include “geometry_msgs/msg/pose.hpp”
#include “rclcpp/rclcpp.hpp”

GPS to Cartesian implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file land_action.hpp
#include <string>
#include <memory>
#include “as2_core/names/actions.hpp”
#include “as2_msgs/action/land.hpp”
#include “as2_behavior_tree/bt_action_node.hpp”

Land action implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file offboard_service.hpp
#include <string>
#include <memory>
#include “as2_behavior_tree/bt_service_node.hpp”
#include <std_srvs/srv/set_bool.hpp>

Offboard service implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file send_event.hpp
#include <string>
#include “as2_behavior_tree/bt_action_node.hpp”
#include “std_msgs/msg/string.hpp”

Send event implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file set_origin.hpp
#include <string>
#include “as2_behavior_tree/bt_service_node.hpp”
#include “as2_msgs/srv/set_origin.hpp”

Set origin implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file takeoff_action.hpp
#include <string>
#include <chrono>
#include <thread>
#include <memory>
#include “as2_behavior_tree/bt_action_node.hpp”
#include “as2_core/names/actions.hpp”
#include “as2_msgs/action/takeoff.hpp”

Takeoff action implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file bt_action_node.hpp
#include <chrono>
#include <memory>
#include <string>
#include <set>
#include <rclcpp/executors.hpp>
#include <rclcpp/allocator/allocator_common.hpp>
#include “behaviortree_cpp/action_node.h”
#include “behaviortree_cpp/bt_factory.h”
#include “rclcpp_action/rclcpp_action.hpp”
file bt_service_node.hpp
#include <memory>
#include <string>
#include <set>
#include <rclcpp/executors.hpp>
#include <rclcpp/allocator/allocator_common.hpp>
#include “behaviortree_cpp/bt_factory.h”
#include “rclcpp/rclcpp.hpp”
file is_flying_condition.hpp
#include <string>
#include “behaviortree_cpp/condition_node.h”
#include “as2_core/names/topics.hpp”
#include “as2_msgs/msg/platform_info.hpp”
#include “as2_msgs/msg/platform_status.hpp”
#include “rclcpp/rclcpp.hpp”

Behavior tree node to check if an aircraft is flying

behavior tree node to check if an aircraft is flying

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file wait_for_alert.hpp
#include <string>
#include “behaviortree_cpp/decorator_node.h”
#include “as2_msgs/msg/alert_event.hpp”
#include “rclcpp/rclcpp.hpp”

Wait for alert implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file wait_for_event.hpp
#include <string>
#include “behaviortree_cpp/decorator_node.h”
#include “geometry_msgs/msg/pose.hpp”
#include “rclcpp/rclcpp.hpp”
#include “std_msgs/msg/string.hpp”

Wait for event implementation as behavior tree node

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file port_specialization.hpp
#include <vector>
#include “behaviortree_cpp/bt_factory.h”
#include “geometry_msgs/msg/point_stamped.hpp”
#include “geometry_msgs/msg/pose.hpp”
#include “geometry_msgs/msg/pose_stamped.hpp”
#include “as2_msgs/msg/pose_with_id.hpp”

Authors

Pedro Arias Pérez Rafael Perez-Segui Miguel Fernández Cortizas

file arm_service.cpp
#include “as2_behavior_tree/action/arm_service.hpp”
file echo.cpp
#include “as2_behavior_tree/action/echo.hpp”
file get_origin.cpp
#include “as2_behavior_tree/action/get_origin.hpp”
file go_to_action.cpp
#include “as2_behavior_tree/action/go_to_action.hpp”
file go_to_gps_action.cpp
#include “as2_behavior_tree/action/go_to_gps_action.hpp”
#include “rclcpp/rclcpp.hpp”
file gps_to_cartesian.cpp
#include “as2_behavior_tree/action/gps_to_cartesian.hpp”
file land_action.cpp
#include “as2_behavior_tree/action/land_action.hpp”
file offboard_service.cpp
#include “as2_behavior_tree/action/offboard_service.hpp”
file send_event.cpp
#include “as2_behavior_tree/action/send_event.hpp”
file set_origin.cpp
#include “as2_behavior_tree/action/set_origin.hpp”
file takeoff_action.cpp
#include “as2_behavior_tree/action/takeoff_action.hpp”
file is_flying_condition.cpp
#include “as2_behavior_tree/condition/is_flying_condition.hpp”
file wait_for_alert.cpp
#include “as2_behavior_tree/decorator/wait_for_alert.hpp”
file wait_for_event.cpp
#include “as2_behavior_tree/decorator/wait_for_event.hpp”
file as2_behavior_tree_node.cpp
#include <behaviortree_cpp/loggers/groot2_publisher.h>
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>
#include <chrono>
#include <thread>
#include “rclcpp/rclcpp.hpp”
#include “as2_behavior_tree/action/arm_service.hpp”
#include “as2_behavior_tree/action/echo.hpp”
#include “as2_behavior_tree/action/follow_path.hpp”
#include “as2_behavior_tree/action/get_origin.hpp”
#include “as2_behavior_tree/action/go_to_action.hpp”
#include “as2_behavior_tree/action/go_to_gps_action.hpp”
#include “as2_behavior_tree/action/gps_to_cartesian.hpp”
#include “as2_behavior_tree/action/land_action.hpp”
#include “as2_behavior_tree/action/offboard_service.hpp”
#include “as2_behavior_tree/action/send_event.hpp”
#include “as2_behavior_tree/action/set_origin.hpp”
#include “as2_behavior_tree/action/takeoff_action.hpp”
#include “as2_behavior_tree/condition/is_flying_condition.hpp”
#include “as2_behavior_tree/decorator/wait_for_alert.hpp”
#include “as2_behavior_tree/decorator/wait_for_event.hpp”

Functions

int main(int argc, char *argv[])
file behavior_server__class.hpp
#include <string>
#include <memory>
#include <as2_behavior/behavior_utils.hpp>
#include <as2_core/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/service.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/server.hpp>

Class definition for a behavior server.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file behavior_server__impl.hpp
#include “as2_behavior/__detail/behavior_server__class.hpp”
file as2_basic_behavior.hpp
#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include “as2_core/aerial_platform.hpp”
#include “as2_core/node.hpp”
#include “as2_core/sensor.hpp”
#include “as2_msgs/msg/thrust.hpp”
#include “geometry_msgs/msg/pose_stamped.hpp”
#include “geometry_msgs/msg/twist_stamped.hpp”
#include “nav_msgs/msg/odometry.hpp”
#include “rclcpp_action/rclcpp_action.hpp”
#include “sensor_msgs/msg/battery_state.hpp”
#include “sensor_msgs/msg/imu.hpp”

Class definition for a basic behavior.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file as2_basic_behavior.hpp
#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include “as2_core/aerial_platform.hpp”
#include “as2_core/node.hpp”
#include “as2_core/sensor.hpp”
#include “as2_msgs/msg/thrust.hpp”
#include “geometry_msgs/msg/pose_stamped.hpp”
#include “geometry_msgs/msg/twist_stamped.hpp”
#include “nav_msgs/msg/odometry.hpp”
#include “rclcpp_action/rclcpp_action.hpp”
#include “sensor_msgs/msg/battery_state.hpp”
#include “sensor_msgs/msg/imu.hpp”

Aerostack2 basic behavior virtual class header file.

Authors

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file behavior_server.hpp
#include “as2_behavior/__impl/behavior_server__impl.hpp”

Class definition for a behavior server.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file behavior_utils.hpp
#include <string>
#include <as2_msgs/msg/behavior_status.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>

Class definition for behavior utils.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file behavior_client.cpp
#include “behavior_client.hpp”

Source file for the behavior client.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file behavior_server.cpp
#include “behavior_server.hpp”

Source file for the behavior server.

Author

Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file follow_path_base.hpp
#include <Eigen/Dense>
#include <memory>
#include <string>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include “as2_behavior/behavior_server.