aerostack2

struct Acro_command

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::SharedPtr 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 nav2_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 nav2_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 nav2_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_

struct Control_flags

Public Members

bool parameters_read = false

bool state_received = false

bool ref_received = false

struct Control_flags

Public Members

bool state_received = false

bool ref_received = false

bool plugin_parameters_read = false

bool position_controller_parameters_read = false

bool velocity_controller_parameters_read = false

bool speed_in_a_plane_controller_parameters_read = false

bool trajectory_controller_parameters_read = false

bool yaw_controller_parameters_read = false

class ControllerBase

Subclassed by differential_flatness_controller::Plugin, pid_speed_controller::Plugin

Public Functions

inline ControllerBase()

inline void initialize(as2::Node *node_ptr)

inline virtual void ownInitialize()

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

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

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

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

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

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

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

virtual bool updateParams(const std::vector<rclcpp::Parameter> &_params_list) = 0

virtual void reset() = 0

inline virtual std::string getDesiredPoseFrameId()

inline virtual std::string getDesiredTwistFrameId()

inline virtual ~ControllerBase()

Protected Functions

inline as2::Node *getNodePtr()

Protected Attributes

as2::Node *node_ptr_

class ControllerHandler

Public Functions

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

inline virtual ~ControllerHandler()

rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)

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

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

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

void reset()

Protected Attributes

as2::Node *node_ptr_

Private Functions

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

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

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

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

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

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

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

bool listPlatformAvailableControlModes()

void controlTimerCallback()

std::string getFrameIdByReferenceFrame(uint8_t reference_frame)

bool findSuitableOutputControlModeForPlatformInputMode(uint8_t &output_mode, const uint8_t input_mode)

bool checkSuitabilityInputMode(uint8_t &input_mode, const uint8_t output_mode)

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

bool findSuitableControlModes(uint8_t &input_mode, uint8_t &output_mode)

bool trySetPlatformHover()

bool tryToBypassController(const uint8_t input_mode, uint8_t &output_mode)

void sendCommand()

void publishCommand()

Private Members

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::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 motion_reference_adquired_ = false

bool state_adquired_ = false

bool use_bypass_ = false

bool bypass_controller_ = false

uint8_t prefered_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_

class ControllerManager : public as2::Node 

Public Functions

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

~ControllerManager()

Public Members

double cmd_freq_

Private Functions

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

void modeTimerCallback()

Private Members

double info_freq_

std::filesystem::path plugin_name_

std::filesystem::path available_modes_config_file_

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.

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 nav2_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 DynamicPolynomialTrajectoryGenerator : public as2_behavior::BehaviorServer<as2_msgs::action::GeneratePolynomialTrajectory>

Public Functions

DynamicPolynomialTrajectoryGenerator(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

inline ~DynamicPolynomialTrajectoryGenerator()

Private Functions

bool on_activate(std::shared_ptr<const as2_msgs::action::GeneratePolynomialTrajectory::Goal> goal) override: As2 Behavior methods

bool on_modify(std::shared_ptr<const as2_msgs::action::GeneratePolynomialTrajectory::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::GeneratePolynomialTrajectory::Goal> &goal, std::shared_ptr<as2_msgs::action::GeneratePolynomialTrajectory::Feedback> &feedback_msg, std::shared_ptr<as2_msgs::action::GeneratePolynomialTrajectory::Result> &result_msg) override

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

void timerUpdateFrameCallback(): Callback to check the errors between frames and update the frame offset.

void stateCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg): Topic Callbacks

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

void modifyWaypointCallback(const as2_msgs::msg::PoseStampedWithIDArray::SharedPtr _msg): For faster waypoint modified

void setup(): Trajectory generator functions

bool goalToDynamicWaypoint(std::shared_ptr<const as2_msgs::action::GeneratePolynomialTrajectory::Goal> goal, dynamic_traj_generator::DynamicWaypoint::Deque &waypoints)

bool evaluateTrajectory(double eval_time)

bool evaluateSetpoint(double eval_time, as2_msgs::msg::TrajectoryPoint &trajectory_command, bool current_setpoint = true)

bool updateFrame(const as2_msgs::action::GeneratePolynomialTrajectory::Goal &goal)

update the trajectory waypoint and waypoint_to_set_queue with the frame offset

Parameters:: goal – the goal of the action
Returns:: bool Return false if the transform between the map and the desired frame is not available and true otherwise

double computeYawAnglePathFacing(double vx, double vy)

double computeYawFaceReference()

Compute the Yaw angle to face the next reference point.

Returns:: double Current yaw angle if the distance to the next reference point is less than the yaw_threshold_ or the angle to face the next reference point otherwise

bool computeErrorFrames()

Compute the error frames between the map and the desired frame.

Returns:: bool Return true if the frame offset is bigger than the transform_threshold_ and false otherwise

void plotTrajectory(): Debug functions

void plotTrajectoryThread()

void plotRefTrajPoint()

Private Members

rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr state_sub_: Subscriptions

rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr yaw_sub_

rclcpp::Subscription<as2_msgs::msg::PoseStampedWithIDArray>::SharedPtr mod_waypoint_sub_

rclcpp::TimerBase::SharedPtr timer_update_frame_: Timer

double frequency_update_frame_ = 0.0

std::shared_ptr<dynamic_traj_generator::DynamicTrajectory> trajectory_generator_: Dynamic trajectory generator library

as2::motionReferenceHandlers::HoverMotion hover_motion_handler_: Handlers

as2::motionReferenceHandlers::TrajectoryMotion trajectory_motion_handler_

as2::tf::TfHandler tf_handler_

std::string base_link_frame_id_: Parameters

std::string desired_frame_id_

std::string map_frame_id_

int sampling_n_ = 1

double sampling_dt_ = 0.0

int path_lenght_ = 0

float yaw_threshold_ = 0

float transform_threshold_ = 1.0

double yaw_speed_threshold_ = 2.0

double wp_close_threshold_ = 0.0

as2_msgs::msg::YawMode yaw_mode_

as2_msgs::action::GeneratePolynomialTrajectory::Goal goal_

as2_msgs::action::GeneratePolynomialTrajectory::Feedback feedback_

as2_msgs::action::GeneratePolynomialTrajectory::Result result_

bool has_yaw_from_topic_ = false

float yaw_from_topic_ = 0.0f

float init_yaw_angle_ = 0.0f

Eigen::Vector3d current_position_

geometry_msgs::msg::TransformStamped current_map_to_odom_transform_

geometry_msgs::msg::TransformStamped last_map_to_odom_transform_

double current_yaw_

as2_msgs::msg::TrajectorySetpoints trajectory_command_

rclcpp::Duration eval_time_ = rclcpp::Duration(0, 0)

rclcpp::Duration eval_time_yaw_ = rclcpp::Duration(0, 0)

rclcpp::Time time_zero_

rclcpp::Time time_zero_yaw_

bool first_run_ = false

bool has_odom_ = false

dynamic_traj_generator::DynamicWaypoint::Deque waypoints_to_set_

std::optional<rclcpp::Time> time_debug_

bool enable_debug_ = false

std::thread plot_thread_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr debug_path_pub_: For debuging Debug publishers

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_

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 follow_path_plugin_params

Public Members

double follow_path_speed = 0.0

double follow_path_threshold = 0.0

class FollowPathAction : public nav2_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 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 Functions

inline void sendHover()

inline float getActualYaw()

bool getState()

bool computeYaw(const uint8_t yaw_mode, const geometry_msgs::msg::Point &target, const geometry_msgs::msg::Point &actual, float &yaw)

bool checkGoal(as2_msgs::action::FollowReference::Goal &_goal)

Private Members

geometry_msgs::msg::PoseStamped actual_pose_

geometry_msgs::msg::TwistStamped actual_twist

std::string base_link_frame_id_

rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_

rclcpp::Subscription<as2_msgs::msg::PlatformInfo>::SharedPtr platform_info_sub_

std::shared_ptr<as2::tf::TfHandler> tf_handler_

as2_msgs::action::FollowReference::Goal goal_

as2_msgs::action::FollowReference::Feedback feedback_

as2_msgs::action::FollowReference::Result result_

std::shared_ptr<as2::motionReferenceHandlers::PositionMotion> position_motion_handler_ = nullptr

std::shared_ptr<as2::motionReferenceHandlers::HoverMotion> hover_motion_handler_ = nullptr

int platform_state_

bool localization_flag_ = false

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 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_

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 nav2_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 nav2_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 nav2_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 nav2_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 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 &params = IndiControllerParams())

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 &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 &params = IndiControllerParams())

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 &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

using PIDParams = pid_controller::PIDParams

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

using PIDParams = pid_controller::PIDParams

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

using Matrix3 = Eigen::Matrix<P, 3, 3>

using MatrixN = Eigen::Matrix<P, num_rotors, 4>

using PIDParams = pid_controller::PIDParams

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 nav2_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_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 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.

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 nav2_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 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

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 use_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 as2_motion_controller_plugin_base::ControllerBase 

Public Functions

inline Plugin()

inline ~Plugin()

virtual void ownInitialize() override: Virtual functions from ControllerBase

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

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

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

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

virtual bool updateParams(const std::vector<rclcpp::Parameter> &_params_list) override

virtual void reset() override

inline virtual std::string getDesiredPoseFrameId() override

inline virtual std::string getDesiredTwistFrameId() override

Private Functions

bool checkParamList(const std::string &param, std::vector<std::string> &_params_list): Controller especific functions

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

inline void resetState()

void resetReferences()

void resetCommands()

void computeActions(geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::TwistStamped &twist, as2_msgs::msg::Thrust &thrust)

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

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)

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)

Private Members

UAV_state uav_state_

UAV_reference control_ref_

Acro_command control_command_

Control_flags flags_

bool hover_flag_ = false

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

std::string odom_frame_id_ = "odom"

std::string base_link_frame_id_ = "base_link"

const Eigen::Vector3d gravitational_accel_ = Eigen::Vector3d(0, 0, -9.81)

const std::vector<std::string> parameters_list_ = {"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",}

std::vector<std::string> parameters_to_read_ = {parameters_list_}

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 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 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()

Private Members

std::shared_ptr<as2::motionReferenceHandlers::SpeedMotion> speed_motion_handler_ = nullptr

rclcpp::Time time_

double land_speed_condition_percentage_

double land_speed_condition_height_

float speed_condition_

int time_condition_ = 1

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_speed_condition_height_

float speed_condition_

int time_condition_ = 1

float initial_height_

float land_height_ = -10.0f

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 

Public Functions

inline Plugin()

inline ~Plugin()

virtual void ownInitialize() override

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

virtual void updateReference(const geometry_msgs::msg::PoseStamped &ref) override

virtual void updateReference(const geometry_msgs::msg::TwistStamped &ref) override

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

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

virtual std::string getDesiredPoseFrameId()

virtual std::string getDesiredTwistFrameId()

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

virtual bool updateParams(const std::vector<rclcpp::Parameter> &_params_list) override

virtual void reset() override

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)

void updateControllerParameter(PID_1D &_pid_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

void updateController3DParameter(PID &_pid_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

void updateSpeedInAPlaneParameter(PID_1D &_pid_1d_handler, PID &_pid_3d_handler, const std::string &_parameter_name, const rclcpp::Parameter &_param)

void resetState()

void resetReferences()

void resetCommands()

bool getOutput(geometry_msgs::msg::TwistStamped &twist_msg)

Private Members

as2_msgs::msg::ControlMode control_mode_in_

as2_msgs::msg::ControlMode control_mode_out_

Control_flags flags_

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_

std::shared_ptr<as2::tf::TfHandler> tf_handler_

std::vector<std::string> plugin_parameters_list_ = {"proportional_limitation", "use_bypass"}

const std::vector<std::string> position_control_parameters_list_ = {"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_list_ = {"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_list_ = {"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_list_ = {"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_list_ = {"yaw_control.reset_integral", "yaw_control.antiwindup_cte", "yaw_control.alpha", "yaw_control.kp", "yaw_control.ki", "yaw_control.kd"}

std::vector<std::string> plugin_parameters_to_read_ = {plugin_parameters_list_}

std::vector<std::string> position_control_parameters_to_read_ = {position_control_parameters_list_}

std::vector<std::string> velocity_control_parameters_to_read_ = {velocity_control_parameters_list_}

std::vector<std::string> speed_in_a_plane_control_parameters_to_read_{speed_in_a_plane_control_parameters_list_}

std::vector<std::string> trajectory_control_parameters_to_read_{trajectory_control_parameters_list_}

std::vector<std::string> yaw_control_parameters_to_read_ = {yaw_control_parameters_list_}

UAV_state uav_state_

UAV_state control_ref_

UAV_command control_command_

bool hover_flag_ = false

Eigen::Vector3d speed_limits_

double yaw_speed_limit_

bool use_bypass_ = true

bool proportional_limitation_ = false

std::string enu_frame_id_ = "odom"

std::string flu_frame_id_ = "base_link"

std::string input_pose_frame_id_ = enu_frame_id_

std::string input_twist_frame_id_ = enu_frame_id_

std::string output_twist_frame_id_ = enu_frame_id_

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 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 check_finished()

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_

as2_msgs::msg::GimbalControl gimbal_control_msg_

rclcpp::Publisher<as2_msgs::msg::GimbalControl>::SharedPtr gimbal_control_pub_

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

virtual RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE (RateBase) 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)

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<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 nav2_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 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 ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, public ignition::gazebo::ISystemPreUpdate

Public Functions

SuctionGripperPlugin()

~SuctionGripperPlugin()

void Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &_eventMgr) override

void PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::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

Entity childItem = {kNullEntity}: The item being moved.

std::string linkName: The gripper link name.

Entity joint = {kNullEntity}: Used to store the joint when we attach to an object.

Entity gripperEntity = {kNullEntity}: The gripper link entity.

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<Entity, 3>, 3> contacts: Two-dimensional array of contact points.

transport::Node::Publisher contactPublisherCenter: Publisher for contact points.

transport::Node::Publisher contactPublisherLeft

transport::Node::Publisher contactPublisherRight

transport::Node::Publisher contactPublisherTop

transport::Node::Publisher contactPublisherBottom

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 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 nav2_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>

TfHandler class to handle the tf2_ros::Buffer and tf2_ros::TransformListener with ease inside a as2::Node class.

Public Functions

explicit TfHandler(as2::Node *_node)

Construct a new Tf Handler object.

Parameters:: _node – an as2::Node object

void setTfTimeoutThreshold(double tf_timeout_threshold)

Set the tf timeout threshold.

Parameters:: tf_timeout_threshold – double in seconds

void setTfTimeoutThreshold(const std::chrono::nanoseconds &tf_timeout_threshold)

Set the tf timeout threshold.

Parameters:: tf_timeout_threshold – std::chrono::nanoseconds

double getTfTimeoutThreshold() const

Get the tf timeout threshold.

Returns:: double

std::shared_ptr<tf2_ros::Buffer> getTfBuffer() const

Get the tf buffer object.

Returns:: std::shared_ptr<tf2_ros::Buffer>

template<typename T> inline T convert(const T &input, const std::string &target_frame, const std::chrono::nanoseconds timeout)

convert a from one frame to another

Parameters:

input – variable to convert
target_frame – the target frame
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

variable in the target frame

template<typename T> inline T convert(const T &input, const std::string &target_frame)

convert a from one frame to another

Parameters:

input – variable to convert
target_frame – the target frame

Throws:

tf2::TransformException – if the transform is not available

Returns:

variable in the target frame

inline geometry_msgs::msg::TwistStamped convert(const geometry_msgs::msg::TwistStamped &_twist, const std::string &target_frame, const std::chrono::nanoseconds timeout)

inline geometry_msgs::msg::TwistStamped convert(const geometry_msgs::msg::TwistStamped &twist, const std::string &target_frame)

inline nav_msgs::msg::Path convert(const nav_msgs::msg::Path &_path, const std::string &target_frame, const std::chrono::nanoseconds timeout)

inline nav_msgs::msg::Path convert(const nav_msgs::msg::Path &path, const std::string &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 – the target frame
source_frame – the source frame
time – the time of the transform in TimePoint
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::PoseStamped

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 – the target frame
source_frame – the source frame
time – the time of the transform in Ros Time
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::PoseStamped

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time = tf2::TimePointZero)

obtain a TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in TimePoint

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::TransformStamped

geometry_msgs::msg::PoseStamped getPoseStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time)

obtain a TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in Ros Time

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::TransformStamped

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 TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in TimePoint
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::QuaternionStamped

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 TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in Ros Time
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::QuaternionStamped

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time = tf2::TimePointZero)

obtain a TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in TimePoint

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::QuaternionStamped

geometry_msgs::msg::QuaternionStamped getQuaternionStamped(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time)

obtain a TransformStamped from the TF_buffer

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform in Ros Time

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::QuaternionStamped

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

Parameters:

target_frame – the target frame
source_frame – the source frame
time – the time of the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

geometry_msgs::msg::QuaternionStamped

template<typename T> inline bool tryConvert(T &input, const std::string &target_frame, const std::chrono::nanoseconds timeout)

convert input to desired frame, checking if frames are valid

Parameters:

input – a variable to get converted
target_frame – the target frame
timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

bool true if the conversion was successful

template<typename T> inline bool tryConvert(T &input, const std::string &target_frame)

convert to desired frame, checking if frames are valid

Parameters:

input – a variable to get converted
target_frame – the target frame

Throws:

tf2::TransformException – if the transform is not available

Returns:

bool true if the conversion was successful

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)

convert a geometry_msgs::msg::QuaternionStamped to desired frame, checking if frames are valid

Parameters:

_quaternion – a geometry_msgs::msg::QuaternionStamped to get converted
_target_frame – the target frame
_timeout – the timeout for the transform

Throws:

tf2::TransformException – if the transform is not available

Returns:

bool true if the conversion was successful

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)

convert a geometry_msgs::msg::QuaternionStamped to desired frame, checking if frames are valid

Parameters:

_quaternion – a geometry_msgs::msg::QuaternionStamped to get converted
_target_frame – the target frame

Throws:

tf2::TransformException – if the transform is not available

Returns:

bool true if the conversion was successful

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

Public Members

Eigen::Vector3d velocity = Eigen::Vector3d::Zero()

double yaw_speed = 0.0

struct UAV_reference

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

Public Members

Eigen::Vector3d position = Eigen::Vector3d::Zero()

Eigen::Vector3d velocity = Eigen::Vector3d::Zero()

tf2::Quaternion attitude_state = tf2::Quaternion::getIdentity()

struct UAV_state

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:

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:

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:

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:

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:

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:

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:

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:

Eigen::Vector3d Rotated vector.

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)

Add namespace to the name of the Transform frame id.

Parameters:

_namespace –
_frame_name –

Returns:

std::string namespace/frame_id

std::string generateTfName(rclcpp::Node *node, std::string _frame_name)

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)

Generate a Transform message from translation and rotation in Euler angles.

Parameters:

_frame_id –
_child_frame_id –
_translation_x –
_translation_y –
_translation_z –
_roll –
_pitch –
_yaw –

Returns:

geometry_msgs::msg::TransformStamped

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

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_plugin_base

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 differential_flatness_controller

namespace follow_path_base

namespace follow_path_plugin_position

namespace follow_path_plugin_trajectory

namespace gazebo_platform

namespace GeographicLib

namespace go_to_base

namespace go_to_plugin_position

namespace go_to_plugin_trajectory

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 ignition

namespace gazebo

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 land_base

namespace land_plugin_platform

namespace land_plugin_speed

namespace land_plugin_trajectory

namespace mbzirc

namespace mocap_pose

namespace nav2_behavior_tree

namespace pid_controller

namespace pid_speed_controller

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 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 <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 arm_service.hpp

#include <memory>

#include <string>

#include “behaviortree_cpp_v3/action_node.h”

#include “nav2_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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#include “nav2_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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#include “as2_behavior_tree/port_specialization.hpp”

#include “as2_msgs/srv/geopath_to_path.hpp”

#include “geometry_msgs/msg/pose.hpp”

#include “nav2_behavior_tree/bt_service_node.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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#include “nav2_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 “behaviortree_cpp_v3/action_node.h”

#include “nav2_behavior_tree/bt_service_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 “behaviortree_cpp_v3/action_node.h”

#include “nav2_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 “behaviortree_cpp_v3/action_node.h”

#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 “behaviortree_cpp_v3/action_node.h”

#include “nav2_behavior_tree/bt_conversions.hpp”

#include “nav2_util/node_utils.hpp”

#include “rclcpp_action/rclcpp_action.hpp”

file bt_service_node.hpp: #include <memory>

#include <string>

#include <set>

#include “behaviortree_cpp_v3/action_node.h”

#include “nav2_behavior_tree/bt_conversions.hpp”

#include “nav2_util/node_utils.hpp”

#include “rclcpp/rclcpp.hpp”

file is_flying_condition.hpp

#include <string>

#include “behaviortree_cpp_v3/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_v3/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_v3/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_v3/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_v3/loggers/bt_zmq_publisher.h>

#include <behaviortree_cpp_v3/bt_factory.h>

#include <behaviortree_cpp_v3/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.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_msgs/action/follow_path.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

follow_path_base header file

Authors: Rafael Perez-Segui

file follow_path_behavior.hpp

#include <string>

#include <memory>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <pluginlib/class_loader.hpp>

#include <rclcpp_action/rclcpp_action.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/action/follow_path.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “follow_path_base.hpp”

follow_path_behavior header file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file follow_path_plugin_position.cpp

#include “as2_motion_reference_handlers/position_motion.hpp”

#include “as2_msgs/msg/yaw_mode.hpp”

#include “follow_path_behavior/follow_path_base.hpp”

#include “std_msgs/msg/header.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the follow path behavior position plugin

Authors: Rafael Perez-Segui

file follow_path_plugin_trajectory.cpp

#include <geometry_msgs/msg/point.hpp>

#include <std_msgs/msg/header.hpp>

#include <std_srvs/srv/trigger.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_msgs/action/generate_polynomial_trajectory.hpp”

#include “as2_msgs/msg/pose_with_id.hpp”

#include “as2_msgs/msg/yaw_mode.hpp”

#include “follow_path_behavior/follow_path_base.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_action/rclcpp_action.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the go to behavior trajectory plugin

Authors: Rafael Perez-Segui

file follow_path_behavior.cpp

#include “follow_path_behavior/follow_path_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

follow_path_behavior file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file follow_path_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “follow_path_behavior/follow_path_behavior.hpp”

follow_path_behavior node file

Authors: Rafael Perez-Segui Pedro Arias Pérez

Functions

int main(int argc, char *argv[])

file follow_reference_behavior.hpp

#include <string>

#include <memory>

#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.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_motion_reference_handlers/position_motion.hpp”

#include “as2_msgs/action/follow_reference.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

follow_reference_behavior header file

Authors: Rafael Perez-Segui Pedro Arias Pérez Javier Melero Deza

file follow_reference_behavior.cpp: #include “follow_reference_behavior/follow_reference_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

file follow_reference_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “follow_reference_behavior/follow_reference_behavior.hpp”

Functions

int main(int argc, char *argv[])

file go_to_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.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_msgs/action/go_to_waypoint.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

Base class for go to plugins header

Authors: Rafael Perez-Segui

file go_to_behavior.hpp

#include <memory>

#include <string>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <pluginlib/class_loader.hpp>

#include <rclcpp_action/rclcpp_action.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/action/go_to_waypoint.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “go_to_base.hpp”

Go to behavior class header file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file go_to_plugin_position.cpp

#include “as2_motion_reference_handlers/position_motion.hpp”

#include “go_to_behavior/go_to_base.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the go to behavior position plugin

Authors: Rafael Perez-Segui

file go_to_plugin_trajectory.cpp

#include <geometry_msgs/msg/point.hpp>

#include <std_msgs/msg/header.hpp>

#include <std_srvs/srv/trigger.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_msgs/action/generate_polynomial_trajectory.hpp”

#include “as2_msgs/msg/pose_with_id.hpp”

#include “as2_msgs/msg/yaw_mode.hpp”

#include “go_to_behavior/go_to_base.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_action/rclcpp_action.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the go to behavior trajectory plugin

Authors: Rafael Perez-Segui

file go_to_behavior.cpp

#include “go_to_behavior/go_to_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

Go to behavior file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file go_to_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “go_to_behavior/go_to_behavior.hpp”

Functions

int main(int argc, char *argv[])

file land_base.hpp

#include <Eigen/Dense>

#include <string>

#include <memory>

#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.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_msgs/action/land.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

Base class for land plugins header

Authors: Rafael Perez-Segui

file land_behavior.hpp

#include <string>

#include <memory>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <pluginlib/class_loader.hpp>

#include <std_srvs/srv/set_bool.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/action/land.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/srv/set_platform_state_machine_event.hpp”

#include “land_base.hpp”

#include “rclcpp/rclcpp.hpp”

Land behavior class header file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file land_plugin_platform.cpp

#include <chrono>

#include <std_srvs/srv/set_bool.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/services.hpp”

#include “land_behavior/land_base.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the land behavior platform plugin

Authors: Rafael Perez-Segui

file land_plugin_speed.cpp

#include “as2_behavior/behavior_server.hpp”

#include “as2_motion_reference_handlers/speed_motion.hpp”

#include “land_behavior/land_base.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the land behavior speed plugin

Authors: Rafael Perez-Segui

file land_plugin_trajectory.cpp

#include <geometry_msgs/msg/point.hpp>

#include <std_msgs/msg/header.hpp>

#include <std_srvs/srv/trigger.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_msgs/action/generate_polynomial_trajectory.hpp”

#include “as2_msgs/msg/pose_with_id.hpp”

#include “as2_msgs/msg/yaw_mode.hpp”

#include “land_behavior/land_base.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_action/rclcpp_action.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the land behavior trajectory plugin

Authors: Rafael Perez-Segui

file land_behavior.cpp

#include “land_behavior/land_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

land_behavior file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file land_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “land_behavior/land_behavior.hpp”

land_behavior node file

Authors: Rafael Perez-Segui Pedro Arias Pérez

Functions

int main(int argc, char *argv[])

file takeoff_base.hpp

#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.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_msgs/action/takeoff.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

Base class for takeoff plugins

Authors: Rafael Perez-Segui

file takeoff_behavior.hpp

#include <memory>

#include <string>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <pluginlib/class_loader.hpp>

#include <rclcpp_action/rclcpp_action.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/action/takeoff.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/srv/set_platform_state_machine_event.hpp”

#include “takeoff_base.hpp”

Takeoff behavior class header file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file takeoff_plugin_platform.cpp

#include <chrono>

#include <std_srvs/srv/set_bool.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/services.hpp”

#include “takeoff_behavior/takeoff_base.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the take off behavior platform plugin

Authors: Rafael Perez-Segui

file takeoff_plugin_position.cpp: #include <geometry_msgs/msg/point.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_motion_reference_handlers/position_motion.hpp”

#include “takeoff_behavior/takeoff_base.hpp”

#include <pluginlib/class_list_macros.hpp>

file takeoff_plugin_speed.cpp

#include “as2_behavior/behavior_server.hpp”

#include “as2_motion_reference_handlers/speed_motion.hpp”

#include “takeoff_behavior/takeoff_base.hpp”

#include <pluginlib/class_list_macros.hpp>

This file contains the implementation of the take off behavior speed plugin

Authors: Rafael Perez-Segui

file takeoff_plugin_trajectory.cpp: #include <geometry_msgs/msg/point.hpp>

#include <std_msgs/msg/header.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_msgs/action/generate_polynomial_trajectory.hpp”

#include “as2_msgs/msg/pose_with_id.hpp”

#include “as2_msgs/msg/yaw_mode.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_action/rclcpp_action.hpp”

#include “takeoff_behavior/takeoff_base.hpp”

#include <pluginlib/class_list_macros.hpp>

file takeoff_behavior.cpp

#include “takeoff_behavior/takeoff_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

Takeoff behavior class source file

Authors: Rafael Perez-Segui Pedro Arias Pérez

file takeoff_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “takeoff_behavior/takeoff_behavior.hpp”

Takeoffbehavior container main file

Authors: Rafael Perez-Segui Pedro Arias Pérez

Functions

int main(int argc, char *argv[])

file cell_node.hpp

#include <memory>

cell_node header file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

Typedefs

using CellNodePtr = std::shared_ptr<CellNode>

file graph_searcher.hpp

#include <math.h>

#include <iostream>

#include <memory>

#include <limits>

#include <unordered_map>

#include <vector>

#include “cell_node.hpp”

graph_searcher header file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

file utils.hpp

#include <tf2/convert.h>

#include <tf2_ros/buffer.h>

#include <tf2_ros/transform_listener.h>

#include <memory>

#include <string>

#include <geometry_msgs/msg/point_stamped.hpp>

#include <nav_msgs/msg/map_meta_data.hpp>

#include <nav_msgs/msg/occupancy_grid.hpp>

#include <rclcpp/rclcpp.hpp>

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include “cell_node.hpp”

utils implementation file.

Authors: Pedro Arias Pérez

file path_planner_behavior.hpp

#include <filesystem>

#include <memory>

#include <string>

#include <vector>

#include <rclcpp/rclcpp.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_msgs/action/navigate_to_point.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include <pluginlib/class_loader.hpp>

#include “visualization_msgs/msg/marker.hpp”

#include “tf2_ros/buffer.h”

#include “tf2_ros/transform_listener.h”

#include “std_srvs/srv/trigger.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_msgs/action/follow_path.hpp”

#include “as2_behaviors_path_planning/path_planner_plugin_base.hpp”

path_planner_behavior header file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

file path_planner_plugin_base.hpp

#include <memory>

#include <vector>

#include <as2_core/node.hpp>

#include <as2_behavior/behavior_utils.hpp>

#include “as2_msgs/action/navigate_to_point.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “geometry_msgs/msg/point.hpp”

#include “tf2_ros/buffer.h”

path_planner_plugin_base header file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

file a_star.hpp

#include <iostream>

#include <memory>

#include <string>

#include <vector>

#include <as2_behaviors_path_planning/path_planner_plugin_base.hpp>

#include “a_star_searcher.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “nav_msgs/msg/occupancy_grid.hpp”

#include “visualization_msgs/msg/marker.hpp”

#include “std_msgs/msg/header.hpp”

#include “nav_msgs/msg/map_meta_data.hpp”

#include “builtin_interfaces/msg/duration.hpp”

a_star header file.

Authors: Pedro Arias Pérez

file a_star_algorithm.hpp

#include <math.h>

#include <array>

#include <cmath>

#include <iostream>

#include <memory>

#include <limits>

#include <unordered_map>

#include <utility>

#include <vector>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/opencv.hpp>

A* algorithm for path planning.

Authors: Miguel Fernandez-Cortizas, Pedro Arias Pérez

Typedefs

using NodePtr = std::shared_ptr<Node>

Functions

static int convertPointToInt(const cv::Point2i &point)

file a_star_searcher.hpp

#include <opencv2/core/types.hpp>

#include <opencv2/opencv.hpp>

#include “nav_msgs/msg/occupancy_grid.hpp”

#include “graph_searcher.hpp”

a_star_searcher header file.

Authors: Pedro Arias Pérez

file a_star.cpp

#include <a_star.hpp>

#include <utils.hpp>

