Skip to content
Snippets Groups Projects
Unverified Commit afd0d842 authored by Steven Macenski's avatar Steven Macenski Committed by GitHub
Browse files

adding doxygen for nav2-utils (#2237)

parent 27db9a49
No related branches found
No related tags found
No related merge requests found
Showing with 375 additions and 11 deletions
......@@ -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)
{
......
......@@ -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
......
......@@ -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)
......
......@@ -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
......
......@@ -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_;
......
......@@ -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:
......
......@@ -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_;
......
......@@ -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,
......
......@@ -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);
......
......@@ -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(
......
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment