diff --git a/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp
index 570feb79c865074341293bc54296b401f4787336..8b401e8a595df9158724868d339484a33db831b3 100644
--- a/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp
+++ b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp
@@ -22,11 +22,17 @@
namespace nav2_util
{
-
+/**
+ * @class nav2_util::ClearEntirelyCostmapServiceClient
+ * @brief A service client to clear costmaps entirely
+ */
class ClearEntirelyCostmapServiceClient
: public nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>
{
public:
+ /**
+* @brief A constructor for nav2_util::ClearEntirelyCostmapServiceClient
+ */
explicit ClearEntirelyCostmapServiceClient(const std::string & service_name)
: nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>(service_name)
{
diff --git a/nav2_util/include/nav2_util/costmap.hpp b/nav2_util/include/nav2_util/costmap.hpp
index c811007e4cedd981b80cdedd7e67af05fbe86ad7..0d868df6e033998f56cd60a188c7cdd3ba8c99bd 100644
--- a/nav2_util/include/nav2_util/costmap.hpp
+++ b/nav2_util/include/nav2_util/costmap.hpp
@@ -36,28 +36,65 @@ enum class TestCostmap
maze2
};
-// Class for a single layered costmap initialized from an
-// occupancy grid representing the map.
+/**
+ * @class nav2_util::Costmap
+ * @brief Class for a single layered costmap initialized from an
+ * occupancy grid representing the map.
+ */
class Costmap
{
public:
typedef uint8_t CostValue;
+ /**
+ * @brief A constructor for nav2_util::Costmap
+ * @param node Ptr to a node
+ * @param trinary_costmap Whether the costmap should be trinary
+ * @param track_unknown_space Whether to track unknown space in costmap
+ * @param lethal_threshold The lethal space cost threshold to use
+ * @param unknown_cost_value Internal costmap cell value for unknown space
+ */
Costmap(
rclcpp::Node * node, bool trinary_costmap = true, bool track_unknown_space = true,
int lethal_threshold = 100, int unknown_cost_value = -1);
Costmap() = delete;
~Costmap();
+ /**
+ * @brief Set the static map of this costmap
+ * @param occupancy_grid Occupancy grid to populate this costmap with
+ */
void set_static_map(const nav_msgs::msg::OccupancyGrid & occupancy_grid);
+ /**
+ * @brief Set the test costmap type of this costmap
+ * @param testCostmapType Type of stored costmap to use
+ */
void set_test_costmap(const TestCostmap & testCostmapType);
+ /**
+ * @brief Get a costmap message from this object
+ * @param specifications Parameters of costmap
+ * @return Costmap msg of this costmap
+ */
nav2_msgs::msg::Costmap get_costmap(const nav2_msgs::msg::CostmapMetaData & specifications);
+ /**
+ * @brief Get a metadata message from this object
+ * @return Costmap metadata of this costmap
+ */
nav2_msgs::msg::CostmapMetaData get_properties() {return costmap_properties_;}
+ /**
+ * @brief Get whether some coordinates are free
+ * @return bool if free
+ */
bool is_free(const unsigned int x_coordinate, const unsigned int y_coordinate) const;
+
+ /**
+ * @brief Get whether some index in the costmap is free
+ * @return bool if free
+ */
bool is_free(const unsigned int index) const;
// Mapping for often used cost values
@@ -68,8 +105,16 @@ public:
static const CostValue free_space;
private:
+ /**
+ * @brief Get data from the test
+ * @return data
+ */
std::vector<uint8_t> get_test_data(const TestCostmap configuration);
+ /**
+ * @brief Get the interpreted value in the costmap
+ * @return uint value
+ */
uint8_t interpret_value(const int8_t value) const;
// Costmap isn't itself a node
diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp
index 52bd5ebb417edfb743cb2d86929a938a9dd05514..f4bef5b1405c9cec5fc873c76be917981410d152 100644
--- a/nav2_util/include/nav2_util/geometry_utils.hpp
+++ b/nav2_util/include/nav2_util/geometry_utils.hpp
@@ -28,6 +28,11 @@ namespace nav2_util
namespace geometry_utils
{
+/**
+ * @brief Get a geometry_msgs Quaternion from a yaw angle
+ * @param angle Yaw angle to generate a quaternion from
+ * @return geometry_msgs Quaternion
+ */
inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
{
tf2::Quaternion q;
@@ -35,6 +40,12 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle)
return tf2::toMsg(q);
}
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::Points
+ * @param pos1 First point
+ * @param pos1 Second point
+ * @param double L2 distance
+ */
inline double euclidean_distance(
const geometry_msgs::msg::Point & pos1,
const geometry_msgs::msg::Point & pos2)
@@ -45,6 +56,12 @@ inline double euclidean_distance(
return std::sqrt(dx * dx + dy * dy + dz * dz);
}
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::Poses
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @param double L2 distance
+ */
inline double euclidean_distance(
const geometry_msgs::msg::Pose & pos1,
const geometry_msgs::msg::Pose & pos2)
@@ -55,6 +72,12 @@ inline double euclidean_distance(
return std::sqrt(dx * dx + dy * dy + dz * dz);
}
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @param double L2 distance
+ */
inline double euclidean_distance(
const geometry_msgs::msg::PoseStamped & pos1,
const geometry_msgs::msg::PoseStamped & pos2)
diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp
index 9180133817f708cdf9495363cae288720334a9c6..3a76786f9b461be22e23944c5fc507ce3cf10bba 100644
--- a/nav2_util/include/nav2_util/lifecycle_node.hpp
+++ b/nav2_util/include/nav2_util/lifecycle_node.hpp
@@ -35,9 +35,21 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
// to interface to classes, such as MessageFilter and TransformListener, that don't yet
// support lifecycle nodes. Once we get the fixes into ROS2, this class will be removed.
+/**
+ * @class nav2_util::LifecycleNode
+ * @brief A lifecycle node wrapper to enable common Nav2 needs such as background node threads
+ * and manipulating parameters
+ */
class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
{
public:
+ /**
+ * @brief A lifecycle node constructor
+ * @param node_name Name for the node
+ * @param namespace Namespace for the node, if any
+ * @param use_rclcpp_node Whether to create an internal client node
+ * @param options Node options
+ */
LifecycleNode(
const std::string & node_name,
const std::string & namespace_ = "",
@@ -59,7 +71,14 @@ public:
int step;
} integer_range;
- // Declare a parameter that has no integer or floating point range constraints
+ /**
+ * @brief Declare a parameter that has no integer or floating point range constraints
+ * @param node_name Name of parameter
+ * @param default_value Default node value to add
+ * @param description Node description
+ * @param additional_constraints Any additional constraints on the parameters to list
+ * @param read_only Whether this param should be considered read only
+ */
void add_parameter(
const std::string & name, const rclcpp::ParameterValue & default_value,
const std::string & description = "", const std::string & additional_constraints = "",
@@ -75,7 +94,15 @@ public:
declare_parameter(descriptor.name, default_value, descriptor);
}
- // Declare a parameter that has a floating point range constraint
+ /**
+ * @brief Declare a parameter that has a floating point range constraint
+ * @param node_name Name of parameter
+ * @param default_value Default node value to add
+ * @param fp_range floating point range
+ * @param description Node description
+ * @param additional_constraints Any additional constraints on the parameters to list
+ * @param read_only Whether this param should be considered read only
+ */
void add_parameter(
const std::string & name, const rclcpp::ParameterValue & default_value,
const floating_point_range fp_range,
@@ -96,7 +123,15 @@ public:
declare_parameter(descriptor.name, default_value, descriptor);
}
- // Declare a parameter that has an integer range constraint
+ /**
+ * @brief Declare a parameter that has an integer range constraint
+ * @param node_name Name of parameter
+ * @param default_value Default node value to add
+ * @param integer_range Integer range
+ * @param description Node description
+ * @param additional_constraints Any additional constraints on the parameters to list
+ * @param read_only Whether this param should be considered read only
+ */
void add_parameter(
const std::string & name, const rclcpp::ParameterValue & default_value,
const integer_range int_range,
@@ -117,12 +152,21 @@ public:
declare_parameter(descriptor.name, default_value, descriptor);
}
+ /**
+ * @brief Get a shared pointer of this
+ */
std::shared_ptr<nav2_util::LifecycleNode> shared_from_this()
{
return std::static_pointer_cast<nav2_util::LifecycleNode>(
rclcpp_lifecycle::LifecycleNode::shared_from_this());
}
+ /**
+ * @brief Abstracted on_error state transition callback, since unimplemented as of 2020
+ * in the managed ROS2 node state machine
+ * @param state State prior to error transition
+ * @return Return type for success or failed transition to error state
+ */
nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_FATAL(
@@ -131,11 +175,20 @@ public:
return nav2_util::CallbackReturn::SUCCESS;
}
- // bond connection to lifecycle manager
+ /**
+ * @brief Create bond connection to lifecycle manager
+ */
void createBond();
+
+ /**
+ * @brief Destroy bond connection to lifecycle manager
+ */
void destroyBond();
protected:
+ /**
+ * @brief Print notifications for lifecycle node
+ */
void print_lifecycle_node_notification();
// Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't
diff --git a/nav2_util/include/nav2_util/line_iterator.hpp b/nav2_util/include/nav2_util/line_iterator.hpp
index a9ff0c1f435cddd4d93f541ae9e7a5f094e22e25..82ea12425ead929884be3bac974dca5dcc6742fd 100644
--- a/nav2_util/include/nav2_util/line_iterator.hpp
+++ b/nav2_util/include/nav2_util/line_iterator.hpp
@@ -38,10 +38,21 @@
namespace nav2_util
{
-/** An iterator implementing Bresenham Ray-Tracing. */
+
+/**
+ * @class nav2_util::LineIterator
+ * @brief An iterator implementing Bresenham Ray-Tracing.
+ */
class LineIterator
{
public:
+ /**
+ * @brief A constructor for LineIterator
+ * @param x0 Starting x
+ * @param y0 Starting y
+ * @param x1 Ending x
+ * @param y1 Ending y
+ */
LineIterator(int x0, int y0, int x1, int y1)
: x0_(x0),
y0_(y0),
@@ -86,11 +97,18 @@ public:
}
}
+ /**
+ * @brief If the iterator is valid
+ * @return bool If valid
+ */
bool isValid() const
{
return curpixel_ <= numpixels_;
}
+ /**
+ * @brief Advance iteration along the line
+ */
void advance()
{
num_ += numadd_; // Increase the numerator by the top of the fraction
@@ -105,28 +123,55 @@ public:
curpixel_++;
}
+ /**
+ * @brief Get current X value
+ * @return X
+ */
int getX() const
{
return x_;
}
+
+ /**
+ * @brief Get current Y value
+ * @return Y
+ */
int getY() const
{
return y_;
}
+ /**
+ * @brief Get initial X value
+ * @return X
+ */
int getX0() const
{
return x0_;
}
+
+ /**
+ * @brief Get initial Y value
+ * @return Y
+ */
int getY0() const
{
return y0_;
}
+ /**
+ * @brief Get terminal X value
+ * @return X
+ */
int getX1() const
{
return x1_;
}
+
+ /**
+ * @brief Get terminal Y value
+ * @return Y
+ */
int getY1() const
{
return y1_;
diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp
index 97ab46cee6a97033425e6c37dedf234ec6e7d886..634b4acd5bc1915997586986b20bd58ab6afb337 100644
--- a/nav2_util/include/nav2_util/node_thread.hpp
+++ b/nav2_util/include/nav2_util/node_thread.hpp
@@ -21,17 +21,31 @@
namespace nav2_util
{
-
+/**
+ * @class nav2_util::NodeThread
+ * @brief A background thread to process node callbacks
+ */
class NodeThread
{
public:
+ /**
+ * @brief A background thread to process node callbacks constructor
+ * @param node_base Interface to Node to spin in thread
+ */
explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
+ /**
+ * @brief A background thread to process node callbacks constructor
+ * @param node Node pointer to spin in thread
+ */
template<typename NodeT>
explicit NodeThread(NodeT node)
: NodeThread(node->get_node_base_interface())
{}
+ /**
+ * @brief A destructor
+ */
~NodeThread();
protected:
diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp
index f75d01ebefbf1b0c1f23a9c0a1df306ea8a21d3b..e6f7b7ede93ef5befcf4bb68ab6ac4b73f07ee54 100644
--- a/nav2_util/include/nav2_util/odometry_utils.hpp
+++ b/nav2_util/include/nav2_util/odometry_utils.hpp
@@ -52,11 +52,28 @@ public:
double filter_duration = 0.3,
const std::string & odom_topic = "odom");
+ /**
+ * @brief Get twist msg from smoother
+ * @return twist Twist msg
+ */
inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;}
+
+ /**
+ * @brief Get twist stamped msg from smoother
+ * @return twist TwistStamped msg
+ */
inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;}
protected:
+ /**
+ * @brief Callback of odometry subscriber to process
+ * @param msg Odometry msg to smooth
+ */
void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg);
+
+ /**
+ * @brief Update internal state of the smoother after getting new data
+ */
void updateState();
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
index 7dafe77e718eb0af49f955727f713e3fa72ac7fb..22f3f7f160b37d12ab7dfe09354343c9aedc6e71 100644
--- a/nav2_util/include/nav2_util/robot_utils.hpp
+++ b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -30,12 +30,29 @@
namespace nav2_util
{
-
+/**
+* @brief get the current pose of the robot
+* @param global_pose Pose to transform
+* @param tf_buffer TF buffer to use for the transformation
+* @param global_frame Frame to transform into
+* @param robot_frame Frame to transform from
+* @param transform_timeout TF Timeout to use for transformation
+* @return bool Whether it could be transformed successfully
+*/
bool getCurrentPose(
geometry_msgs::msg::PoseStamped & global_pose,
tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
const std::string robot_frame = "base_link", const double transform_timeout = 0.1);
+/**
+* @brief get an arbitrary pose in a target frame
+* @param input_pose Pose to transform
+* @param transformed_pose Output transformation
+* @param tf_buffer TF buffer to use for the transformation
+* @param target_frame Frame to transform into
+* @param transform_timeout TF Timeout to use for transformation
+* @return bool Whether it could be transformed successfully
+*/
bool transformPoseInTargetFrame(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped & transformed_pose,
diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp
index 65ae36e9517aad832f02397edb924a956364c7ea..021eb61c4ea01e151593ccc4efe5a878e4fb4f2b 100644
--- a/nav2_util/include/nav2_util/service_client.hpp
+++ b/nav2_util/include/nav2_util/service_client.hpp
@@ -22,10 +22,19 @@
namespace nav2_util
{
+/**
+ * @class nav2_util::ServiceClient
+ * @brief A simple wrapper on ROS2 services for invoke() and block-style calling
+ */
template<class ServiceT>
class ServiceClient
{
public:
+ /**
+ * @brief A constructor
+ * @param service_name name of the service to call
+ * @param provided_node Node to create the service client off of
+ */
explicit ServiceClient(
const std::string & service_name,
const rclcpp::Node::SharedPtr & provided_node)
@@ -44,6 +53,12 @@ public:
using RequestType = typename ServiceT::Request;
using ResponseType = typename ServiceT::Response;
+ /**
+ * @brief Invoke the service and block until completed or timed out
+ * @param request The request object to call the service using
+ * @param timeout Maximum timeout to wait for, default infinite
+ * @return Response A pointer to the service response from the request
+ */
typename ResponseType::SharedPtr invoke(
typename RequestType::SharedPtr & request,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1))
@@ -72,6 +87,12 @@ public:
return future_result.get();
}
+ /**
+ * @brief Invoke the service and block until completed
+ * @param request The request object to call the service using
+ * @param Response A pointer to the service response from the request
+ * @return bool Whether it was successfully called
+ */
bool invoke(
typename RequestType::SharedPtr & request,
typename ResponseType::SharedPtr & response)
@@ -101,6 +122,10 @@ public:
return response.get();
}
+ /**
+ * @brief Block until a service is available
+ * @param timeout Maximum timeout to wait for, default infinite
+ */
void wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
{
auto sleep_dur = std::chrono::milliseconds(10);
diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp
index f0c01b71acbfbbcd24605f5160408ad84d39bb52..4cd23dcb42775f24e4a14aa435b14ac5e385503c 100644
--- a/nav2_util/include/nav2_util/simple_action_server.hpp
+++ b/nav2_util/include/nav2_util/simple_action_server.hpp
@@ -28,12 +28,23 @@
namespace nav2_util
{
+/**
+ * @class nav2_util::SimpleActionServer
+ * @brief An action server wrapper to make applications simpler using Actions
+ */
template<typename ActionT, typename nodeT = rclcpp::Node>
class SimpleActionServer
{
public:
typedef std::function<void ()> ExecuteCallback;
+ /**
+ * @brief An constructor for SimpleActionServer
+ * @param node Ptr to node to make actions
+ * @param action_name Name of the action to call
+ * @param execute_callback Execution callback function of Action
+ * @param server_timeout Timeout to to react to stop or preemption requests
+ */
explicit SimpleActionServer(
typename nodeT::SharedPtr node,
const std::string & action_name,
@@ -47,6 +58,13 @@ public:
action_name, execute_callback, server_timeout)
{}
+ /**
+ * @brief An constructor for SimpleActionServer
+ * @param <node interfaces> Abstract node interfaces to make actions
+ * @param action_name Name of the action to call
+ * @param execute_callback Execution callback function of Action
+ * @param server_timeout Timeout to to react to stop or preemption requests
+ */
explicit SimpleActionServer(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
@@ -75,6 +93,12 @@ public:
std::bind(&SimpleActionServer::handle_accepted, this, _1));
}
+ /**
+ * @brief handle the goal requested: accept or reject. This implementation always accepts.
+ * @param uuid Goal ID
+ * @param Goal A shared pointer to the specific goal
+ * @return GoalResponse response of the goal processed
+ */
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & /*uuid*/,
std::shared_ptr<const typename ActionT::Goal>/*goal*/)
@@ -89,6 +113,12 @@ public:
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
+ /**
+ * @brief Accepts cancellation requests of action server.
+ * @param uuid Goal ID
+ * @param Goal A server goal handle to cancel
+ * @return CancelResponse response of the goal cancelled
+ */
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>/*handle*/)
{
@@ -97,6 +127,10 @@ public:
return rclcpp_action::CancelResponse::ACCEPT;
}
+ /**
+ * @brief Handles accepted goals and adds to preempted queue to switch to
+ * @param Goal A server goal handle to cancel
+ */
void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
@@ -129,6 +163,9 @@ public:
}
}
+ /**
+ * @brief Computed background work and processes stop requests
+ */
void work()
{
while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) {
@@ -168,6 +205,9 @@ public:
debug_msg("Worker thread done.");
}
+ /**
+ * @brief Active action server
+ */
void activate()
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
@@ -175,6 +215,9 @@ public:
stop_execution_ = false;
}
+ /**
+ * @brief Deactive action server
+ */
void deactivate()
{
debug_msg("Deactivating...");
@@ -208,6 +251,10 @@ public:
debug_msg("Deactivation completed.");
}
+ /**
+ * @brief Whether the action server is munching on a goal
+ * @return bool If its running or not
+ */
bool is_running()
{
return execution_future_.valid() &&
@@ -215,18 +262,30 @@ public:
std::future_status::timeout);
}
+ /**
+ * @brief Whether the action server is active or not
+ * @return bool If its active or not
+ */
bool is_server_active()
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
return server_active_;
}
+ /**
+ * @brief Whether the action server has been asked to be preempted with a new goal
+ * @return bool If there's a preemption request or not
+ */
bool is_preempt_requested() const
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
return preempt_requested_;
}
+ /**
+ * @brief Accept pending goals
+ * @return Goal Ptr to the goal that's going to be accepted
+ */
const std::shared_ptr<const typename ActionT::Goal> accept_pending_goal()
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
@@ -250,6 +309,10 @@ public:
return current_handle_->get_goal();
}
+ /**
+ * @brief Get the current goal object
+ * @return Goal Ptr to the goal that's being processed currently
+ */
const std::shared_ptr<const typename ActionT::Goal> get_current_goal() const
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
@@ -262,6 +325,10 @@ public:
return current_handle_->get_goal();
}
+ /**
+ * @brief Whether or not a cancel command has come in
+ * @return bool Whether a cancel command has been requested or not
+ */
bool is_cancel_requested() const
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
@@ -280,6 +347,10 @@ public:
return current_handle_->is_canceling();
}
+ /**
+ * @brief Terminate all pending and active actions
+ * @param result A result object to send to the terminated actions
+ */
void terminate_all(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>())
@@ -290,6 +361,10 @@ public:
preempt_requested_ = false;
}
+ /**
+ * @brief Terminate the active action
+ * @param result A result object to send to the terminated action
+ */
void terminate_current(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>())
@@ -298,6 +373,10 @@ public:
terminate(current_handle_, result);
}
+ /**
+ * @brief Return success of the active action
+ * @param result A result object to send to the terminated actions
+ */
void succeeded_current(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>())
@@ -311,6 +390,10 @@ public:
}
}
+ /**
+ * @brief Publish feedback to the action server clients
+ * @param feedback A feedback object to send to the clients
+ */
void publish_feedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
{
if (!is_active(current_handle_)) {
@@ -343,17 +426,30 @@ protected:
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
+ /**
+ * @brief Generate an empty result object for an action type
+ */
constexpr auto empty_result() const
{
return std::make_shared<typename ActionT::Result>();
}
+ /**
+ * @brief Whether a given goal handle is currently active
+ * @param handle Goal handle to check
+ * @return Whether this goal handle is active
+ */
constexpr bool is_active(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle) const
{
return handle != nullptr && handle->is_active();
}
+ /**
+ * @brief Terminate a particular action with a result
+ * @param handle goal handle to terminate
+ * @param the Results object to terminate the action with
+ */
void terminate(
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle,
typename std::shared_ptr<typename ActionT::Result> result =
@@ -373,6 +469,9 @@ protected:
}
}
+ /**
+ * @brief Info logging
+ */
void info_msg(const std::string & msg) const
{
RCLCPP_INFO(
@@ -380,6 +479,9 @@ protected:
"[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
}
+ /**
+ * @brief Debug logging
+ */
void debug_msg(const std::string & msg) const
{
RCLCPP_DEBUG(
@@ -387,6 +489,9 @@ protected:
"[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
}
+ /**
+ * @brief Error logging
+ */
void error_msg(const std::string & msg) const
{
RCLCPP_ERROR(
@@ -394,6 +499,9 @@ protected:
"[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
}
+ /**
+ * @brief Warn logging
+ */
void warn_msg(const std::string & msg) const
{
RCLCPP_WARN(
diff --git a/nav2_util/include/nav2_util/string_utils.hpp b/nav2_util/include/nav2_util/string_utils.hpp
index 1f3478a68d04b2fc48388a38834c8856907e381b..06d93b82a2651c7e1532f5bacdad731872aa21eb 100644
--- a/nav2_util/include/nav2_util/string_utils.hpp
+++ b/nav2_util/include/nav2_util/string_utils.hpp
@@ -23,9 +23,20 @@ namespace nav2_util
typedef std::vector<std::string> Tokens;
+/*
+ * @brief Remove leading slash from a topic name
+ * @param in String of topic in
+ * @return String out without slash
+*/
std::string strip_leading_slash(const std::string & in);
-/// Split a string at the delimiters
+///
+/*
+ * @brief Split a string at the delimiters
+ * @param in String to split
+ * @param Delimiter criteria
+ * @return Tokens
+*/
Tokens split(const std::string & tokenstring, char delimiter);
} // namespace nav2_util