#include <pluginlib/class_list_macros.hpp>

a_star implementation file.

Authors: Pedro Arias Pérez

file a_star_searcher.cpp

#include “a_star_searcher.hpp”

a_star_searcher implementation file.

Authors: Pedro Arias Pérez

file voronoi.hpp

#include <memory>

#include <string>

#include <vector>

#include <as2_behaviors_path_planning/path_planner_plugin_base.hpp>

#include “dynamicvoronoi/dynamicvoronoi.h”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “nav_msgs/msg/occupancy_grid.hpp”

#include “visualization_msgs/msg/marker.hpp”

#include “std_msgs/msg/header.hpp”

#include “nav_msgs/msg/map_meta_data.hpp”

#include “cell_node.hpp”

#include “voronoi_searcher.hpp”

voronoi header file.

Authors: Pedro Arias Pérez

file voronoi_searcher.hpp

#include “dynamicvoronoi/dynamicvoronoi.h”

#include “graph_searcher.hpp”

voronoi_searcher header file.

Authors: Pedro Arias Pérez

file voronoi.cpp

#include <voronoi.hpp>

#include <utils.hpp>

#include <pluginlib/class_list_macros.hpp>

voronoi implementation file.

Authors: Pedro Arias Pérez

Defines

FREE_SPACE

OCC_SPACE

UNKNOWN_SPACE

MAX_DIST

file voronoi_searcher.cpp

#include <algorithm>

#include “voronoi_searcher.hpp”

voronoi_searcher implementation file.

Authors: Pedro Arias Pérez

file bucketedqueue.h: #include <vector>

#include <set>

#include <queue>

#include <assert.h>

#include “point.h”

#include <map>

#include “bucketedqueue.hxx”

file bucketedqueue.hxx: #include “bucketedqueue.h”

#include “limits.h”

#include <stdio.h>

#include <stdlib.h>

file dynamicvoronoi.cpp: #include “dynamicvoronoi.h”

#include <math.h>

#include <iostream>

file dynamicvoronoi.h: #include <stdlib.h>

#include <stdio.h>

#include <limits.h>

#include <queue>

#include “bucketedqueue.h”

file point.h

Defines

INTPOINT

file path_planner_behavior.cpp

#include “as2_behaviors_path_planning/path_planner_behavior.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/topics.hpp”

#include “rclcpp_components/register_node_macro.hpp”

path_planner_behavior implementation file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

file path_planner_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “path_planner_behavior.hpp”

path_planner_behavior node file.

Authors: Pedro Arias Pérez Miguel Fernandez-Cortizas

Functions

int main(int argc, char *argv[])

file detect_aruco_markers_behavior.hpp

#include <cv_bridge/cv_bridge.h>

#include <tf2/LinearMath/Quaternion.h>

#include <Eigen/Dense>

#include <memory>

#include <string>

#include <vector>

#include <rclcpp/rclcpp.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/sensor.hpp”

#include “as2_msgs/action/detect_aruco_markers.hpp”

#include “as2_msgs/msg/pose_stamped_with_id.hpp”

#include “as2_msgs/msg/pose_stamped_with_id_array.hpp”

#include “sensor_msgs/image_encodings.hpp”

#include “sensor_msgs/msg/image.hpp”

#include <image_transport/image_transport.hpp>

#include <opencv2/aruco.hpp>

#include <opencv2/calib3d.hpp>

Aruco detector header file.

Authors: David Perez Saura

Functions

std::string targetIds2string(const std::vector<uint16_t> &target_ids)

file detect_aruco_markers_behavior.cpp

#include “detect_aruco_markers_behavior.hpp”

Aruco detector implementation file.

Authors: David Perez Saura

Functions

std::string targetIds2string(const std::vector<uint16_t> &target_ids)

file detect_aruco_markers_behavior_node.cpp

#include <rclcpp/rclcpp.hpp>

#include “detect_aruco_markers_behavior.hpp”

Aruco detector node file.

Authors: David Perez Saura

Functions

int main(int argc, char *argv[])

file point_gimbal_behavior.hpp

#include <string>

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include <rclcpp_action/rclcpp_action.hpp>

#include “as2_msgs/action/point_gimbal.hpp”

#include “as2_msgs/msg/gimbal_control.hpp”

#include “geometry_msgs/msg/quaternion_stamped.hpp”

#include “geometry_msgs/msg/vector3.hpp”

#include “geometry_msgs/msg/vector3_stamped.hpp”

Point Gimbal behavior header file.

Authors: Pedro Arias-Perez, Rafael Perez-Segui

file point_gimbal_behavior.cpp

#include “point_gimbal_behavior.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/utils/frame_utils.hpp”

Point Gimbal behavior implementation file.

Authors: Pedro Arias-Perez, Rafael Perez-Segui

file point_gimbal_behavior_node.cpp

#include <rclcpp/rclcpp.hpp>

#include “point_gimbal_behavior.hpp”

Point Gimbal node file.

Authors: Pedro Arias-Perez

Functions

int main(int argc, char *argv[])

file set_arming_state_behavior.hpp

#include <chrono>

#include <string>

#include <memory>

#include “as2_behavior/behavior_server.hpp”

#include “as2_msgs/action/set_arming_state.hpp”

#include “std_srvs/srv/set_bool.hpp”

Class definition for the set arming state behavior.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file set_offboard_mode_behavior.hpp

#include <chrono>

#include <string>

#include <memory>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/services.hpp”

#include “as2_msgs/action/set_offboard_mode.hpp”

#include “std_srvs/srv/set_bool.hpp”

Class definition for the set offboard mode behavior.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file set_arming_state_behavior_main.cpp

#include <rclcpp/rclcpp.hpp>

#include “as2_behaviors_platform/set_arming_state_behavior.hpp”

Source file for the set arming state behavior main.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Functions

int main(int argc, char **argv)

file set_offboard_mode_behavior_main.cpp

#include <rclcpp/rclcpp.hpp>

#include “as2_behaviors_platform/set_offboard_mode_behavior.hpp”

Source file for the set offboard mode behavior main.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Functions

int main(int argc, char **argv)

file generate_polynomial_trajectory_behavior.hpp

#include <tf2/LinearMath/Quaternion.h>

#include <Eigen/Dense>

#include <string>

#include <memory>

#include <rclcpp/clock.hpp>

#include <rclcpp/rclcpp.hpp>

#include <std_srvs/srv/set_bool.hpp>

#include “as2_behavior/behavior_server.hpp”

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_motion_reference_handlers/trajectory_motion.hpp”

#include “as2_msgs/action/generate_polynomial_trajectory.hpp”

#include “as2_msgs/srv/set_speed.hpp”

#include “dynamic_trajectory_generator/dynamic_trajectory.hpp”

#include “dynamic_trajectory_generator/dynamic_waypoint.hpp”

#include “as2_msgs/msg/pose_stamped_with_id_array.hpp”

#include “as2_msgs/msg/pose_with_id.hpp”

#include “as2_msgs/msg/traj_gen_info.hpp”

#include <geometry_msgs/msg/point.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include <nav_msgs/msg/path.hpp>

#include <std_msgs/msg/float32.hpp>

#include <visualization_msgs/msg/marker.hpp>

#include <visualization_msgs/msg/marker_array.hpp>

Class definition for the GeneratePolynomialTrajectoryBehavior class.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Functions

void generateDynamicPoint(const as2_msgs::msg::PoseStampedWithID &msg, dynamic_traj_generator::DynamicWaypoint &dynamic_point): Auxiliar Functions

file generate_polynomial_trajectory_behavior.cpp

#include “generate_polynomial_trajectory_behavior.hpp”

#include “rclcpp_components/register_node_macro.hpp”

Source file for the GeneratePolynomialTrajectoryBehavior class.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Functions

void generateDynamicPoint(const as2_msgs::msg::PoseStampedWithID &msg, dynamic_traj_generator::DynamicWaypoint &dynamic_point): Auxiliar Functions

file generate_polynomial_trajectory_behavior_node.cpp

#include “as2_core/core_functions.hpp”

#include “generate_polynomial_trajectory_behavior.hpp”

Source file for the GeneratePolynomialTrajectoryBehavior node.

Author: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Functions

int main(int argc, char *argv[])

file aerial_platform.hpp

#include <chrono>

#include <functional>

#include <memory>

#include <string>

#include <vector>

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/platform_state_machine.hpp”

#include “as2_msgs/msg/alert_event.hpp”

#include “as2_msgs/msg/control_mode.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/platform_status.hpp”

#include “as2_msgs/msg/thrust.hpp”

#include “as2_msgs/msg/trajectory_setpoints.hpp”

#include “as2_msgs/srv/list_control_modes.hpp”

#include “as2_msgs/srv/set_control_mode.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “geometry_msgs/msg/twist_stamped.hpp”

#include “nav_msgs/msg/odometry.hpp”

#include “as2_core/node.hpp”

#include “rclcpp/publisher.hpp”

#include “rclcpp/publisher_options.hpp”

#include “rclcpp/rclcpp.hpp”

#include “std_srvs/srv/set_bool.hpp”

#include “utils/control_mode_utils.hpp”

#include “utils/yaml_utils.hpp”

Aerostack2 Aerial Platformm class header file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file core_functions.hpp

#include <memory>

#include “as2_core/node.hpp”

#include “as2_core/rate.hpp”

#include “rclcpp/publisher.hpp”

#include “rclcpp/publisher_options.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_lifecycle/lifecycle_node.hpp”

Aerostack2 core functions header file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file actions.hpp

#include <rclcpp/rclcpp.hpp>

This file contains the definition of the actions that can be performed in aerostack2.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file services.hpp

#include <rclcpp/rclcpp.hpp>

This file contains the definitions of the services used in aerostack2.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file topics.hpp: #include <rclcpp/rclcpp.hpp>

file node.hpp

#include <chrono>

#include <exception>

#include <functional>

#include <memory>

#include <string>

#include <utility>

#include <rclcpp/create_timer.hpp>

#include <rclcpp/timer.hpp>

#include “as2_core/rate.hpp”

#include “rclcpp/publisher.hpp”

#include “rclcpp/publisher_options.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rclcpp_lifecycle/lifecycle_node.hpp”

Aerostack2 node header file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Defines

AS2_RCLCPP_NODE

AS2_LIFECYLCE_NODE

AS2_NODE_FATHER

AS2_NODE_FATHER_TYPE

file platform_state_machine.hpp

#include <functional>

#include <string>

#include <vector>

#include <memory>

#include “as2_core/node.hpp”

#include “as2_msgs/msg/platform_state_machine_event.hpp”

