aerostack2

struct Acro_command

Public Members

Eigen::Vector3d PQR = Eigen::Vector3d::Zero()

double thrust = 0.0

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.

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 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(std::shared_ptr<const typename actionT::Goal> goal)

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

std::chrono::nanoseconds tf_timeout_ = TF_TIMEOUT

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::PoseStampedWithID>::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 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::PoseStampedWithID::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::Vector &waypoints)

bool evaluateTrajectory(double eval_time)

bool evaluateSetpoint(double eval_time, as2_msgs::msg::TrajectoryPoint &trajectory_command)

double computeYawAnglePathFacing(double vx, double vy)

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::PoseStampedWithID>::SharedPtr mod_waypoint_sub_

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::chrono::nanoseconds tf_timeout_

std::string base_link_frame_id_

Parameters

std::string desired_frame_id_

int sampling_n_ = 1

double sampling_dt_ = 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_

double current_yaw_

as2_msgs::msg::TrajectorySetpoints trajectory_command_

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

rclcpp::Time time_zero_

bool first_run_ = false

bool has_odom_ = false

bool enable_debug_ = true

std::thread plot_thread_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_

For debuging Debug publishers

rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr 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 follow_path_plugin_params

Public Members

double follow_path_speed = 0.0

double follow_path_threshold = 0.0

double tf_timeout_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