add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,277 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
|
||||
#define NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
|
||||
#include "nav2_core/controller.hpp"
|
||||
#include "nav2_core/progress_checker.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_msgs/msg/speed_limit.hpp"
|
||||
#include "nav_2d_utils/odom_subscriber.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/simple_action_server.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
#include "pluginlib/class_loader.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
class ProgressChecker;
|
||||
/**
|
||||
* @class nav2_controller::ControllerServer
|
||||
* @brief This class hosts variety of plugins of different algorithms to
|
||||
* complete control tasks from the exposed FollowPath action server.
|
||||
*/
|
||||
class ControllerServer : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
using ControllerMap = std::unordered_map<std::string, nav2_core::Controller::Ptr>;
|
||||
using GoalCheckerMap = std::unordered_map<std::string, nav2_core::GoalChecker::Ptr>;
|
||||
|
||||
/**
|
||||
* @brief Constructor for nav2_controller::ControllerServer
|
||||
* @param options Additional options to control creation of the node.
|
||||
*/
|
||||
explicit ControllerServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
/**
|
||||
* @brief Destructor for nav2_controller::ControllerServer
|
||||
*/
|
||||
~ControllerServer();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Configures controller parameters and member variables
|
||||
*
|
||||
* Configures controller plugin and costmap; Initialize odom subscriber,
|
||||
* velocity publisher and follow path action server.
|
||||
* @param state LifeCycle Node's state
|
||||
* @return Success or Failure
|
||||
* @throw pluginlib::PluginlibException When failed to initialize controller
|
||||
* plugin
|
||||
*/
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Activates member variables
|
||||
*
|
||||
* Activates controller, costmap, velocity publisher and follow path action
|
||||
* server
|
||||
* @param state LifeCycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Deactivates member variables
|
||||
*
|
||||
* Deactivates follow path action server, controller, costmap and velocity
|
||||
* publisher. Before calling deactivate state, velocity is being set to zero.
|
||||
* @param state LifeCycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Calls clean up states and resets member variables.
|
||||
*
|
||||
* Controller and costmap clean up state is called, and resets rest of the
|
||||
* variables
|
||||
* @param state LifeCycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Called when in Shutdown state
|
||||
* @param state LifeCycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
|
||||
|
||||
using Action = nav2_msgs::action::FollowPath;
|
||||
using ActionServer = nav2_util::SimpleActionServer<Action>;
|
||||
|
||||
// Our action server implements the FollowPath action
|
||||
std::unique_ptr<ActionServer> action_server_;
|
||||
|
||||
/**
|
||||
* @brief FollowPath action server callback. Handles action server updates and
|
||||
* spins server until goal is reached
|
||||
*
|
||||
* Provides global path to controller received from action client. Twist
|
||||
* velocities for the robot are calculated and published using controller at
|
||||
* the specified rate till the goal is reached.
|
||||
* @throw nav2_core::PlannerException
|
||||
*/
|
||||
void computeControl();
|
||||
|
||||
/**
|
||||
* @brief Find the valid controller ID name for the given request
|
||||
*
|
||||
* @param c_name The requested controller name
|
||||
* @param name Reference to the name to use for control if any valid available
|
||||
* @return bool Whether it found a valid controller to use
|
||||
*/
|
||||
bool findControllerId(const std::string & c_name, std::string & name);
|
||||
|
||||
/**
|
||||
* @brief Find the valid goal checker ID name for the specified parameter
|
||||
*
|
||||
* @param c_name The goal checker name
|
||||
* @param name Reference to the name to use for goal checking if any valid available
|
||||
* @return bool Whether it found a valid goal checker to use
|
||||
*/
|
||||
bool findGoalCheckerId(const std::string & c_name, std::string & name);
|
||||
|
||||
/**
|
||||
* @brief Assigns path to controller
|
||||
* @param path Path received from action server
|
||||
*/
|
||||
void setPlannerPath(const nav_msgs::msg::Path & path);
|
||||
/**
|
||||
* @brief Calculates velocity and publishes to "cmd_vel" topic
|
||||
*/
|
||||
void computeAndPublishVelocity();
|
||||
/**
|
||||
* @brief Calls setPlannerPath method with an updated path received from
|
||||
* action server
|
||||
*/
|
||||
void updateGlobalPath();
|
||||
/**
|
||||
* @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
|
||||
* @param velocity Twist velocity to be published
|
||||
*/
|
||||
void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity);
|
||||
/**
|
||||
* @brief Calls velocity publisher to publish zero velocity
|
||||
*/
|
||||
void publishZeroVelocity();
|
||||
/**
|
||||
* @brief Checks if goal is reached
|
||||
* @return true or false
|
||||
*/
|
||||
bool isGoalReached();
|
||||
/**
|
||||
* @brief Obtain current pose of the robot
|
||||
* @param pose To store current pose of the robot
|
||||
* @return true if able to obtain current pose of the robot, else false
|
||||
*/
|
||||
bool getRobotPose(geometry_msgs::msg::PoseStamped & pose);
|
||||
|
||||
/**
|
||||
* @brief get the thresholded velocity
|
||||
* @param velocity The current velocity from odometry
|
||||
* @param threshold The minimum velocity to return non-zero
|
||||
* @return double velocity value
|
||||
*/
|
||||
double getThresholdedVelocity(double velocity, double threshold)
|
||||
{
|
||||
return (std::abs(velocity) > threshold) ? velocity : 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief get the thresholded Twist
|
||||
* @param Twist The current Twist from odometry
|
||||
* @return Twist Twist after thresholds applied
|
||||
*/
|
||||
nav_2d_msgs::msg::Twist2D getThresholdedTwist(const nav_2d_msgs::msg::Twist2D & twist)
|
||||
{
|
||||
nav_2d_msgs::msg::Twist2D twist_thresh;
|
||||
twist_thresh.x = getThresholdedVelocity(twist.x, min_x_velocity_threshold_);
|
||||
twist_thresh.y = getThresholdedVelocity(twist.y, min_y_velocity_threshold_);
|
||||
twist_thresh.theta = getThresholdedVelocity(twist.theta, min_theta_velocity_threshold_);
|
||||
return twist_thresh;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a parameter change is detected
|
||||
* @param event ParameterEvent message
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
std::mutex dynamic_params_lock_;
|
||||
|
||||
// The controller needs a costmap node
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
|
||||
|
||||
// Publishers and subscribers
|
||||
std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
|
||||
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
|
||||
rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
|
||||
|
||||
// Progress Checker Plugin
|
||||
pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
|
||||
nav2_core::ProgressChecker::Ptr progress_checker_;
|
||||
std::string default_progress_checker_id_;
|
||||
std::string default_progress_checker_type_;
|
||||
std::string progress_checker_id_;
|
||||
std::string progress_checker_type_;
|
||||
|
||||
// Goal Checker Plugin
|
||||
pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
|
||||
GoalCheckerMap goal_checkers_;
|
||||
std::vector<std::string> default_goal_checker_ids_;
|
||||
std::vector<std::string> default_goal_checker_types_;
|
||||
std::vector<std::string> goal_checker_ids_;
|
||||
std::vector<std::string> goal_checker_types_;
|
||||
std::string goal_checker_ids_concat_, current_goal_checker_;
|
||||
|
||||
// Controller Plugins
|
||||
pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
|
||||
ControllerMap controllers_;
|
||||
std::vector<std::string> default_ids_;
|
||||
std::vector<std::string> default_types_;
|
||||
std::vector<std::string> controller_ids_;
|
||||
std::vector<std::string> controller_types_;
|
||||
std::string controller_ids_concat_, current_controller_;
|
||||
|
||||
double controller_frequency_;
|
||||
double min_x_velocity_threshold_;
|
||||
double min_y_velocity_threshold_;
|
||||
double min_theta_velocity_threshold_;
|
||||
|
||||
double failure_tolerance_;
|
||||
|
||||
// Whether we've published the single controller warning yet
|
||||
geometry_msgs::msg::PoseStamped end_pose_;
|
||||
|
||||
// Last time the controller generated a valid command
|
||||
rclcpp::Time last_valid_cmd_time_;
|
||||
|
||||
// Current path container
|
||||
nav_msgs::msg::Path current_path_;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Callback for speed limiting messages
|
||||
* @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
|
||||
*/
|
||||
void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
|
||||
@@ -0,0 +1,67 @@
|
||||
// Copyright (c) 2023 Dexory
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
|
||||
#define NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_controller/plugins/simple_progress_checker.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
/**
|
||||
* @class PoseProgressChecker
|
||||
* @brief This plugin is used to check the position and the angle of the robot to make sure
|
||||
* that it is actually progressing or rotating towards a goal.
|
||||
*/
|
||||
|
||||
class PoseProgressChecker : public SimpleProgressChecker
|
||||
{
|
||||
public:
|
||||
void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name) override;
|
||||
bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Calculates robots movement from baseline pose
|
||||
* @param pose Current pose of the robot
|
||||
* @return true, if movement is greater than radius_, or false
|
||||
*/
|
||||
bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
|
||||
|
||||
static double poseAngleDistance(
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const geometry_msgs::msg::Pose2D &);
|
||||
|
||||
double required_movement_angle_;
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
std::string plugin_name_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
|
||||
@@ -0,0 +1,78 @@
|
||||
// Copyright (c) 2025 Prabhav Saxena
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
|
||||
#define NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
/**
|
||||
* @class PositionGoalChecker
|
||||
* @brief Goal Checker plugin that only checks XY position, ignoring orientation
|
||||
*/
|
||||
class PositionGoalChecker : public nav2_core::GoalChecker
|
||||
{
|
||||
public:
|
||||
PositionGoalChecker();
|
||||
~PositionGoalChecker() override = default;
|
||||
|
||||
void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
|
||||
|
||||
void reset() override;
|
||||
|
||||
bool isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist & velocity) override;
|
||||
|
||||
bool getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance) override;
|
||||
|
||||
/**
|
||||
* @brief Set the XY goal tolerance
|
||||
* @param tolerance New tolerance value
|
||||
*/
|
||||
void setXYGoalTolerance(double tolerance);
|
||||
|
||||
protected:
|
||||
double xy_goal_tolerance_;
|
||||
double xy_goal_tolerance_sq_;
|
||||
bool stateful_;
|
||||
bool position_reached_;
|
||||
std::string plugin_name_;
|
||||
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a parameter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_
|
||||
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
|
||||
#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
#include "rcl_interfaces/msg/set_parameters_result.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
/**
|
||||
* @class SimpleGoalChecker
|
||||
* @brief Goal Checker plugin that only checks the position difference
|
||||
*
|
||||
* This class can be stateful if the stateful parameter is set to true (which it is by default).
|
||||
* This means that the goal checker will not check if the xy position matches again once it is found to be true.
|
||||
*/
|
||||
class SimpleGoalChecker : public nav2_core::GoalChecker
|
||||
{
|
||||
public:
|
||||
SimpleGoalChecker();
|
||||
// Standard GoalChecker Interface
|
||||
void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
|
||||
void reset() override;
|
||||
bool isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist & velocity) override;
|
||||
bool getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance) override;
|
||||
|
||||
protected:
|
||||
double xy_goal_tolerance_, yaw_goal_tolerance_;
|
||||
bool stateful_, check_xy_;
|
||||
// Cached squared xy_goal_tolerance_
|
||||
double xy_goal_tolerance_sq_;
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
std::string plugin_name_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
// Copyright (c) 2019 Intel Corporation
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
|
||||
#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_core/progress_checker.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
/**
|
||||
* @class SimpleProgressChecker
|
||||
* @brief This plugin is used to check the position of the robot to make sure
|
||||
* that it is actually progressing towards a goal.
|
||||
*/
|
||||
|
||||
class SimpleProgressChecker : public nav2_core::ProgressChecker
|
||||
{
|
||||
public:
|
||||
void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name) override;
|
||||
bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
|
||||
void reset() override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Calculates robots movement from baseline pose
|
||||
* @param pose Current pose of the robot
|
||||
* @return true, if movement is greater than radius_, or false
|
||||
*/
|
||||
bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
|
||||
/**
|
||||
* @brief Resets baseline pose with the current pose of the robot
|
||||
* @param pose Current pose of the robot
|
||||
*/
|
||||
void resetBaselinePose(const geometry_msgs::msg::Pose2D & pose);
|
||||
|
||||
static double pose_distance(
|
||||
const geometry_msgs::msg::Pose2D &,
|
||||
const geometry_msgs::msg::Pose2D &);
|
||||
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
|
||||
double radius_;
|
||||
rclcpp::Duration time_allowance_{0, 0};
|
||||
|
||||
geometry_msgs::msg::Pose2D baseline_pose_;
|
||||
rclcpp::Time baseline_time_;
|
||||
|
||||
bool baseline_pose_set_{false};
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
std::string plugin_name_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
|
||||
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
|
||||
#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_controller/plugins/simple_goal_checker.hpp"
|
||||
|
||||
namespace nav2_controller
|
||||
{
|
||||
|
||||
/**
|
||||
* @class StoppedGoalChecker
|
||||
* @brief Goal Checker plugin that checks the position difference and velocity
|
||||
*/
|
||||
class StoppedGoalChecker : public SimpleGoalChecker
|
||||
{
|
||||
public:
|
||||
StoppedGoalChecker();
|
||||
// Standard GoalChecker Interface
|
||||
void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
const std::string & plugin_name,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
|
||||
bool isGoalReached(
|
||||
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
|
||||
const geometry_msgs::msg::Twist & velocity) override;
|
||||
bool getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & vel_tolerance) override;
|
||||
|
||||
protected:
|
||||
double rot_stopped_velocity_, trans_stopped_velocity_;
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
std::string plugin_name_;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
};
|
||||
|
||||
} // namespace nav2_controller
|
||||
|
||||
#endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
|
||||
Reference in New Issue
Block a user