#include “as2_msgs/msg/platform_status.hpp”

#include “as2_msgs/srv/set_platform_state_machine_event.hpp”

Aerostack2 Platform State Machine Header file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file rate.hpp

#include <chrono>

#include <memory>

#include <thread>

#include “rclcpp/macros.hpp”

#include “rclcpp/utilities.hpp”

#include “rclcpp/visibility_control.hpp”

ROS2 rate class modified to work with the as2_core namespace and add more functionalities.

Authors: Miguel Fernández Cortizas

file sensor.hpp

#include <cv_bridge/cv_bridge.h>

#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/transform_broadcaster.h>

#include <chrono>

#include <functional>

#include <memory>

#include <string>

#include <vector>

#include <image_transport/camera_publisher.hpp>

#include <opencv2/opencv.hpp>

#include <image_transport/image_transport.hpp>

#include <rclcpp/publisher.hpp>

#include <rclcpp/publisher_options.hpp>

#include <rclcpp/rclcpp.hpp>

#include <sensor_msgs/msg/battery_state.hpp>

#include <sensor_msgs/msg/camera_info.hpp>

#include <sensor_msgs/msg/fluid_pressure.hpp>

#include <sensor_msgs/msg/image.hpp>

#include <sensor_msgs/msg/imu.hpp>

#include <sensor_msgs/msg/laser_scan.hpp>

#include <sensor_msgs/msg/magnetic_field.hpp>

#include <sensor_msgs/msg/nav_sat_fix.hpp>

#include <sensor_msgs/msg/range.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <geometry_msgs/msg/quaternion_stamped.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include “as2_core/custom/tf2_geometry_msgs.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/tf_utils.hpp”

Sensor class for AS2 header file.

Authors: Rafael Pérez Seguí David Pérez Saura Miguel Fernández Cortizas Pedro Arias Pérez

file synchronous_service_client.hpp

#include <memory>

#include <string>

#include “as2_core/node.hpp”

#include “rclcpp/rclcpp.hpp”

Class for handling synchronous service clients in ROS2 without taking care about the spin() method.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez

file control_mode_utils.hpp

#include <yaml-cpp/yaml.h>

#include <bitset>

#include <filesystem>

#include <fstream>

#include <iostream>

#include <sstream>

#include <string>

#include <vector>

#include “as2_msgs/msg/control_mode.hpp”

#include “rclcpp/logging.hpp”

Utility functions for handling control modes over the aerostack2 framework.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

Defines

MATCH_ALL

MATCH_CONTROL_MODE

MATCH_YAW_MODE

MATCH_REFERENCE_FRAME

UNSET_MODE_MASK

HOVER_MODE_MASK

file frame_utils.hpp

#include <math.h>

#include <Eigen/Geometry>

#include “as2_core/custom/tf2_geometry_msgs.hpp”

#include <geometry_msgs/msg/quaternion.hpp>

Aerostack2 frame utils header file.

Authors: Rafael Pérez Seguí Pedro Arias Pérez

file gps_utils.hpp: #include <string>

#include <GeographicLib/LocalCartesian.hpp>

#include “geographic_msgs/msg/geo_pose_stamped.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “sensor_msgs/msg/nav_sat_fix.hpp”

file tf_utils.hpp

#include <tf2/convert.h>

#include <tf2/time.h>

#include <tf2_ros/create_timer_ros.h>

#include <string>

#include <utility>

#include <memory>

#include <geometry_msgs/msg/point_stamped.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <geometry_msgs/msg/vector3_stamped.hpp>

#include <nav_msgs/msg/path.hpp>

#include “as2_core/custom/tf2_geometry_msgs.hpp”

#include “as2_core/node.hpp”

#include “tf2_ros/buffer.h”

#include “tf2_ros/transform_listener.h”

Tranform utilities library header file.

Authors: David Perez Saura Miguel Fernandez Cortizas Rafael Perez Segui

file yaml_utils.hpp: #include <yaml-cpp/yaml.h>

#include <bitset>

#include <filesystem>

#include <fstream>

#include <iostream>

#include <sstream>

#include <string>

#include <vector>

#include <algorithm>

file _as2_core_pybind11.cpp

#include <pybind11/pybind11.h>

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

Functions

PYBIND11_MODULE(as2_names, m)

file aerial_platform.cpp

#include “as2_core/aerial_platform.hpp”

Aerostack2 Aerial Platformm class implementation file.

Authors: Miguel Fernandez Cortizas

file core_functions.cpp

#include “core_functions.hpp”

Aerostack2 core functions implementation file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file node.cpp

#include “as2_core/node.hpp”

Aerostack2 node implementation file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file platform_state_machine.cpp

#include “as2_core/platform_state_machine.hpp”

Aerostack2 Platform State Machine implementation file.

Authors: Miguel Fernandez Cortizas

file rate.cpp

#include “as2_core/rate.hpp”

ROS2 rate class modified to work with the as2_core namespace and add more functionalities.

Authors: Miguel Fernández Cortizas

file sensor.cpp

#include “as2_core/sensor.hpp”

Sensor class for AS2 implementation file.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file control_mode_utils.cpp

#include “as2_core/utils/control_mode_utils.hpp”

#include <rclcpp/logger.hpp>

#include <rclcpp/logging.hpp>

Utility functions for handling control modes over the aerostack2 framework.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file frame_utils.cpp

#include “as2_core/utils/frame_utils.hpp”

Aerostack2 frame utils functions implementation file.

Authors: Rafael Pérez Seguí Pedro Arias Pérez

file gps_utils.cpp: #include “as2_core/utils/gps_utils.hpp”

file tf_utils.cpp

#include “as2_core/utils/tf_utils.hpp”

Tranform utilities library implementation file.

Authors: David Perez Saura

file yaml_utils.cpp

#include “as2_core/utils/yaml_utils.hpp”

Utils for loading YAML files in Aerostack2 implementation.

Authors: Miguel Fernández Cortizas

file as2_realsense_interface.hpp

#include <tf2_ros/static_transform_broadcaster.h>

#include <string>

#include <vector>

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/node.hpp”

#include “as2_core/sensor.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include <librealsense2/rs.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <geometry_msgs/msg/vector3.hpp>

#include “nav_msgs/msg/odometry.hpp”

#include “sensor_msgs/msg/imu.hpp”

as2_realsense_interface header file.

Author: David Perez Saura

file as2_realsense_interface.cpp

#include “as2_realsense_interface.hpp”

as2_realsense_interface source file.

Author: David Perez Saura

file as2_realsense_interface_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_realsense_interface.hpp”

as2_realsense_interface node file.

Author: David Perez Saura

Functions

int main(int argc, char *argv[])

file as2_usb_camera_interface.hpp

#include <tf2/LinearMath/Quaternion.h>

#include <cv_bridge/cv_bridge.h>

#include <vector>

#include <memory>

#include <string>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/sensor.hpp”

#include “sensor_msgs/image_encodings.hpp”

#include “sensor_msgs/msg/image.hpp”

#include <image_transport/image_transport.hpp>

#include <opencv2/calib3d.hpp>

#include <opencv2/highgui.hpp>

#include <opencv2/imgproc.hpp>

#include <opencv2/opencv.hpp>

Class definition for the USB camera interface node.

Main for the USB camera interface node.

Authors: David Perez Saura, Miguel Fernandez Cortizas

file as2_usb_camera_interface.cpp: #include “as2_usb_camera_interface.hpp”

file as2_usb_camera_interface_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_usb_camera_interface.hpp”

Functions

int main(int argc, char *argv[])

file map_server.hpp: #include <filesystem>

#include <memory>

#include <pluginlib/class_loader.hpp>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/node.hpp”

#include “plugin_base.hpp”

file plugin_base.hpp

#include <as2_core/names/topics.hpp>

#include <as2_core/node.hpp>

Plugin base class for Map Server node.

Authors: Pedro Arias Pérez

file plugin_base.hpp

#include <tf2_ros/buffer.h>

#include <tf2_ros/buffer_interface.h>

#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/transform_broadcaster.h>

#include <memory>

#include <string>

#include <rclcpp/rclcpp.hpp>

#include <rclcpp/subscription_base.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include <as2_core/node.hpp>

#include <as2_core/utils/tf_utils.hpp>

#include <as2_core/names/topics.hpp>

An state estimation plugin base for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file scan2occ_grid.hpp

#include <tf2_ros/buffer.h>

#include <tf2_ros/transform_listener.h>

#include <memory>

#include <string>

#include <vector>

#include <as2_map_server/plugin_base.hpp>

#include <geometry_msgs/msg/point_stamped.hpp>

#include <nav_msgs/msg/occupancy_grid.hpp>

#include <rclcpp/rclcpp.hpp>

#include <sensor_msgs/msg/laser_scan.hpp>

#include <opencv2/core/types.hpp>

#include <opencv2/opencv.hpp>

2d mapping plugin.

Authors: Pedro Arias Pérez

file scan2occ_grid.cpp

#include “scan2occ_grid.hpp”

#include <tf2/convert.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <pluginlib/class_list_macros.hpp>

2d mapping plugin.

Authors: Pedro Arias Pérez

file map_server.cpp: #include “as2_map_server/map_server.hpp”

file map_server_node.cpp

#include “as2_map_server/map_server.hpp”

#include “as2_core/core_functions.hpp”

Functions

int main(int argc, char *argv[])

file controller_base.hpp

#include <string>

#include <vector>

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

#include “as2_msgs/msg/control_mode.hpp”

#include “as2_msgs/msg/thrust.hpp”

#include “as2_msgs/msg/trajectory_setpoints.hpp”

Declares the as2_motion_controller_plugin_base class which is the base class for all controller plugins.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file controller_handler.hpp

#include <rcl/time.h>

#include <tf2/time.h>

#include <message_filters/subscriber.h>

#include <message_filters/sync_policies/approximate_time.h>

#include <message_filters/time_synchronizer.h>

#include <memory>

#include <algorithm>

#include <chrono>

#include <cstdint>

#include <fstream>

#include <vector>

#include <string>

#include <rclcpp/clock.hpp>

#include <rclcpp/logging.hpp>

#include <rclcpp/rate.hpp>

#include <rclcpp/service.hpp>

#include <rclcpp/timer.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/control_mode_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/msg/control_mode.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/thrust.hpp”

#include “as2_msgs/msg/trajectory_setpoints.hpp”

#include “as2_msgs/srv/list_control_modes.hpp”

#include “as2_msgs/srv/set_control_mode.hpp”

#include “controller_base.hpp”

Controller handler class definition.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

Defines

MATCH_ALL

MATCH_MODE_AND_FRAME

