diff --git a/nav2_graceful_motion_controller/CMakeLists.txt b/nav2_graceful_controller/CMakeLists.txt similarity index 91% rename from nav2_graceful_motion_controller/CMakeLists.txt rename to nav2_graceful_controller/CMakeLists.txt index 3625eeba5d..11a6b89e0b 100644 --- a/nav2_graceful_motion_controller/CMakeLists.txt +++ b/nav2_graceful_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_graceful_motion_controller) +project(nav2_graceful_controller) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) @@ -39,11 +39,11 @@ set(dependencies add_library(smooth_control_law SHARED src/smooth_control_law.cpp) ament_target_dependencies(smooth_control_law ${dependencies}) -# Add Graceful Motion Controller -set(library_name nav2_graceful_motion_controller) +# Add Graceful Controller +set(library_name nav2_graceful_controller) add_library(${library_name} SHARED - src/graceful_motion_controller.cpp + src/graceful_controller.cpp src/parameter_handler.cpp src/path_handler.cpp src/utils.cpp diff --git a/nav2_graceful_motion_controller/README.md b/nav2_graceful_controller/README.md similarity index 100% rename from nav2_graceful_motion_controller/README.md rename to nav2_graceful_controller/README.md diff --git a/nav2_graceful_motion_controller/doc/trajectories.png b/nav2_graceful_controller/doc/trajectories.png similarity index 100% rename from nav2_graceful_motion_controller/doc/trajectories.png rename to nav2_graceful_controller/doc/trajectories.png diff --git a/nav2_graceful_controller/graceful_controller_plugin.xml b/nav2_graceful_controller/graceful_controller_plugin.xml new file mode 100644 index 0000000000..7a4a843223 --- /dev/null +++ b/nav2_graceful_controller/graceful_controller_plugin.xml @@ -0,0 +1,7 @@ + + + + Graceful controller for Nav2 + + + diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp similarity index 89% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp index 97fb2a9956..c9123c510f 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp @@ -1,91 +1,91 @@ -// Copyright (c) 2023 Alberto J. Tudela Roldán -// -// 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_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ - -#include - -#include "angles/angles.h" -#include "geometry_msgs/msg/pose.hpp" -#include "tf2/utils.h" -#include "tf2/transform_datatypes.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -namespace nav2_graceful_motion_controller -{ - -/** - * @brief Egocentric polar coordinates defined as the difference between the - * robot pose and the target pose relative to the robot position and orientation. - */ -struct EgocentricPolarCoordinates -{ - float r; // Radial distance between the robot pose and the target pose. - // Negative value if the robot is moving backwards. - float phi; // Orientation of target with respect to the line of sight - // from the robot to the target. - float delta; // Steering angle of the robot with respect to the line of sight. - - EgocentricPolarCoordinates( - const float & r_in = 0.0, - const float & phi_in = 0.0, - const float & delta_in = 0.0) - : r(r_in), phi(phi_in), delta(delta_in) {} - - /** - * @brief Construct a new egocentric polar coordinates as the difference between the robot pose - * and the target pose relative to the robot position and orientation, both referenced to the same frame. - * - * Thus, r, phi and delta are always at the origin of the frame. - * - * @param target Target pose. - * @param current Current pose. Defaults to the origin. - * @param backward If true, the robot is moving backwards. Defaults to false. - */ - explicit EgocentricPolarCoordinates( - const geometry_msgs::msg::Pose & target, - const geometry_msgs::msg::Pose & current = geometry_msgs::msg::Pose(), bool backward = false) - { - // Compute the difference between the target and the current pose - float dX = target.position.x - current.position.x; - float dY = target.position.y - current.position.y; - // Compute the line of sight from the robot to the target - // Flip it if the robot is moving backwards - float line_of_sight = backward ? (std::atan2(-dY, dX) + M_PI) : std::atan2(-dY, dX); - // Compute the ego polar coordinates - r = sqrt(dX * dX + dY * dY); - phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight); - delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight); - // If the robot is moving backwards, flip the sign of the radial distance - r *= backward ? -1.0 : 1.0; - } - - /** - * @brief Construct a new egocentric polar coordinates for the target pose. - * - * @param target Target pose. - * @param backward If true, the robot is moving backwards. Defaults to false. - */ - explicit EgocentricPolarCoordinates( - const geometry_msgs::msg::Pose & target, - bool backward = false) - { - EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward); - } -}; - -} // namespace nav2_graceful_motion_controller - -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_CONTROLLER__EGO_POLAR_COORDS_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__EGO_POLAR_COORDS_HPP_ + +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.h" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +namespace nav2_graceful_controller +{ + +/** + * @brief Egocentric polar coordinates defined as the difference between the + * robot pose and the target pose relative to the robot position and orientation. + */ +struct EgocentricPolarCoordinates +{ + float r; // Radial distance between the robot pose and the target pose. + // Negative value if the robot is moving backwards. + float phi; // Orientation of target with respect to the line of sight + // from the robot to the target. + float delta; // Steering angle of the robot with respect to the line of sight. + + EgocentricPolarCoordinates( + const float & r_in = 0.0, + const float & phi_in = 0.0, + const float & delta_in = 0.0) + : r(r_in), phi(phi_in), delta(delta_in) {} + + /** + * @brief Construct a new egocentric polar coordinates as the difference between the robot pose + * and the target pose relative to the robot position and orientation, both referenced to the same frame. + * + * Thus, r, phi and delta are always at the origin of the frame. + * + * @param target Target pose. + * @param current Current pose. Defaults to the origin. + * @param backward If true, the robot is moving backwards. Defaults to false. + */ + explicit EgocentricPolarCoordinates( + const geometry_msgs::msg::Pose & target, + const geometry_msgs::msg::Pose & current = geometry_msgs::msg::Pose(), bool backward = false) + { + // Compute the difference between the target and the current pose + float dX = target.position.x - current.position.x; + float dY = target.position.y - current.position.y; + // Compute the line of sight from the robot to the target + // Flip it if the robot is moving backwards + float line_of_sight = backward ? (std::atan2(-dY, dX) + M_PI) : std::atan2(-dY, dX); + // Compute the ego polar coordinates + r = sqrt(dX * dX + dY * dY); + phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight); + delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight); + // If the robot is moving backwards, flip the sign of the radial distance + r *= backward ? -1.0 : 1.0; + } + + /** + * @brief Construct a new egocentric polar coordinates for the target pose. + * + * @param target Target pose. + * @param backward If true, the robot is moving backwards. Defaults to false. + */ + explicit EgocentricPolarCoordinates( + const geometry_msgs::msg::Pose & target, + bool backward = false) + { + EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward); + } +}; + +} // namespace nav2_graceful_controller + +#endif // NAV2_GRACEFUL_CONTROLLER__EGO_POLAR_COORDS_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp similarity index 80% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp index 7a593c0363..1a77a3adfc 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/graceful_motion_controller.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ +#ifndef NAV2_GRACEFUL_CONTROLLER__GRACEFUL_CONTROLLER_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__GRACEFUL_CONTROLLER_HPP_ #include #include @@ -27,30 +27,30 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_graceful_motion_controller/path_handler.hpp" -#include "nav2_graceful_motion_controller/parameter_handler.hpp" -#include "nav2_graceful_motion_controller/smooth_control_law.hpp" -#include "nav2_graceful_motion_controller/utils.hpp" +#include "nav2_graceful_controller/path_handler.hpp" +#include "nav2_graceful_controller/parameter_handler.hpp" +#include "nav2_graceful_controller/smooth_control_law.hpp" +#include "nav2_graceful_controller/utils.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { /** - * @class nav2_graceful_motion_controller::GracefulMotionController + * @class nav2_graceful_controller::GracefulController * @brief Graceful controller plugin */ -class GracefulMotionController : public nav2_core::Controller +class GracefulController : public nav2_core::Controller { public: /** - * @brief Constructor for nav2_graceful_motion_controller::GracefulMotionController + * @brief Constructor for nav2_graceful_controller::GracefulController */ - GracefulMotionController() = default; + GracefulController() = default; /** - * @brief Destructor for nav2_graceful_motion_controller::GracefulMotionController + * @brief Destructor for nav2_graceful_controller::GracefulController */ - ~GracefulMotionController() override = default; + ~GracefulController() override = default; /** * @brief Configure controller state machine @@ -158,7 +158,7 @@ class GracefulMotionController : public nav2_core::Controller std::shared_ptr costmap_ros_; std::unique_ptr> collision_checker_; - rclcpp::Logger logger_{rclcpp::get_logger("GracefulMotionController")}; + rclcpp::Logger logger_{rclcpp::get_logger("GracefulController")}; Parameters * params_; double goal_dist_tolerance_; @@ -170,11 +170,11 @@ class GracefulMotionController : public nav2_core::Controller motion_target_pub_; std::shared_ptr> slowdown_pub_; - std::unique_ptr path_handler_; - std::unique_ptr param_handler_; - std::unique_ptr control_law_; + std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + std::unique_ptr control_law_; }; -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__GRACEFUL_MOTION_CONTROLLER_HPP_ +#endif // NAV2_GRACEFUL_CONTROLLER__GRACEFUL_CONTROLLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp similarity index 79% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index ef2aebfb99..74d56d48f6 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -1,97 +1,97 @@ -// Copyright (c) 2023 Alberto J. Tudela Roldán -// -// 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_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ - -#include -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_util/node_utils.hpp" - -namespace nav2_graceful_motion_controller -{ - -struct Parameters -{ - double transform_tolerance; - double motion_target_dist; - double max_robot_pose_search_dist; - double k_phi; - double k_delta; - double beta; - double lambda; - double v_linear_min; - double v_linear_max; - double v_linear_max_initial; - double v_angular_max; - double v_angular_max_initial; - double slowdown_radius; - bool initial_rotation; - double initial_rotation_min_angle; - bool final_rotation; - double rotation_scaling_factor; - bool allow_backward; -}; - -/** - * @class nav2_graceful_motion_controller::ParameterHandler - * @brief Handles parameters and dynamic parameters for GracefulMotionController - */ -class ParameterHandler -{ -public: - /** - * @brief Constructor for nav2_graceful_motion_controller::ParameterHandler - */ - ParameterHandler( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x); - - /** - * @brief Destructor for nav2_graceful_motion_controller::ParameterHandler - */ - ~ParameterHandler() = default; - - std::mutex & getMutex() {return mutex_;} - - Parameters * getParams() {return ¶ms_;} - -protected: - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - Parameters params_; - std::string plugin_name_; - rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; -}; - -} // namespace nav2_graceful_motion_controller - -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__PARAMETER_HANDLER_HPP_ +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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_GRACEFUL_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_graceful_controller +{ + +struct Parameters +{ + double transform_tolerance; + double motion_target_dist; + double max_robot_pose_search_dist; + double k_phi; + double k_delta; + double beta; + double lambda; + double v_linear_min; + double v_linear_max; + double v_linear_max_initial; + double v_angular_max; + double v_angular_max_initial; + double slowdown_radius; + bool initial_rotation; + double initial_rotation_min_angle; + bool final_rotation; + double rotation_scaling_factor; + bool allow_backward; +}; + +/** + * @class nav2_graceful_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for GracefulMotionController + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_graceful_controller::ParameterHandler + */ + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destructor for nav2_graceful_controller::ParameterHandler + */ + ~ParameterHandler() = default; + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("GracefulMotionController")}; +}; + +} // namespace nav2_graceful_controller + +#endif // NAV2_GRACEFUL_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp similarity index 79% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp index 4ca986c39e..aaa64f1bb6 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/path_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp @@ -1,83 +1,83 @@ -// Copyright (c) 2022 Samsung Research America -// -// 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_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ - -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_util/geometry_utils.hpp" - -namespace nav2_graceful_motion_controller -{ - -/** - * @class nav2_graceful_motion_controller::PathHandler - * @brief Handles input paths to transform them to local frames required - */ -class PathHandler -{ -public: - /** - * @brief Constructor for nav2_graceful_motion_controller::PathHandler - */ - PathHandler( - tf2::Duration transform_tolerance, - std::shared_ptr tf, - std::shared_ptr costmap_ros); - - /** - * @brief Destructor for nav2_graceful_motion_controller::PathHandler - */ - ~PathHandler() = default; - - /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for motionTarget - * Points ineligible to be selected as a motion target point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) - * @param pose pose to transform - * @param max_robot_pose_search_dist Distance to search for matching nearest path point - * @return Path in new frame - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist); - - /** - * @brief Sets the global plan - * - * @param path The global plan - */ - void setPlan(const nav_msgs::msg::Path & path); - - /** - * @brief Gets the global plan - * - * @return The global plan - */ - nav_msgs::msg::Path getPlan() {return global_plan_;} - -protected: - rclcpp::Duration transform_tolerance_{0, 0}; - std::shared_ptr tf_buffer_; - std::shared_ptr costmap_ros_; - nav_msgs::msg::Path global_plan_; - rclcpp::Logger logger_ {rclcpp::get_logger("GracefulPathHandler")}; -}; - -} // namespace nav2_graceful_motion_controller - -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__PATH_HANDLER_HPP_ +// Copyright (c) 2022 Samsung Research America +// +// 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_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_graceful_controller +{ + +/** + * @class nav2_graceful_controller::PathHandler + * @brief Handles input paths to transform them to local frames required + */ +class PathHandler +{ +public: + /** + * @brief Constructor for nav2_graceful_controller::PathHandler + */ + PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + /** + * @brief Destructor for nav2_graceful_controller::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for motionTarget + * Points ineligible to be selected as a motion target point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist); + + /** + * @brief Sets the global plan + * + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path); + + /** + * @brief Gets the global plan + * + * @return The global plan + */ + nav_msgs::msg::Path getPlan() {return global_plan_;} + +protected: + rclcpp::Duration transform_tolerance_{0, 0}; + std::shared_ptr tf_buffer_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; + rclcpp::Logger logger_ {rclcpp::get_logger("GracefulPathHandler")}; +}; + +} // namespace nav2_graceful_controller + +#endif // NAV2_GRACEFUL_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp similarity index 92% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp index 276610ea6c..ab68f75f68 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/smooth_control_law.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ +#ifndef NAV2_GRACEFUL_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ #include #include @@ -21,11 +21,11 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { /** - * @class nav2_graceful_motion_controller::SmoothControlLaw + * @class nav2_graceful_controller::SmoothControlLaw * @brief Smooth control law for graceful motion based on "A smooth control law for graceful motion" * (Jong Jin Park and Benjamin Kuipers). */ @@ -33,7 +33,7 @@ class SmoothControlLaw { public: /** - * @brief Constructor for nav2_graceful_motion_controller::SmoothControlLaw + * @brief Constructor for nav2_graceful_controller::SmoothControlLaw * * @param k_phi Ratio of the rate of change in phi to the rate of change in r. * @param k_delta Constant factor applied to the heading error feedback. @@ -49,7 +49,7 @@ class SmoothControlLaw double v_linear_min, double v_linear_max, double v_angular_max); /** - * @brief Destructor for nav2_graceful_motion_controller::SmoothControlLaw + * @brief Destructor for nav2_graceful_controller::SmoothControlLaw */ ~SmoothControlLaw() = default; @@ -188,6 +188,6 @@ class SmoothControlLaw double v_angular_max_; }; -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ +#endif // NAV2_GRACEFUL_CONTROLLER__SMOOTH_CONTROL_LAW_HPP_ diff --git a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp similarity index 86% rename from nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp rename to nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp index ba0868ac20..788656bd64 100644 --- a/nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/utils.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ -#define NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ +#ifndef NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_ +#define NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_ #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { /** * @brief Create a PointStamped message of the motion target for @@ -42,6 +42,6 @@ geometry_msgs::msg::PointStamped createMotionTargetMsg( visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius); -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller -#endif // NAV2_GRACEFUL_MOTION_CONTROLLER__UTILS_HPP_ +#endif // NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_ diff --git a/nav2_graceful_motion_controller/package.xml b/nav2_graceful_controller/package.xml similarity index 95% rename from nav2_graceful_motion_controller/package.xml rename to nav2_graceful_controller/package.xml index 17565ad784..559b4c43c4 100644 --- a/nav2_graceful_motion_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -1,7 +1,7 @@ - nav2_graceful_motion_controller + nav2_graceful_controller 1.2.0 Graceful motion controller Alberto Tudela diff --git a/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp similarity index 90% rename from nav2_graceful_motion_controller/src/graceful_motion_controller.cpp rename to nav2_graceful_controller/src/graceful_controller.cpp index 44d17d1c1d..f8610a55c9 100644 --- a/nav2_graceful_motion_controller/src/graceful_motion_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -14,13 +14,13 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_graceful_motion_controller/graceful_motion_controller.hpp" +#include "nav2_graceful_controller/graceful_controller.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { -void GracefulMotionController::configure( +void GracefulController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr tf, const std::shared_ptr costmap_ros) @@ -64,11 +64,11 @@ void GracefulMotionController::configure( RCLCPP_INFO(logger_, "Configured Graceful Motion Controller: %s", plugin_name_.c_str()); } -void GracefulMotionController::cleanup() +void GracefulController::cleanup() { RCLCPP_INFO( logger_, - "Cleaning up controller: %s of type graceful_motion_controller::GracefulMotionController", + "Cleaning up controller: %s of type graceful_controller::GracefulController", plugin_name_.c_str()); transformed_plan_pub_.reset(); local_plan_pub_.reset(); @@ -80,11 +80,11 @@ void GracefulMotionController::cleanup() control_law_.reset(); } -void GracefulMotionController::activate() +void GracefulController::activate() { RCLCPP_INFO( logger_, - "Activating controller: %s of type graceful_motion_controller::GracefulMotionController", + "Activating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); transformed_plan_pub_->on_activate(); local_plan_pub_->on_activate(); @@ -92,11 +92,11 @@ void GracefulMotionController::activate() slowdown_pub_->on_activate(); } -void GracefulMotionController::deactivate() +void GracefulController::deactivate() { RCLCPP_INFO( logger_, - "Deactivating controller: %s of type graceful_motion_controller::GracefulMotionController", + "Deactivating controller: %s of type nav2_graceful_controller::GracefulController", plugin_name_.c_str()); transformed_plan_pub_->on_deactivate(); local_plan_pub_->on_deactivate(); @@ -104,7 +104,7 @@ void GracefulMotionController::deactivate() slowdown_pub_->on_deactivate(); } -geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityCommands( +geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/, nav2_core::GoalChecker * goal_checker) @@ -133,11 +133,11 @@ geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityComman // Get the particular point on the path at the motion target distance and publish it auto motion_target = getMotionTarget(params_->motion_target_dist, transformed_plan); - auto motion_target_point = nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); + auto motion_target_point = nav2_graceful_controller::createMotionTargetMsg(motion_target); motion_target_pub_->publish(motion_target_point); // Publish marker for slowdown radius around motion target for debugging / visualization - auto slowdown_marker = nav2_graceful_motion_controller::createSlowdownMarker( + auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker( motion_target, params_->slowdown_radius); slowdown_pub_->publish(slowdown_marker); @@ -210,13 +210,13 @@ geometry_msgs::msg::TwistStamped GracefulMotionController::computeVelocityComman return cmd_vel; } -void GracefulMotionController::setPlan(const nav_msgs::msg::Path & path) +void GracefulController::setPlan(const nav_msgs::msg::Path & path) { path_handler_->setPlan(path); goal_reached_ = false; } -void GracefulMotionController::setSpeedLimit( +void GracefulController::setSpeedLimit( const double & speed_limit, const bool & percentage) { std::lock_guard param_lock(param_handler_->getMutex()); @@ -240,7 +240,7 @@ void GracefulMotionController::setSpeedLimit( } } -geometry_msgs::msg::PoseStamped GracefulMotionController::getMotionTarget( +geometry_msgs::msg::PoseStamped GracefulController::getMotionTarget( const double & motion_target_dist, const nav_msgs::msg::Path & transformed_plan) { @@ -258,7 +258,7 @@ geometry_msgs::msg::PoseStamped GracefulMotionController::getMotionTarget( return *goal_pose_it; } -bool GracefulMotionController::simulateTrajectory( +bool GracefulController::simulateTrajectory( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & motion_target, const geometry_msgs::msg::TransformStamped & costmap_transform, @@ -313,7 +313,7 @@ bool GracefulMotionController::simulateTrajectory( return true; } -geometry_msgs::msg::Twist GracefulMotionController::rotateToTarget(const double & angle_to_target) +geometry_msgs::msg::Twist GracefulController::rotateToTarget(const double & angle_to_target) { geometry_msgs::msg::Twist vel; vel.linear.x = 0.0; @@ -321,7 +321,7 @@ geometry_msgs::msg::Twist GracefulMotionController::rotateToTarget(const double return vel; } -bool GracefulMotionController::inCollision(const double & x, const double & y, const double & theta) +bool GracefulController::inCollision(const double & x, const double & y, const double & theta) { unsigned int mx, my; if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) { @@ -357,9 +357,9 @@ bool GracefulMotionController::inCollision(const double & x, const double & y, c return false; } -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller // Register this controller as a nav2_core plugin PLUGINLIB_EXPORT_CLASS( - nav2_graceful_motion_controller::GracefulMotionController, + nav2_graceful_controller::GracefulController, nav2_core::Controller) diff --git a/nav2_graceful_motion_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp similarity index 96% rename from nav2_graceful_motion_controller/src/parameter_handler.cpp rename to nav2_graceful_controller/src/parameter_handler.cpp index ae3bbdc1cc..61bfc7a9df 100644 --- a/nav2_graceful_motion_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -1,171 +1,171 @@ -// Copyright (c) 2023 Alberto J. Tudela Roldán -// -// 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. - -#include -#include -#include -#include -#include -#include - -#include "nav2_graceful_motion_controller/parameter_handler.hpp" - -namespace nav2_graceful_motion_controller -{ - -using nav2_util::declare_parameter_if_not_declared; -using rcl_interfaces::msg::ParameterType; - -ParameterHandler::ParameterHandler( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, - rclcpp::Logger & logger, const double costmap_size_x) -{ - plugin_name_ = plugin_name; - logger_ = logger; - - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(costmap_size_x / 2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2)); - declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); - - node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); - node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); - if (params_.max_robot_pose_search_dist < 0.0) { - RCLCPP_WARN( - logger_, "Max robot search distance is negative, setting to max to search" - " every point on path for the closest value."); - params_.max_robot_pose_search_dist = std::numeric_limits::max(); - } - - node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi); - node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta); - node->get_parameter(plugin_name_ + ".beta", params_.beta); - node->get_parameter(plugin_name_ + ".lambda", params_.lambda); - node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min); - node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max); - params_.v_linear_max_initial = params_.v_linear_max; - node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); - params_.v_angular_max_initial = params_.v_angular_max; - node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); - node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); - node->get_parameter( - plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle); - node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation); - node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor); - node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); - - if (params_.initial_rotation && params_.allow_backward) { - RCLCPP_WARN( - logger_, "Initial rotation and allow backward parameters are both true, " - "setting allow backward to false."); - params_.allow_backward = false; - } - - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1)); -} - -rcl_interfaces::msg::SetParametersResult -ParameterHandler::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - std::lock_guard lock_reinit(mutex_); - - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".transform_tolerance") { - params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".motion_target_dist") { - params_.motion_target_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".k_phi") { - params_.k_phi = parameter.as_double(); - } else if (name == plugin_name_ + ".k_delta") { - params_.k_delta = parameter.as_double(); - } else if (name == plugin_name_ + ".beta") { - params_.beta = parameter.as_double(); - } else if (name == plugin_name_ + ".lambda") { - params_.lambda = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_min") { - params_.v_linear_min = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_max") { - params_.v_linear_max = parameter.as_double(); - params_.v_linear_max_initial = params_.v_linear_max; - } else if (name == plugin_name_ + ".v_angular_max") { - params_.v_angular_max = parameter.as_double(); - params_.v_angular_max_initial = params_.v_angular_max; - } else if (name == plugin_name_ + ".slowdown_radius") { - params_.slowdown_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".initial_rotation_min_angle") { - params_.initial_rotation_min_angle = parameter.as_double(); - } else if (name == plugin_name_ + ".rotation_scaling_factor") { - params_.rotation_scaling_factor = parameter.as_double(); - } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".initial_rotation") { - if (parameter.as_bool() && params_.allow_backward) { - RCLCPP_WARN( - logger_, "Initial rotation and allow backward parameters are both true, " - "rejecting parameter change."); - continue; - } - params_.initial_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".final_rotation") { - params_.final_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_backward") { - if (params_.initial_rotation && parameter.as_bool()) { - RCLCPP_WARN( - logger_, "Initial rotation and allow backward parameters are both true, " - "rejecting parameter change."); - continue; - } - params_.allow_backward = parameter.as_bool(); - } - } - } - - result.successful = true; - return result; -} - -} // namespace nav2_graceful_motion_controller +// Copyright (c) 2023 Alberto J. Tudela Roldán +// +// 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. + +#include +#include +#include +#include +#include +#include + +#include "nav2_graceful_controller/parameter_handler.hpp" + +namespace nav2_graceful_controller +{ + +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x) +{ + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2)); + declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); + + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist); + if (params_.max_robot_pose_search_dist < 0.0) { + RCLCPP_WARN( + logger_, "Max robot search distance is negative, setting to max to search" + " every point on path for the closest value."); + params_.max_robot_pose_search_dist = std::numeric_limits::max(); + } + + node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi); + node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta); + node->get_parameter(plugin_name_ + ".beta", params_.beta); + node->get_parameter(plugin_name_ + ".lambda", params_.lambda); + node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min); + node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max); + params_.v_linear_max_initial = params_.v_linear_max; + node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max); + params_.v_angular_max_initial = params_.v_angular_max; + node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius); + node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation); + node->get_parameter( + plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle); + node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation); + node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor); + node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); + + if (params_.initial_rotation && params_.allow_backward) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "setting allow backward to false."); + params_.allow_backward = false; + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".transform_tolerance") { + params_.transform_tolerance = parameter.as_double(); + } else if (name == plugin_name_ + ".motion_target_dist") { + params_.motion_target_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".k_phi") { + params_.k_phi = parameter.as_double(); + } else if (name == plugin_name_ + ".k_delta") { + params_.k_delta = parameter.as_double(); + } else if (name == plugin_name_ + ".beta") { + params_.beta = parameter.as_double(); + } else if (name == plugin_name_ + ".lambda") { + params_.lambda = parameter.as_double(); + } else if (name == plugin_name_ + ".v_linear_min") { + params_.v_linear_min = parameter.as_double(); + } else if (name == plugin_name_ + ".v_linear_max") { + params_.v_linear_max = parameter.as_double(); + params_.v_linear_max_initial = params_.v_linear_max; + } else if (name == plugin_name_ + ".v_angular_max") { + params_.v_angular_max = parameter.as_double(); + params_.v_angular_max_initial = params_.v_angular_max; + } else if (name == plugin_name_ + ".slowdown_radius") { + params_.slowdown_radius = parameter.as_double(); + } else if (name == plugin_name_ + ".initial_rotation_min_angle") { + params_.initial_rotation_min_angle = parameter.as_double(); + } else if (name == plugin_name_ + ".rotation_scaling_factor") { + params_.rotation_scaling_factor = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".initial_rotation") { + if (parameter.as_bool() && params_.allow_backward) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "rejecting parameter change."); + continue; + } + params_.initial_rotation = parameter.as_bool(); + } else if (name == plugin_name_ + ".final_rotation") { + params_.final_rotation = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_backward") { + if (params_.initial_rotation && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Initial rotation and allow backward parameters are both true, " + "rejecting parameter change."); + continue; + } + params_.allow_backward = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_graceful_controller diff --git a/nav2_graceful_motion_controller/src/path_handler.cpp b/nav2_graceful_controller/src/path_handler.cpp similarity index 94% rename from nav2_graceful_motion_controller/src/path_handler.cpp rename to nav2_graceful_controller/src/path_handler.cpp index a2b8966224..b60df60436 100644 --- a/nav2_graceful_motion_controller/src/path_handler.cpp +++ b/nav2_graceful_controller/src/path_handler.cpp @@ -1,126 +1,126 @@ -// Copyright (c) 2022 Samsung Research America -// -// 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. - -#include -#include -#include -#include -#include -#include - -#include "nav2_core/controller_exceptions.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav_2d_utils/tf_help.hpp" -#include "nav2_graceful_motion_controller/path_handler.hpp" - -namespace nav2_graceful_motion_controller -{ - -using nav2_util::geometry_utils::euclidean_distance; - -PathHandler::PathHandler( - tf2::Duration transform_tolerance, - std::shared_ptr tf, - std::shared_ptr costmap_ros) -: transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) -{ -} - -nav_msgs::msg::Path PathHandler::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist) -{ - // Check first if the plan is empty - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // Let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, global_plan_.header.frame_id, pose, robot_pose, - transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // Find the first pose in the global plan that's further than max_robot_pose_search_dist - // from the robot using integrated distance - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto transformation_begin = - nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // We'll discard points on the plan that are outside the local costmap - double dist_threshold = std::max( - costmap_ros_->getCostmap()->getSizeInMetersX(), - costmap_ros_->getCostmap()->getSizeInMetersY()) / 2.0; - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & global_plan_pose) { - return euclidean_distance(global_plan_pose, robot_pose) > dist_threshold; - }); - - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!nav_2d_utils::transformPose( - tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose, - transformed_pose, transform_tolerance_)) - { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - -void PathHandler::setPlan(const nav_msgs::msg::Path & path) -{ - global_plan_ = path; -} - -} // namespace nav2_graceful_motion_controller +// Copyright (c) 2022 Samsung Research America +// +// 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. + +#include +#include +#include +#include +#include +#include + +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_2d_utils/tf_help.hpp" +#include "nav2_graceful_controller/path_handler.hpp" + +namespace nav2_graceful_controller +{ + +using nav2_util::geometry_utils::euclidean_distance; + +PathHandler::PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros) +: transform_tolerance_(transform_tolerance), tf_buffer_(tf), costmap_ros_(costmap_ros) +{ +} + +nav_msgs::msg::Path PathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist) +{ + // Check first if the plan is empty + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + // Let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!nav_2d_utils::transformPose( + tf_buffer_, global_plan_.header.frame_id, pose, robot_pose, + transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + // Find the first pose in the global plan that's further than max_robot_pose_search_dist + // from the robot using integrated distance + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // We'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()) / 2.0; + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & global_plan_pose) { + return euclidean_distance(global_plan_pose, robot_pose) > dist_threshold; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!nav_2d_utils::transformPose( + tf_buffer_, costmap_ros_->getBaseFrameID(), stamped_pose, + transformed_pose, transform_tolerance_)) + { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +void PathHandler::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +} // namespace nav2_graceful_controller diff --git a/nav2_graceful_motion_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp similarity index 95% rename from nav2_graceful_motion_controller/src/smooth_control_law.cpp rename to nav2_graceful_controller/src/smooth_control_law.cpp index ee5a38ea87..545a7dd6a8 100644 --- a/nav2_graceful_motion_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_controller/src/smooth_control_law.cpp @@ -14,10 +14,10 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" -#include "nav2_graceful_motion_controller/smooth_control_law.hpp" +#include "nav2_graceful_controller/ego_polar_coords.hpp" +#include "nav2_graceful_controller/smooth_control_law.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { SmoothControlLaw::SmoothControlLaw( @@ -121,4 +121,4 @@ double SmoothControlLaw::calculateCurvature(double r, double phi, double delta) return -1.0 / r * (prop_term + feedback_term); } -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller diff --git a/nav2_graceful_motion_controller/src/utils.cpp b/nav2_graceful_controller/src/utils.cpp similarity index 92% rename from nav2_graceful_motion_controller/src/utils.cpp rename to nav2_graceful_controller/src/utils.cpp index 488b679022..e6cdbadd56 100644 --- a/nav2_graceful_motion_controller/src/utils.cpp +++ b/nav2_graceful_controller/src/utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_graceful_motion_controller/utils.hpp" +#include "nav2_graceful_controller/utils.hpp" -namespace nav2_graceful_motion_controller +namespace nav2_graceful_controller { geometry_msgs::msg::PointStamped createMotionTargetMsg( @@ -48,4 +48,4 @@ visualization_msgs::msg::Marker createSlowdownMarker( return slowdown_marker; } -} // namespace nav2_graceful_motion_controller +} // namespace nav2_graceful_controller diff --git a/nav2_graceful_motion_controller/test/CMakeLists.txt b/nav2_graceful_controller/test/CMakeLists.txt similarity index 51% rename from nav2_graceful_motion_controller/test/CMakeLists.txt rename to nav2_graceful_controller/test/CMakeLists.txt index 543c9b7290..1becbba0b1 100644 --- a/nav2_graceful_motion_controller/test/CMakeLists.txt +++ b/nav2_graceful_controller/test/CMakeLists.txt @@ -1,14 +1,14 @@ find_package(nav2_controller REQUIRED) -# Tests for Graceful Motion Controller -ament_add_gtest(test_graceful_motion_controller - test_graceful_motion_controller.cpp +# Tests for Graceful Controller +ament_add_gtest(test_graceful_controller + test_graceful_controller.cpp ) -ament_target_dependencies(test_graceful_motion_controller +ament_target_dependencies(test_graceful_controller ${dependencies} nav2_controller ) -target_link_libraries(test_graceful_motion_controller +target_link_libraries(test_graceful_controller ${library_name} ) diff --git a/nav2_graceful_motion_controller/test/test_egopolar.cpp b/nav2_graceful_controller/test/test_egopolar.cpp similarity index 85% rename from nav2_graceful_motion_controller/test/test_egopolar.cpp rename to nav2_graceful_controller/test/test_egopolar.cpp index 0b1bd959cf..2bd3c4c244 100644 --- a/nav2_graceful_motion_controller/test/test_egopolar.cpp +++ b/nav2_graceful_controller/test/test_egopolar.cpp @@ -16,10 +16,10 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" +#include "nav2_graceful_controller/ego_polar_coords.hpp" TEST(EgocentricPolarCoordinatesTest, constructorDefault) { - nav2_graceful_motion_controller::EgocentricPolarCoordinates coords; + nav2_graceful_controller::EgocentricPolarCoordinates coords; EXPECT_FLOAT_EQ(0.0, coords.r); EXPECT_FLOAT_EQ(0.0, coords.phi); @@ -31,7 +31,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorWithValues) { float phi_value = 1.2; float delta_value = -0.5; - nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(r_value, phi_value, + nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value); EXPECT_FLOAT_EQ(r_value, coords.r); @@ -50,7 +50,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) { current.position.y = 1.0; current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2)); - nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(target, current); + nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current); EXPECT_FLOAT_EQ(3.6055512428283691, coords.r); EXPECT_FLOAT_EQ(-0.18279374837875384, coords.phi); @@ -68,7 +68,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) { current.position.y = 1.0; current.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -0.2)); - nav2_graceful_motion_controller::EgocentricPolarCoordinates coords(target, current, true); + nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current, true); EXPECT_FLOAT_EQ(-6.4031243, coords.r); EXPECT_FLOAT_EQ(-0.096055523, coords.phi); diff --git a/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp similarity index 95% rename from nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp rename to nav2_graceful_controller/test/test_graceful_controller.cpp index da6f82fe36..26e929a580 100644 --- a/nav2_graceful_motion_controller/test/test_graceful_motion_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -18,17 +18,17 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "nav2_graceful_motion_controller/ego_polar_coords.hpp" -#include "nav2_graceful_motion_controller/smooth_control_law.hpp" -#include "nav2_graceful_motion_controller/graceful_motion_controller.hpp" +#include "nav2_graceful_controller/ego_polar_coords.hpp" +#include "nav2_graceful_controller/smooth_control_law.hpp" +#include "nav2_graceful_controller/graceful_controller.hpp" -class SCLFixture : public nav2_graceful_motion_controller::SmoothControlLaw +class SCLFixture : public nav2_graceful_controller::SmoothControlLaw { public: SCLFixture( double k_phi, double k_delta, double beta, double lambda, double slowdown_radius, double v_linear_min, double v_linear_max, double v_angular_max) - : nav2_graceful_motion_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda, + : nav2_graceful_controller::SmoothControlLaw(k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max) {} double getCurvatureKPhi() {return k_phi_;} @@ -41,17 +41,17 @@ class SCLFixture : public nav2_graceful_motion_controller::SmoothControlLaw double getSpeedAngularMax() {return v_angular_max_;} double calculateCurvature(geometry_msgs::msg::Pose target, geometry_msgs::msg::Pose current) { - auto ego_coords = nav2_graceful_motion_controller::EgocentricPolarCoordinates(target, current); - return nav2_graceful_motion_controller::SmoothControlLaw::calculateCurvature( + auto ego_coords = nav2_graceful_controller::EgocentricPolarCoordinates(target, current); + return nav2_graceful_controller::SmoothControlLaw::calculateCurvature( ego_coords.r, ego_coords.phi, ego_coords.delta); } }; -class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMotionController +class GMControllerFixture : public nav2_graceful_controller::GracefulController { public: GMControllerFixture() - : nav2_graceful_motion_controller::GracefulMotionController() {} + : nav2_graceful_controller::GracefulController() {} bool getInitialRotation() {return params_->initial_rotation;} @@ -62,27 +62,27 @@ class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMoti geometry_msgs::msg::PoseStamped getMotionTarget( const double & motion_target_distance, const nav_msgs::msg::Path & plan) { - return nav2_graceful_motion_controller::GracefulMotionController::getMotionTarget( + return nav2_graceful_controller::GracefulController::getMotionTarget( motion_target_distance, plan); } geometry_msgs::msg::PointStamped createMotionTargetMsg( const geometry_msgs::msg::PoseStamped & motion_target) { - return nav2_graceful_motion_controller::createMotionTargetMsg(motion_target); + return nav2_graceful_controller::createMotionTargetMsg(motion_target); } visualization_msgs::msg::Marker createSlowdownMarker( const geometry_msgs::msg::PoseStamped & motion_target) { - return nav2_graceful_motion_controller::createSlowdownMarker( + return nav2_graceful_controller::createSlowdownMarker( motion_target, params_->slowdown_radius); } geometry_msgs::msg::Twist rotateToTarget(const double & angle_to_target) { - return nav2_graceful_motion_controller::GracefulMotionController::rotateToTarget( + return nav2_graceful_controller::GracefulController::rotateToTarget( angle_to_target); } @@ -92,7 +92,7 @@ class GMControllerFixture : public nav2_graceful_motion_controller::GracefulMoti const geometry_msgs::msg::TransformStamped & costmap_transform, nav_msgs::msg::Path & trajectory, const bool & backward) { - return nav2_graceful_motion_controller::GracefulMotionController::simulateTrajectory( + return nav2_graceful_controller::GracefulController::simulateTrajectory( robot_pose, motion_target, costmap_transform, trajectory, backward); } @@ -232,7 +232,7 @@ TEST(SmoothControlLawTest, calculateNextPose) { EXPECT_NEAR(tf2::getYaw(next_pose.orientation), 0.0, 0.1); } -TEST(GracefulMotionControllerTest, configure) { +TEST(GracefulControllerTest, configure) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -257,7 +257,7 @@ TEST(GracefulMotionControllerTest, configure) { controller->cleanup(); } -TEST(GracefulMotionControllerTest, dynamicParameters) { +TEST(GracefulControllerTest, dynamicParameters) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -356,7 +356,7 @@ TEST(GracefulMotionControllerTest, dynamicParameters) { EXPECT_EQ(controller->getAllowBackward(), false); } -TEST(GracefulMotionControllerTest, getDifferentMotionTargets) { +TEST(GracefulControllerTest, getDifferentMotionTargets) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -406,7 +406,7 @@ TEST(GracefulMotionControllerTest, getDifferentMotionTargets) { EXPECT_EQ(motion_target.pose.orientation, tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0))); } -TEST(GracefulMotionControllerTest, createMotionTargetMsg) { +TEST(GracefulControllerTest, createMotionTargetMsg) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -434,7 +434,7 @@ TEST(GracefulMotionControllerTest, createMotionTargetMsg) { EXPECT_EQ(motion_target_msg.point.z, 0.01); } -TEST(GracefulMotionControllerTest, createSlowdownMsg) { +TEST(GracefulControllerTest, createSlowdownMsg) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -486,7 +486,7 @@ TEST(GracefulMotionControllerTest, createSlowdownMsg) { EXPECT_EQ(slowdown_msg.color.b, 0.0); } -TEST(GracefulMotionControllerTest, rotateToTarget) { +TEST(GracefulControllerTest, rotateToTarget) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -524,7 +524,7 @@ TEST(GracefulMotionControllerTest, rotateToTarget) { EXPECT_EQ(cmd_vel.angular.z, -0.25); } -TEST(GracefulMotionControllerTest, setSpeedLimit) { +TEST(GracefulControllerTest, setSpeedLimit) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); auto costmap_ros = std::make_shared("global_costmap"); @@ -568,7 +568,7 @@ TEST(GracefulMotionControllerTest, setSpeedLimit) { EXPECT_EQ(speed_limit, 2.0); } -TEST(GracefulMotionControllerTest, emptyPlan) { +TEST(GracefulControllerTest, emptyPlan) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -619,7 +619,7 @@ TEST(GracefulMotionControllerTest, emptyPlan) { EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::InvalidPath); } -TEST(GracefulMotionControllerTest, poseOutsideCostmap) { +TEST(GracefulControllerTest, poseOutsideCostmap) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -674,7 +674,7 @@ TEST(GracefulMotionControllerTest, poseOutsideCostmap) { EXPECT_THROW(controller->transformGlobalPlan(robot_pose), nav2_core::ControllerException); } -TEST(GracefulMotionControllerTest, noPruningPlan) { +TEST(GracefulControllerTest, noPruningPlan) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -740,7 +740,7 @@ TEST(GracefulMotionControllerTest, noPruningPlan) { EXPECT_EQ(transformed_plan.poses.size(), global_plan.poses.size()); } -TEST(GracefulMotionControllerTest, pruningPlan) { +TEST(GracefulControllerTest, pruningPlan) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -817,7 +817,7 @@ TEST(GracefulMotionControllerTest, pruningPlan) { EXPECT_EQ(transformed_plan.poses.size(), 3); } -TEST(GracefulMotionControllerTest, pruningPlanOutsideCostmap) { +TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -883,7 +883,7 @@ TEST(GracefulMotionControllerTest, pruningPlanOutsideCostmap) { EXPECT_EQ(transformed_plan.poses.size(), 2); } -TEST(GracefulMotionControllerTest, computeVelocityCommandRotate) { +TEST(GracefulControllerTest, computeVelocityCommandRotate) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -964,7 +964,7 @@ TEST(GracefulMotionControllerTest, computeVelocityCommandRotate) { EXPECT_LE(cmd_vel.twist.angular.x, 0.5); } -TEST(GracefulMotionControllerTest, computeVelocityCommandRegular) { +TEST(GracefulControllerTest, computeVelocityCommandRegular) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -1039,7 +1039,7 @@ TEST(GracefulMotionControllerTest, computeVelocityCommandRegular) { EXPECT_EQ(cmd_vel.twist.angular.z, 0.0); } -TEST(GracefulMotionControllerTest, computeVelocityCommandRegularBackwards) { +TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); @@ -1120,7 +1120,7 @@ TEST(GracefulMotionControllerTest, computeVelocityCommandRegularBackwards) { EXPECT_LT(cmd_vel.twist.angular.z, 0.0); } -TEST(GracefulMotionControllerTest, computeVelocityCommandFinal) { +TEST(GracefulControllerTest, computeVelocityCommandFinal) { auto node = std::make_shared("testGraceful"); auto tf = std::make_shared(node->get_clock()); diff --git a/nav2_graceful_motion_controller/graceful_controller_plugin.xml b/nav2_graceful_motion_controller/graceful_controller_plugin.xml deleted file mode 100644 index b9c70ec7ab..0000000000 --- a/nav2_graceful_motion_controller/graceful_controller_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Graceful motion controller for Nav2 - - -