MATCH_MODE

MATCH_MODE_AND_YAW

UNSET_MODE_MASK

HOVER_MODE_MASK

file controller_manager.hpp

#include <memory>

#include <chrono>

#include <filesystem>

#include <pluginlib/class_loader.hpp>

#include <rclcpp/logging.hpp>

#include “as2_core/node.hpp”

#include “as2_core/utils/control_mode_utils.hpp”

#include “as2_core/utils/yaml_utils.hpp”

#include “as2_msgs/msg/controller_info.hpp”

#include “controller_handler.hpp”

Controller manager class definition.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file differential_flatness_controller.hpp

#include <Eigen/Dense>

#include <Eigen/Geometry>

#include <vector>

#include <string>

#include <rclcpp/logging.hpp>

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/utils/frame_utils.hpp”

#include “as2_core/utils/tf_utils.hpp”

#include “as2_msgs/msg/thrust.hpp”

#include “as2_msgs/msg/trajectory_setpoints.hpp”

#include “as2_motion_controller/controller_base.hpp”

Declares the controller plugin differential flatness.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file differential_flatness_controller.cpp

#include “differential_flatness_controller.hpp”

#include <pluginlib/class_list_macros.hpp>

Differential flatness controller plugin for the Aerostack framework.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file pid_speed_controller.hpp: #include <chrono>

#include <string>

#include <vector>

#include <memory>

#include <rclcpp/logging.hpp>

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.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_msgs/msg/thrust.hpp”

#include “as2_msgs/msg/trajectory_setpoints.hpp”

#include “as2_motion_controller/controller_base.hpp”

#include “pid_controller/pid.hpp”

#include “pid_controller/pid_1d.hpp”

file pid_speed_controller.cpp: #include “pid_speed_controller.hpp”

#include <pluginlib/class_list_macros.hpp>

file controller_handler.cpp

#include “as2_motion_controller/controller_handler.hpp”

#include <as2_core/utils/tf_utils.hpp>

Controller handler class implementation.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file controller_manager.cpp

#include “as2_motion_controller/controller_manager.hpp”

Controller manager class implementation.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

file controller_manager_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_motion_controller/controller_manager.hpp”

Controller manager node main file.

Authors: Miguel Fernández Cortizas Rafael Pérez Seguí

Functions

int main(int argc, char *argv[])

file acro_motion.hpp

#include <string>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <geometry_msgs/msg/vector3.hpp>

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

This file contains the definition of the ACROMotion class.

Authors: Rafael Pérez Seguí

file basic_motion_references.hpp

#include <string>

#include <as2_msgs/msg/control_mode.hpp>

#include <as2_msgs/msg/thrust.hpp>

#include <as2_msgs/msg/controller_info.hpp>

#include <as2_msgs/msg/trajectory_setpoints.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

Virtual class for basic motion references headers.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file hover_motion.hpp

#include <string>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

This file contains the definition of the HoverMotion class.

Authors: Rafael Pérez Seguí Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura

file position_motion.hpp

#include <string>

#include <as2_core/utils/tf_utils.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

This file contains the definition of the SpeedMotion class.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file speed_in_a_plane_motion.hpp

#include <string>

#include <as2_core/utils/tf_utils.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

This file contains the definition of the SpeedInAPlaneMotion class.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file speed_motion.hpp

#include <string>

#include <as2_core/utils/tf_utils.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

This file contains the definition of the SpeedMotion class.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file trajectory_motion.hpp

#include <Eigen/Dense>

#include <string>

#include <vector>

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

this file contains the implementation of the TrajectoryMotion class

this file contains the definition of the TrajectoryMotion class

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file acro_motion.cpp

#include “as2_motion_reference_handlers/acro_motion.hpp”

This file contains the implementation of the ACROMotion class.

Authors: Rafael Pérez Seguí

file basic_motion_references.cpp

#include “as2_motion_reference_handlers/basic_motion_references.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/synchronous_service_client.hpp”

#include “as2_core/utils/control_mode_utils.hpp”

#include “as2_msgs/srv/set_control_mode.hpp”

Virtual class for basic motion references implementations.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file hover_motion.cpp

#include “as2_motion_reference_handlers/hover_motion.hpp”

This file contains the implementation of the HoverMotion class.

Authors: Rafael Pérez Seguí Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura

file position_motion.cpp

#include “as2_motion_reference_handlers/position_motion.hpp”

This file contains the implementation of the PositionMotion class.

This file contains the implementation of the take off behavior position plugin

Authors: Rafael Perez-Segui

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file speed_in_a_plane_motion.cpp

#include “as2_motion_reference_handlers/speed_in_a_plane_motion.hpp”

This file contains the implementation of the SpeedInAPlaneMotion class.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file speed_motion.cpp

#include “as2_motion_reference_handlers/speed_motion.hpp”

This file contains the implementation of the SpeedMotion class.

Authors: Miguel Fernández Cortizas Pedro Arias Pérez David Pérez Saura Rafael Pérez Seguí

file trajectory_motion.cpp: #include “as2_motion_reference_handlers/trajectory_motion.hpp”

file acro_bridge.hpp

#include <iostream>

#include <memory>

#include <string>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <ros_gz_bridge/convert.hpp>

#include “rclcpp/rclcpp.hpp”

#include <std_msgs/msg/header.hpp>

#include “gz/msgs/header.pb.h”

#include “gz/msgs/float_v.pb.h”

#include “as2_msgs/msg/acro.hpp”

Gazebo bridge ACRO header file.

Authors: Francisco José Anguita Chamorro

file azimuth_bridge.hpp: #include <math.h>

#include <iostream>

#include <memory>

#include <string>

#include “rclcpp/publisher.hpp”

#include “rclcpp/rclcpp.hpp”

#include <as2_core/names/topics.hpp>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <ros_gz_bridge/convert.hpp>

#include <std_msgs/msg/float32.hpp>

file gimbal_bridge.hpp: #include <math.h>

#include <iostream>

#include <memory>

#include <string>

#include <as2_core/names/topics.hpp>

#include <as2_core/utils/frame_utils.hpp>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <rclcpp/clock.hpp>

#include <ros_gz_bridge/convert.hpp>

#include “as2_msgs/msg/gimbal_control.hpp”

#include “rclcpp/publisher.hpp”

#include “rclcpp/rclcpp.hpp”

file gps_bridge.hpp: #include <iostream>

#include <memory>

#include <string>

#include <as2_core/names/topics.hpp>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <ros_gz_bridge/convert.hpp>

#include “rclcpp/rclcpp.hpp”

#include “sensor_msgs/msg/nav_sat_fix.hpp”

file ground_truth_bridge.hpp: #include <iostream>

#include <memory>

#include <string>

#include “rclcpp/publisher.hpp”

#include “rclcpp/rclcpp.hpp”

#include <as2_core/names/topics.hpp>

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “geometry_msgs/msg/twist_stamped.hpp”

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <ros_gz_bridge/convert.hpp>

file object_tf_broadcaster.hpp: #include <geometry_msgs/msg/transform_stamped.h>

#include <tf2_msgs/msg/tf_message.h>

#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/transform_broadcaster.h>

#include <iostream>

#include <memory>

#include <string>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include <rclcpp/clock.hpp>

#include <rclcpp/rclcpp.hpp>

#include <ros_gz_bridge/convert.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>

file set_entity_pose_bridge.hpp

#include <math.h>

#include <memory>

#include <string>

#include <gz/msgs.hh>

#include <gz/transport.hh>

#include “rclcpp/publisher.hpp”

#include “rclcpp/rclcpp.hpp”

#include <ros_gz_bridge/convert.hpp>

#include <ros_gz_interfaces/srv/set_entity_pose.hpp>

Set entity pose bridge header file.

Authors: Javier Melero Deza Pedro Arias Pérez

file Common.cpp: #include “Common.hpp”

#include <string>

#include <gz/common/Console.hh>

#include <gz/math/Rand.hh>

#include <gz/math/eigen3/Conversions.hh>

#include <gz/sim/components/ChildLinkName.hh>

#include <gz/sim/components/Joint.hh>

#include <gz/sim/components/JointAxis.hh>

#include <gz/sim/components/Link.hh>

#include <gz/sim/components/Name.hh>

#include <gz/sim/components/ParentEntity.hh>

#include <gz/sim/components/Pose.hh>

#include “gz/sim/components/LinearVelocity.hh”

#include “gz/sim/components/AngularVelocity.hh”

file Common.cpp: #include “Common.hpp”

#include <string>

#include <gz/common/Console.hh>

#include <gz/math/Rand.hh>

#include <gz/math/eigen3/Conversions.hh>

#include <gz/sim/components/ChildLinkName.hh>

#include <gz/sim/components/Joint.hh>

#include <gz/sim/components/JointAxis.hh>

#include <gz/sim/components/Link.hh>

#include <gz/sim/components/Name.hh>

#include <gz/sim/components/ParentEntity.hh>

#include <gz/sim/components/Pose.hh>

#include “gz/sim/components/LinearVelocity.hh”

#include “gz/sim/components/AngularVelocity.hh”

file Common.hpp: #include <Eigen/Geometry>

#include <optional>

#include <vector>

#include <sdf/sdf.hh>

#include “gz/sim/config.hh”

#include “gz/sim/EntityComponentManager.hh”

#include “gz/sim/Model.hh”

#include “Parameters.hpp”

file Common.hpp: #include <Eigen/Geometry>

#include <optional>

#include <vector>

#include <sdf/sdf.hh>

#include “gz/sim/config.hh”

#include “gz/sim/EntityComponentManager.hh”

#include “gz/sim/Model.hh”

#include “Parameters.hpp”

file IndiController.hpp

#include “PIDController.hpp”

IndiController class definition.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Rafael Pérez Seguí

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

file IndiController.hpp

#include “PIDController.hpp”

IndiController class definition.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Rafael Pérez Seguí

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

file INDIControlPlugin.cpp

#include <gz/msgs/actuators.pb.h>

#include <gz/msgs/twist.pb.h>

#include <limits>

#include <gz/common/Profiler.hh>

#include <gz/plugin/Register.hh>

#include <gz/transport/Node.hh>

#include <gz/math/Inertial.hh>

#include <gz/math/Vector3.hh>

#include <gz/math/eigen3/Conversions.hh>

#include <sdf/sdf.hh>

#include “gz/sim/components/Actuators.hh”

#include “gz/sim/components/Gravity.hh”

#include “gz/sim/components/Inertial.hh”

#include “gz/sim/components/Link.hh”

#include “gz/sim/components/Model.hh”

#include “gz/sim/components/ParentEntity.hh”

#include “gz/sim/components/World.hh”

#include “gz/sim/Link.hh”

#include “gz/sim/Model.hh”

#include “INDIControlPlugin.hpp”

Functions

GZ_ADD_PLUGIN (MulticopterINDIControl, System, MulticopterINDIControl::ISystemConfigure, MulticopterINDIControl::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(MulticopterINDIControl

file INDIControlPlugin.cpp

#include <gz/msgs/actuators.pb.h>

#include <gz/msgs/twist.pb.h>

#include <limits>

#include <gz/common/Profiler.hh>

#include <gz/plugin/Register.hh>

#include <gz/transport/Node.hh>

#include <gz/math/Inertial.hh>

#include <gz/math/Vector3.hh>

#include <gz/math/eigen3/Conversions.hh>

#include <sdf/sdf.hh>

#include “gz/sim/components/Actuators.hh”

#include “gz/sim/components/Gravity.hh”

#include “gz/sim/components/Inertial.hh”

#include “gz/sim/components/Link.hh”

#include “gz/sim/components/Model.hh”

#include “gz/sim/components/ParentEntity.hh”

#include “gz/sim/components/World.hh”

#include “gz/sim/Link.hh”

#include “gz/sim/Model.hh”

#include “INDIControlPlugin.hpp”

Functions

IGNITION_ADD_PLUGIN (MulticopterINDIControl, System, MulticopterINDIControl::ISystemConfigure, MulticopterINDIControl::ISystemPreUpdate) IGNITION_ADD_PLUGIN_ALIAS(MulticopterINDIControl

file INDIControlPlugin.hpp: #include <gz/msgs/actuators.pb.h>

#include <gz/msgs/boolean.pb.h>

#include <gz/msgs/float_v.pb.h>

#include <gz/msgs/twist.pb.h>

#include <Eigen/Geometry>

#include <memory>

#include <string>

#include <vector>

#include <gz/transport/Node.hh>

#include “gz/sim/System.hh”

#include “gz/sim/Link.hh”

#include “gz/sim/Model.hh”

#include “Common.hpp”

#include “IndiController.hpp”

file INDIControlPlugin.hpp: #include <gz/msgs/actuators.pb.h>

#include <gz/msgs/boolean.pb.h>

#include <gz/msgs/float_v.pb.h>

#include <gz/msgs/twist.pb.h>

#include <Eigen/Geometry>

#include <memory>

#include <string>

#include <vector>

#include <gz/transport/Node.hh>

#include <gz/sim/System.hh>

#include “gz/sim/Link.hh”

#include “gz/sim/Model.hh”

#include “Common.hpp”

#include “IndiController.hpp”

file Parameters.hpp: #include <Eigen/Geometry>

#include <vector>

#include “gz/sim/config.hh”

file Parameters.hpp: #include <Eigen/Geometry>

#include <vector>

#include “gz/sim/config.hh”

file PIDController.hpp: #include <math.h>

#include <Eigen/Dense>

#include <Eigen/Geometry>

#include <iostream>

#include <limits>

file PIDController.hpp: #include <math.h>

#include <Eigen/Dense>

#include <Eigen/Geometry>

#include <iostream>

#include <limits>

file SuctionGripper.cpp: #include <gz/plugin/Register.hh>

#include <gz/transport/Node.hh>

#include <gz/msgs/contacts.pb.hh>

#include <gz/msgs.hh>

#include <gz/gazebo/components.hh>

#include <gz/gazebo/Model.hh>

#include “SuctionGripper.hpp”

file SuctionGripper.hpp: #include <memory>

#include <string>

#include <utility>

#include <gz/gazebo/System.hh>

file acro_bridge.cpp

#include “as2_gazebo_assets/acro_bridge.hpp”

Gazebo bridge ACRO implementation file.

Authors: Francisco José Anguita Chamorro

file acro_bridge_node.cpp

#include “as2_gazebo_assets/acro_bridge.hpp”

Gazebo bridge ACRO main node file.

Authors: Francisco José Anguita Chamorro

Functions

int main(int argc, char *argv[])

file azimuth_bridge.cpp: #include “as2_gazebo_assets/azimuth_bridge.hpp”

file azimuth_bridge_node.cpp

#include “as2_gazebo_assets/azimuth_bridge.hpp”

Functions

int main(int argc, char *argv[])

file gimbal_bridge.cpp: #include “as2_gazebo_assets/gimbal_bridge.hpp”

file gimbal_bridge_node.cpp

#include “as2_gazebo_assets/gimbal_bridge.hpp”

Functions

int main(int argc, char *argv[])

file gps_bridge.cpp

#include “as2_gazebo_assets/gps_bridge.hpp”

Ignition bridge gps implementation file.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Pedro Arias Pérez Rafael Pérez Seguí Miguel Fernández Cortizas David Pérez Saura

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

file gps_bridge_node.cpp

#include “as2_gazebo_assets/gps_bridge.hpp”

Functions

int main(int argc, char *argv[])

file ground_truth_bridge.cpp

#include “as2_gazebo_assets/ground_truth_bridge.hpp”

Ignition bridge ground truth implementation file.

Gazebo bridge ground truth implementation file.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Javier Melero Deza Pedro Arias Pérez

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Pedro Arias Pérez Rafael Pérez Seguí Miguel Fernández Cortizas David Pérez Saura

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

file ground_truth_bridge_node.cpp

#include “as2_gazebo_assets/ground_truth_bridge.hpp”

Functions

int main(int argc, char *argv[])

file object_tf_broadcaster.cpp

#include “as2_gazebo_assets/object_tf_broadcaster.hpp”

Ignition bridge tf broadcaster implementation file.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

Authors: Javier Melero Deza Pedro Arias Pérez Rafael Pérez Seguí Miguel Fernández Cortizas David Pérez Saura

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

file object_tf_broadcaster_node.cpp

#include “as2_gazebo_assets/object_tf_broadcaster.hpp”

Functions

int main(int argc, char *argv[])

file set_entity_pose_bridge.cpp

#include “as2_gazebo_assets/set_entity_pose_bridge.hpp”

Set entity pose bridge implementation file.

Authors: Javier Melero Deza Pedro Arias Pérez

file set_entity_pose_bridge_node.cpp

#include “as2_gazebo_assets/set_entity_pose_bridge.hpp”

Set entity pose bridge node.

Authors: Javier Melero Deza Pedro Arias Pérez

Functions

int main(int argc, char *argv[])

file as2_state_estimator.hpp

#include <tf2_ros/buffer.h>

#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/transform_broadcaster.h>

#include <tf2_ros/transform_listener.h>

#include <filesystem>

#include <memory>

#include <pluginlib/class_loader.hpp>

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <geometry_msgs/msg/twist_stamped.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include <as2_core/names/topics.hpp>

#include <as2_core/node.hpp>

#include <as2_core/utils/frame_utils.hpp>

#include <as2_core/utils/tf_utils.hpp>

#include “plugin_base.hpp”

An state estimation server for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file ground_truth.hpp

#include <utility>

#include <regex>

#include <memory>

#include <geographic_msgs/msg/geo_point.hpp>

#include <as2_core/names/services.hpp>

#include <as2_core/utils/gps_utils.hpp>

#include <as2_core/utils/tf_utils.hpp>

#include <as2_msgs/srv/get_origin.hpp>

#include <as2_msgs/srv/set_origin.hpp>

#include “as2_state_estimator/plugin_base.hpp”

An state estimation plugin ground truth for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file ground_truth.cpp

#include “ground_truth.hpp”

#include <pluginlib/class_list_macros.hpp>

An state estimation plugin ground truth for AeroStack2 implementation

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file ground_truth_odometry_fuse.hpp

#include <Eigen/Geometry>

#include <string>

#include <utility>

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include <nav_msgs/msg/odometry.hpp>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <mocap4r2_msgs/msg/rigid_bodies.hpp>

#include <as2_core/names/services.hpp>

#include <as2_core/utils/gps_utils.hpp>

#include <as2_msgs/srv/get_origin.hpp>

#include <as2_msgs/srv/set_origin.hpp>

#include “as2_state_estimator/plugin_base.hpp”

An state estimation plugin external odom for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file ground_truth_odometry_fuse.cpp

#include “ground_truth_odometry_fuse.hpp”

#include <pluginlib/class_list_macros.hpp>

An state estimation plugin ground truth and external odom fusion for AeroStack2

Authors: Rafael Pérez Seguí

file mocap_pose.hpp

#include <tf2/LinearMath/Quaternion.h>

#include <tf2/LinearMath/Vector3.h>

#include <string>

#include <vector>

#include <mocap4r2_msgs/msg/rigid_bodies.hpp>

#include <rclcpp/duration.hpp>

#include “as2_state_estimator/plugin_base.hpp”

An state estimation plugin mocap_pose for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file mocap_pose.cpp

#include “mocap_pose.hpp”

#include <pluginlib/class_list_macros.hpp>

An state estimation plugin mocap_pose for AeroStack2 implementation

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file raw_odometry.hpp

#include <string>

#include <utility>

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include <geographic_msgs/msg/geo_point.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <as2_core/names/services.hpp>

#include <as2_core/utils/gps_utils.hpp>

#include <as2_msgs/srv/get_origin.hpp>

#include <as2_msgs/srv/set_origin.hpp>

#include “as2_state_estimator/plugin_base.hpp”

An state estimation plugin external odom for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file raw_odometry.cpp

#include “raw_odometry.hpp”

#include <pluginlib/class_list_macros.hpp>

An state estimation plugin external odom for AeroStack2 implementation

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file as2_state_estimator.cpp

#include “as2_state_estimator.hpp”

An state estimation server for AeroStack2 implementation

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

file as2_state_estimator_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_state_estimator.hpp”

Node for the state estimation server for AeroStack2

Authors: David Pérez Saura Rafael Pérez Seguí Javier Melero Deza Miguel Fernández Cortizas Pedro Arias Pérez

Functions

int main(int argc, char **argv)

file alphanumeric_viewer.hpp

#include <curses.h>

#include <math.h>

#include <stdio.h>

#include <tf2/utils.h>

#include <iostream>

#include <sstream>

#include <string>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_msgs/msg/controller_info.hpp”

#include “as2_msgs/msg/platform_info.hpp”

#include “as2_msgs/msg/thrust.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “geometry_msgs/msg/twist_stamped.hpp”

#include “sensor_msgs/msg/battery_state.hpp”

#include “sensor_msgs/msg/imu.hpp”

#include “sensor_msgs/msg/nav_sat_fix.hpp”

#include “sensor_msgs/msg/temperature.hpp”

Alphanumeric viewer header file.

Authors: Javier Melero Deza

Defines

ASCII_KEY_UP

ASCII_KEY_DOWN

ASCII_KEY_RIGHT

ASCII_KEY_LEFT

file alphanumeric_viewer.cpp

#include “alphanumeric_viewer.hpp”

Alphanumeric viewer source file.

Authors: Javier Melero Deza

Typedefs

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

file alphanumeric_viewer_node.cpp

#include “alphanumeric_viewer.hpp”

#include “as2_core/core_functions.hpp”

Alphanumeric viewer node file.

Authors: Javier Melero Deza

Functions

int main(int argc, char *argv[])

file teleop_panel.cpp: #include “teleop_panel.hpp”

#include <QHBoxLayout>

#include <QLabel>

#include <QLineEdit>

#include <QPainter>

#include <QPushButton>

#include <QTimer>

#include <QVBoxLayout>

#include <stdio.h>

#include <memory>

#include “pluginlib/class_list_macros.hpp”

file teleop_panel.hpp: #include <functional>

#include <future>

#include <memory>

#include <string>

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_motion_reference_handlers/hover_motion.hpp”

#include “as2_msgs/action/land.hpp”

#include “as2_msgs/action/takeoff.hpp”

#include “as2_msgs/msg/alert_event.hpp”

#include “rclcpp/rclcpp.hpp”

#include “rviz_common/panel.hpp”

#include “std_srvs/srv/set_bool.hpp”

#include <rclcpp_action/rclcpp_action.hpp>

file as2_external_object_to_tf.hpp

#include <geometry_msgs/msg/transform_stamped.h>

#include <tf2/utils.h>

#include <tf2_msgs/msg/tf_message.h>

#include <tf2_ros/static_transform_broadcaster.h>

#include <tf2_ros/transform_broadcaster.h>

#include <fstream>

#include <iostream>

#include <memory>

#include <map>

#include <vector>

#include <string>

#include <tuple>

#include “as2_msgs/srv/add_static_transform.hpp”

#include “as2_msgs/srv/add_static_transform_gps.hpp”

#include <geographic_msgs/msg/geo_point.hpp>

#include <geometry_msgs/msg/quaternion.hpp>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/names/actions.hpp”

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/gps_utils.hpp”

#include “as2_msgs/srv/get_origin.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “mocap4r2_msgs/msg/rigid_bodies.hpp”

#include “sensor_msgs/msg/nav_sat_fix.hpp”

#include “std_msgs/msg/float32.hpp”

#include “std_msgs/msg/string.hpp”

as2_external_object_to_tf header file.

Author: Javilinos

file as2_external_object_to_tf.cpp

#include “as2_external_object_to_tf.hpp”

#include “as2_core/utils/frame_utils.hpp”

#include “yaml-cpp/yaml.h”

as2_external_object_to_tf source file.

Author: Javilinos

Typedefs

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

file as2_external_object_to_tf_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_external_object_to_tf.hpp”

as2_external_object_to_tf node file.

Author: Javilinos

Functions

int main(int argc, char *argv[])

file as2_geozones.hpp

#include <limits>

#include <algorithm>

#include <filesystem>

#include <fstream>

#include <iostream>

#include <iterator>

#include <memory>

#include <string>

#include <vector>

#include “yaml-cpp/yaml.h”

#include “as2_msgs/msg/geozone.hpp”

#include “as2_msgs/msg/polygon_list.hpp”

#include “as2_msgs/srv/get_geozone.hpp”

#include “as2_msgs/srv/set_geozone.hpp”

#include <ament_index_cpp/get_package_share_directory.hpp>

#include <geographic_msgs/msg/geo_point.hpp>

#include <rclcpp/qos.hpp>

#include <rclcpp/rclcpp.hpp>

#include “as2_core/names/services.hpp”

#include “as2_core/names/topics.hpp”

#include “as2_core/node.hpp”

#include “as2_core/utils/gps_utils.hpp”

#include “as2_msgs/msg/alert_event.hpp”

#include “as2_msgs/srv/get_origin.hpp”

#include “geometry_msgs/msg/point32.hpp”

#include “geometry_msgs/msg/polygon_stamped.hpp”

#include “geometry_msgs/msg/pose_stamped.hpp”

#include “nav_msgs/msg/odometry.hpp”

#include “sensor_msgs/msg/nav_sat_fix.hpp”

#include “std_msgs/msg/int8.hpp”

#include “pnpoly.hpp”

as2_geozones header file.

Author: Javilinos

file pnpoly.hpp: #include <cmath>

#include <cstdint>

#include <algorithm>

#include <array>

#include <limits>

#include <type_traits>

#include <vector>

#include <ostream>

#include <iostream>

file as2_geozones.cpp

#include “as2_geozones.hpp”

as2_geozones source file.

Author: Javilinos

Typedefs

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

file as2_geozones_node.cpp

#include “as2_core/core_functions.hpp”

#include “as2_geozones.hpp”

#include “pnpoly.hpp”

as2_geozones node file.

Author: Javilinos

Functions

int main(int argc, char *argv[])

file README.md

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior/include/as2_behavior/__detail

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior/include/as2_behavior/__impl

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/a_star

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/acro_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/plugins/action

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/include/as2_behavior_tree/action

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_alphanumeric_viewer/include/as2_alphanumeric_viewer

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_alphanumeric_viewer

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior/include/as2_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/include/as2_behavior_tree

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/include/as2_behaviors_path_planning

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_platform

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_platform/include/as2_behaviors_platform

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_trajectory_generation

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/include/as2_core

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_external_object_to_tf/include/as2_external_object_to_tf

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_external_object_to_tf

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/include/as2_gazebo_assets

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_geozones

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_geozones/include/as2_geozones

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/include/as2_map_server

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/include/as2_motion_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_reference_handlers/include/as2_motion_reference_handlers

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_reference_handlers

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_gazebo/include/as2_platform_gazebo

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_gazebo

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_multirotor_simulator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_multirotor_simulator/include/as2_platform_multirotor_simulator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_realsense_interface/include/as2_realsense_interface

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_realsense_interface

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_visualization/as2_rviz_plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/include/as2_state_estimator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_usb_camera_interface

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_usb_camera_interface/include/as2_usb_camera_interface

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_visualization

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/azimuth_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/common

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/include/as2_behavior_tree/condition

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/plugins/condition

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/plugins/decorator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/include/as2_behavior_tree/decorator

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/detect_aruco_markers_behavior/include/detect_aruco_markers_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/detect_aruco_markers_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/differential_flatness_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/differential_flatness_controller/include/differential_flatness_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/voronoi/thirdparty/dynamicvoronoi

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_path_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_path_behavior/include/follow_path_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_reference_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_reference_behavior/include/follow_reference_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior/include/generate_polynomial_trajectory_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/gimbal_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/go_to_behavior/include/go_to_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/go_to_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/gps_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/ground_truth_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth_odometry_fuse

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_reference_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_usb_camera_interface/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/mocap_pose/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_realsense_interface/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/go_to_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/pid_speed_controller/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/plugins/scan2occ_grid/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth_odometry_fuse/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_path_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/differential_flatness_controller/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/common/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/a_star/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/voronoi/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_alphanumeric_viewer/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/takeoff_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_gazebo/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_multirotor_simulator/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/detect_aruco_markers_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/land_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/point_gimbal_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_platform/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_external_object_to_tf/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_geozones/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/raw_odometry/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_reference_handlers/include

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/plugins/indi-control

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/plugins/indi-control_ign

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/land_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/land_behavior/include/land_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/mocap_pose

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/include/as2_core/names

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/object_tf_broadcaster

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/pid_speed_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/pid_speed_controller/include/pid_speed_controller

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/takeoff_behavior/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/land_behavior/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/go_to_behavior/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_path_behavior/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/plugins

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/point_gimbal_behavior/include/point_gimbal_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/point_gimbal_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/raw_odometry

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/plugins/scan2occ_grid

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src/set_entity_pose_bridge

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/mocap_pose/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth_odometry_fuse/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_external_object_to_tf/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/raw_odometry/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_utilities/as2_geozones/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_visualization/as2_rviz_plugins/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_user_interfaces/as2_alphanumeric_viewer/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_state_estimator/plugins/ground_truth/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_gazebo/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_aerial_platforms/as2_platform_multirotor_simulator/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behavior_tree/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_path_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/follow_reference_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/go_to_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/land_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/takeoff_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/a_star/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/voronoi/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_simulation_assets/as2_gazebo_assets/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/detect_aruco_markers_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_platform/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_reference_handlers/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/pid_speed_controller/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_motion_controller/plugins/differential_flatness_controller/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_map_server/plugins/scan2occ_grid/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_usb_camera_interface/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_hardware_drivers/as2_realsense_interface/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_trajectory_generation/generate_polynomial_trajectory_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_perception/point_gimbal_behavior/src

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/takeoff_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_motion/takeoff_behavior/include/takeoff_behavior

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/voronoi/thirdparty

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/src/utils

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_core/include/as2_core/utils

dir /__w/aerostack2.github.io/aerostack2.github.io/main/docs/_09_development/_api_documentation/aerostack2/as2_behaviors/as2_behaviors_path_planning/plugins/voronoi

page Aerostack2

Aerostack2

Aerostack2 is a ROS 2 framework developed to create autonomous multi-aerial-robots systems in an easy and powerful way.

Currently is currently developed and tested over ROS 2 humble (over Ubuntu 22.04) .

Versions below 1.0.9 were also developed and tested over ROS 2 galactic (over Ubuntu 20.04), can be found in the branch EOL/galactic.

We also have docker images with Aerostack2 preinstalled in ROS 2 Humble, check it out at Aerostack2 Dockerhub.

Most important features:

Natively developed on ROS 2.
Complete modularity, allowing elements to be changed or interchanged without affecting the rest of the system.
Independence of the aerial platform. Easy Sim2Real deployment.
Project-oriented, allowing to install and use only the necessary packages for the application to be developed.
Swarming orientation.

Please visit the [Aerostack2 Documentation] for a complete documentation.

Installation instructions can be found [here].

https://user-images.githubusercontent.com/35956525/231999883-e491aa08-2835-47a9-9c68-5b2936e8594e.mp4

Credits

If you use the code in the academic context, please cite:

M. Fernandez-Cortizas, M. Molina, P. Arias-Perez, R. Perez-Segui, D. Perez-Saura, and P. Campoy, 2023, “Aerostack2: A software framework for developing multi-robot aerial systems”, ArXiv DOI 2303.18237.