From 4e878a20a1bcc1d41a9ff82b8ffbb732e3effab4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:49:25 +0100 Subject: [PATCH 01/19] Add IsStoppedBTNode Signed-off-by: Tony Najjar --- nav2_behavior_tree/CMakeLists.txt | 3 + .../condition/is_stopped_condition.hpp | 95 ++++++++++++++++ .../condition/is_stopped_condition.cpp | 102 ++++++++++++++++++ 3 files changed, 200 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 30955681deb..3aaec265445 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -95,6 +95,9 @@ list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp) list(APPEND plugin_libs nav2_is_stuck_condition_bt_node) +add_library(nav2_is_stopped_condition_bt_node SHARED plugins/condition/is_stopped_condition.cpp) +list(APPEND plugin_libs nav2_is_stopped_condition_bt_node) + add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp) list(APPEND plugin_libs nav2_transform_available_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp new file mode 100644 index 00000000000..43a03ee34aa --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "nav_msgs/msg/odometry.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is stuck somewhere and FAILURE otherwise + */ +class IsStoppedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsStoppedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsStoppedCondition() = delete; + + /** + * @brief A destructor for nav2_behavior_tree::IsStoppedCondition + */ + ~IsStoppedCondition() override; + + /** + * @brief Callback function for odom topic + * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message + */ + void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), + BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped") + }; + } + +private: + // The node that will be used for any ROS operations + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + std::thread callback_group_executor_thread; + + std::atomic is_stopped_; + double velocity_threshold_; + std::chrono::milliseconds time_stopped_threshold_; + rclcpp::Time stopped_stamp_; + std::mutex mutex_; + + // Listen to odometry + rclcpp::Subscription::SharedPtr odom_sub_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp new file mode 100644 index 00000000000..c0dc266a3c5 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -0,0 +1,102 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nav2_behavior_tree +{ + +IsStoppedCondition::IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + is_stopped_(false), + velocity_threshold_(0.1), + time_stopped_threshold_(1000), + stopped_stamp_(rclcpp::Time(0)) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();}); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + odom_sub_ = node_->create_subscription( + "odom", + rclcpp::SystemDefaultsQoS(), + std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), + sub_option); +} + +IsStoppedCondition::~IsStoppedCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); + callback_group_executor_.cancel(); + callback_group_executor_thread.join(); +} + +void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) +{ + getInput("velocity_threshold", velocity_threshold_); + getInput("time_stopped_threshold", time_stopped_threshold_); + + // Check if the robot is stopped for a certain amount of time + if (msg->twist.twist.linear.x < velocity_threshold_ && + msg->twist.twist.linear.y < velocity_threshold_ && + msg->twist.twist.angular.z < velocity_threshold_) + { + if (stopped_stamp_ == rclcpp::Time(0)) { + stopped_stamp_ = rclcpp::Time(msg->header.stamp); + } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + is_stopped_ = true; + } + } else { + stopped_stamp_ = rclcpp::Time(0); + + std::lock_guard lock(mutex_); + is_stopped_ = false; + } +} + +BT::NodeStatus IsStoppedCondition::tick() +{ + std::lock_guard lock(mutex_); + if (is_stopped_) { + return BT::NodeStatus::SUCCESS; + } + else if (stopped_stamp_ != rclcpp::Time(0)) { + // Robot was stopped but not for long enough + return BT::NodeStatus::RUNNING; + } + else { + return BT::NodeStatus::FAILURE; + } +} + + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsStopped"); +} From 9139f9f81d0e0fabbae7f1ed63e307a2f5713427 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:52:58 +0100 Subject: [PATCH 02/19] add topic name + reformat Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 11 +++++++++-- .../plugins/condition/is_stopped_condition.cpp | 14 ++++++++------ 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 43a03ee34aa..ac34fdcbbce 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -68,8 +68,14 @@ class IsStoppedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped") + BT::InputPort("velocity_threshold", 0.1, + "Velocity threshold below which robot is considered stopped"), + BT::InputPort("time_stopped_threshold", 1000, + "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), + BT::InputPort( + "topic_name", + "odom", + "the odometry topic name"), }; } @@ -80,6 +86,7 @@ class IsStoppedCondition : public BT::ConditionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::thread callback_group_executor_thread; + std::string topic_name_; std::atomic is_stopped_; double velocity_threshold_; std::chrono::milliseconds time_stopped_threshold_; diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index c0dc266a3c5..6080d975e6c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -32,6 +32,8 @@ IsStoppedCondition::IsStoppedCondition( stopped_stamp_(rclcpp::Time(0)) { node_ = config().blackboard->get("node"); + getInput("topic_name", topic_name_); + callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -41,7 +43,7 @@ IsStoppedCondition::IsStoppedCondition( rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; odom_sub_ = node_->create_subscription( - "odom", + topic_name_, rclcpp::SystemDefaultsQoS(), std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), sub_option); @@ -66,7 +68,9 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: { if (stopped_stamp_ == rclcpp::Time(0)) { stopped_stamp_ = rclcpp::Time(msg->header.stamp); - } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > + rclcpp::Duration(time_stopped_threshold_)) + { is_stopped_ = true; } } else { @@ -82,12 +86,10 @@ BT::NodeStatus IsStoppedCondition::tick() std::lock_guard lock(mutex_); if (is_stopped_) { return BT::NodeStatus::SUCCESS; - } - else if (stopped_stamp_ != rclcpp::Time(0)) { + } else if (stopped_stamp_ != rclcpp::Time(0)) { // Robot was stopped but not for long enough return BT::NodeStatus::RUNNING; - } - else { + } else { return BT::NodeStatus::FAILURE; } } From c7ec42efddc9b43a235d617622841487ed37ca9a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:55:55 +0100 Subject: [PATCH 03/19] fix comment Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index ac34fdcbbce..efe37182095 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -28,7 +28,7 @@ namespace nav2_behavior_tree /** * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS - * if robot is stuck somewhere and FAILURE otherwise + * if robot is considered stopped for long enough, RUNNING if stopped but not for long enough and FAILURE otherwise */ class IsStoppedCondition : public BT::ConditionNode { @@ -99,4 +99,4 @@ class IsStoppedCondition : public BT::ConditionNode } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_ +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ From 5a8ea9209eac33c3258d1e019cc7c8839a45b43a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 18:20:09 +0100 Subject: [PATCH 04/19] fix abs Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 6080d975e6c..5a9e34d094c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -29,7 +29,7 @@ IsStoppedCondition::IsStoppedCondition( is_stopped_(false), velocity_threshold_(0.1), time_stopped_threshold_(1000), - stopped_stamp_(rclcpp::Time(0)) + stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); getInput("topic_name", topic_name_); @@ -60,13 +60,14 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: { getInput("velocity_threshold", velocity_threshold_); getInput("time_stopped_threshold", time_stopped_threshold_); + std::lock_guard lock(mutex_); // Check if the robot is stopped for a certain amount of time - if (msg->twist.twist.linear.x < velocity_threshold_ && - msg->twist.twist.linear.y < velocity_threshold_ && - msg->twist.twist.angular.z < velocity_threshold_) + if (abs(msg->twist.twist.linear.x) < velocity_threshold_ && + abs(msg->twist.twist.linear.y) < velocity_threshold_ && + abs(msg->twist.twist.angular.z) < velocity_threshold_) { - if (stopped_stamp_ == rclcpp::Time(0)) { + if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { stopped_stamp_ = rclcpp::Time(msg->header.stamp); } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) @@ -74,9 +75,7 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: is_stopped_ = true; } } else { - stopped_stamp_ = rclcpp::Time(0); - - std::lock_guard lock(mutex_); + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); is_stopped_ = false; } } @@ -86,8 +85,9 @@ BT::NodeStatus IsStoppedCondition::tick() std::lock_guard lock(mutex_); if (is_stopped_) { return BT::NodeStatus::SUCCESS; - } else if (stopped_stamp_ != rclcpp::Time(0)) { + } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { // Robot was stopped but not for long enough + RCLCPP_INFO(node_->get_logger(), "Robot is not stopped, waiting"); return BT::NodeStatus::RUNNING; } else { return BT::NodeStatus::FAILURE; From 9b5a93ef0b88da7022fd74849c63f632192c4045 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 18:23:56 +0100 Subject: [PATCH 05/19] remove log Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 5a9e34d094c..a6775b656f9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -87,7 +87,6 @@ BT::NodeStatus IsStoppedCondition::tick() return BT::NodeStatus::SUCCESS; } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { // Robot was stopped but not for long enough - RCLCPP_INFO(node_->get_logger(), "Robot is not stopped, waiting"); return BT::NodeStatus::RUNNING; } else { return BT::NodeStatus::FAILURE; From 89c8d341f7bb22b7f00be2528d5215e46e2abd2e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:16 +0100 Subject: [PATCH 06/19] add getter functions for raw twist Signed-off-by: Tony Najjar --- .../include/nav2_util/odometry_utils.hpp | 35 +++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 185a86d1471..276f1a0414e 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -68,13 +68,44 @@ class OdomSmoother * @brief Get twist msg from smoother * @return twist Twist msg */ - inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + inline geometry_msgs::msg::Twist getTwist() + { + std::lock_guard lock(odom_mutex_); + return vel_smooth_.twist; + } /** * @brief Get twist stamped msg from smoother * @return twist TwistStamped msg */ - inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} + inline geometry_msgs::msg::TwistStamped getTwistStamped() + { + std::lock_guard lock(odom_mutex_); + return vel_smooth_; + } + + /** + * @brief Get raw twist msg from smoother (without smoothing) + * @return twist Twist msg + */ + inline geometry_msgs::msg::Twist getRawTwist() + { + std::lock_guard lock(odom_mutex_); + return odom_history_.back().twist.twist; + } + + /** + * @brief Get raw twist stamped msg from smoother (without smoothing) + * @return twist TwistStamped msg + */ + inline geometry_msgs::msg::TwistStamped getRawTwistStamped() + { + std::lock_guard lock(odom_mutex_); + geometry_msgs::msg::TwistStamped twist_stamped; + twist_stamped.header = odom_history_.back().header; + twist_stamped.twist = odom_history_.back().twist.twist; + return twist_stamped; + } protected: /** From c0098c4bcfb599b017a88c9f5d586c30abac856f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:27 +0100 Subject: [PATCH 07/19] remove unused code Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/decorator/speed_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 87c9eb5bf16..f5b4eb3bd86 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,8 +50,6 @@ SpeedController::SpeedController( d_rate_ = max_rate_ - min_rate_; d_speed_ = max_speed_ - min_speed_; - std::string odom_topic; - node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); odom_smoother_ = config().blackboard->get>( "odom_smoother"); } From 18b8b761d1ac383e60cfa12fd953b7dcf0094c36 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:45 +0100 Subject: [PATCH 08/19] use odomsmoother Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 21 +----- .../condition/is_stopped_condition.cpp | 65 ++++++------------- 2 files changed, 22 insertions(+), 64 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index efe37182095..51b249afb50 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" +#include "nav2_util/odometry_utils.hpp" namespace nav2_behavior_tree { @@ -49,12 +50,6 @@ class IsStoppedCondition : public BT::ConditionNode */ ~IsStoppedCondition() override; - /** - * @brief Callback function for odom topic - * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message - */ - void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); - /** * @brief The main override required by a BT action * @return BT::NodeStatus Status of tick execution @@ -72,29 +67,17 @@ class IsStoppedCondition : public BT::ConditionNode "Velocity threshold below which robot is considered stopped"), BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), - BT::InputPort( - "topic_name", - "odom", - "the odometry topic name"), }; } private: - // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::thread callback_group_executor_thread; - std::string topic_name_; - std::atomic is_stopped_; double velocity_threshold_; std::chrono::milliseconds time_stopped_threshold_; rclcpp::Time stopped_stamp_; - std::mutex mutex_; - // Listen to odometry - rclcpp::Subscription::SharedPtr odom_sub_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index a6775b656f9..14c21b43a0c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -26,74 +26,49 @@ IsStoppedCondition::IsStoppedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - is_stopped_(false), velocity_threshold_(0.1), time_stopped_threshold_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); - getInput("topic_name", topic_name_); - - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();}); - - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - odom_sub_ = node_->create_subscription( - topic_name_, - rclcpp::SystemDefaultsQoS(), - std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), - sub_option); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); } IsStoppedCondition::~IsStoppedCondition() { RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); - callback_group_executor_.cancel(); - callback_group_executor_thread.join(); } -void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) +BT::NodeStatus IsStoppedCondition::tick() { - getInput("velocity_threshold", velocity_threshold_); - getInput("time_stopped_threshold", time_stopped_threshold_); - std::lock_guard lock(mutex_); + auto twist = odom_smoother_->getRawTwistStamped(); + + // if there is no timestamp, set it to now + if (twist.header.stamp.sec == 0 && twist.header.stamp.nanosec == 0) { + twist.header.stamp = node_->get_clock()->now(); + } - // Check if the robot is stopped for a certain amount of time - if (abs(msg->twist.twist.linear.x) < velocity_threshold_ && - abs(msg->twist.twist.linear.y) < velocity_threshold_ && - abs(msg->twist.twist.angular.z) < velocity_threshold_) + if (abs(twist.twist.linear.x) < velocity_threshold_ && + abs(twist.twist.linear.y) < velocity_threshold_ && + abs(twist.twist.angular.z) < velocity_threshold_) { if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { - stopped_stamp_ = rclcpp::Time(msg->header.stamp); - } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > - rclcpp::Duration(time_stopped_threshold_)) - { - is_stopped_ = true; + stopped_stamp_ = rclcpp::Time(twist.header.stamp); + } + + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::RUNNING; } - } else { - stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - is_stopped_ = false; - } -} -BT::NodeStatus IsStoppedCondition::tick() -{ - std::lock_guard lock(mutex_); - if (is_stopped_) { - return BT::NodeStatus::SUCCESS; - } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { - // Robot was stopped but not for long enough - return BT::NodeStatus::RUNNING; } else { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); return BT::NodeStatus::FAILURE; } } - } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" From 5c0560aa89853070c6388ccd91c4b503cf25bd86 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:40:22 +0100 Subject: [PATCH 09/19] fix formatting Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 7 ++++--- .../plugins/condition/is_stopped_condition.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 51b249afb50..388f471d317 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" @@ -65,8 +66,8 @@ class IsStoppedCondition : public BT::ConditionNode return { BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("time_stopped_threshold", 1000, - "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), + BT::InputPort("duration_stopped", 1000, + "Duration (ms) the velocity must remain below the threshold."), }; } @@ -74,7 +75,7 @@ class IsStoppedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; double velocity_threshold_; - std::chrono::milliseconds time_stopped_threshold_; + std::chrono::milliseconds duration_stopped_; rclcpp::Time stopped_stamp_; std::shared_ptr odom_smoother_; diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 14c21b43a0c..7e38e8659d4 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -27,7 +27,7 @@ IsStoppedCondition::IsStoppedCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), velocity_threshold_(0.1), - time_stopped_threshold_(1000), + duration_stopped_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); @@ -57,7 +57,7 @@ BT::NodeStatus IsStoppedCondition::tick() stopped_stamp_ = rclcpp::Time(twist.header.stamp); } - if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; From 66ed384b5bec683d68370e02c7bc6171eaa45a5b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:45:00 +0100 Subject: [PATCH 10/19] update groot Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 2 +- nav2_behavior_tree/nav2_tree_nodes.xml | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 388f471d317..2456c2315da 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -67,7 +67,7 @@ class IsStoppedCondition : public BT::ConditionNode BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), BT::InputPort("duration_stopped", 1000, - "Duration (ms) the velocity must remain below the threshold."), + "Duration (ms) the velocity must remain below the threshold"), }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 71c20ee4ecc..67059def0eb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -227,6 +227,11 @@ + + Velocity threshold below which robot is considered stopped + Duration (ms) the velocity must remain below the threshold + + Child frame for transform Parent frame for transform From 91230b2b619a6adf4ffacf6bc2da8cf016297baa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 27 Nov 2024 10:26:00 +0100 Subject: [PATCH 11/19] Add test Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 2 +- .../test/plugins/condition/CMakeLists.txt | 2 + .../plugins/condition/test_is_stopped.cpp | 107 ++++++++++++++++++ .../include/nav2_util/odometry_utils.hpp | 24 ++++ nav2_util/src/odometry_utils.cpp | 3 +- 5 files changed, 136 insertions(+), 2 deletions(-) create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 2456c2315da..cafe2b666f8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -64,7 +64,7 @@ class IsStoppedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("velocity_threshold", 0.1, + BT::InputPort("velocity_threshold", 0.01, "Velocity threshold below which robot is considered stopped"), BT::InputPort("duration_stopped", 1000, "Duration (ms) the velocity must remain below the threshold"), diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index de380c769af..106c4a96f27 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -16,6 +16,8 @@ plugin_add_test(test_condition_transform_available test_transform_available.cpp plugin_add_test(test_condition_is_stuck test_is_stuck.cpp nav2_is_stuck_condition_bt_node) +plugin_add_test(test_condition_is_stopped test_is_stopped.cpp nav2_is_stopped_condition_bt_node) + plugin_add_test(test_condition_is_battery_charging test_is_battery_charging.cpp nav2_is_battery_charging_condition_bt_node) plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_battery_low_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp new file mode 100644 index 00000000000..aaf1afe6cab --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + odom_smoother_ = std::make_shared(node_); + config_->blackboard->set( + "odom_smoother", odom_smoother_); // NOLINT + bt_node_ = std::make_shared("is_stopped", *config_); + } + + void TearDown() + { + bt_node_.reset(); + odom_smoother_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr odom_smoother_; +}; + +std::shared_ptr +IsStoppedTestFixture::bt_node_ = nullptr; +std::shared_ptr +IsStoppedTestFixture::odom_smoother_ = nullptr; + + +TEST_F(IsStoppedTestFixture, test_behavior) +{ + auto odom_pub = node_->create_publisher("odom", + rclcpp::SystemDefaultsQoS()); + nav_msgs::msg::Odometry odom_msg; + + // Test FAILURE when robot is moving + auto time = node_->now(); + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(500ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test RUNNING when robot is stopped but not long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(500ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + + // Test SUCCESS when robot is stopped for long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(1100ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test FAILURE immediately after robot starts moving + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.angular.z = 0.1; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 276f1a0414e..ab9132b5ffe 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -71,6 +71,12 @@ class OdomSmoother inline geometry_msgs::msg::Twist getTwist() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } return vel_smooth_.twist; } @@ -81,6 +87,12 @@ class OdomSmoother inline geometry_msgs::msg::TwistStamped getTwistStamped() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::TwistStamped twist_stamped; + return twist_stamped; + } return vel_smooth_; } @@ -91,6 +103,12 @@ class OdomSmoother inline geometry_msgs::msg::Twist getRawTwist() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } return odom_history_.back().twist.twist; } @@ -102,6 +120,11 @@ class OdomSmoother { std::lock_guard lock(odom_mutex_); geometry_msgs::msg::TwistStamped twist_stamped; + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + return twist_stamped; + } twist_stamped.header = odom_history_.back().header; twist_stamped.twist = odom_history_.back().twist.twist; return twist_stamped; @@ -119,6 +142,7 @@ class OdomSmoother */ void updateState(); + bool received_odom_; rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; geometry_msgs::msg::TwistStamped vel_smooth_; diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 9bf585bdfe2..6b50c332bcc 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -27,7 +27,7 @@ OdomSmoother::OdomSmoother( const rclcpp::Node::WeakPtr & parent, double filter_duration, const std::string & odom_topic) -: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +: received_odom_(false), odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); odom_sub_ = node->create_subscription( @@ -66,6 +66,7 @@ OdomSmoother::OdomSmoother( void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(odom_mutex_); + received_odom_ = true; // update cumulated odom only if history is not empty if (!odom_history_.empty()) { From d2c2db323e4573042a67c562d939c63d5f7a70ec Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 27 Nov 2024 11:00:07 +0100 Subject: [PATCH 12/19] reset at success Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 7e38e8659d4..4ac5352f461 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -58,6 +58,7 @@ BT::NodeStatus IsStoppedCondition::tick() } if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; From 37a56365d3eadeac2850c6d506e68508c0fbef93 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 12:56:38 +0100 Subject: [PATCH 13/19] FIX velocity_threshold_ Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 4ac5352f461..9dc629b18f9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -26,7 +26,7 @@ IsStoppedCondition::IsStoppedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - velocity_threshold_(0.1), + velocity_threshold_(0.01), duration_stopped_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { From d237785666574f809398f6e0f5104bc9a25246d4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:28:58 +0100 Subject: [PATCH 14/19] Fix stopped Node Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 6 +++- .../condition/is_stopped_condition.cpp | 7 +++-- .../plugins/condition/test_is_stopped.cpp | 30 ++++++++++++++----- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index cafe2b666f8..40197939d1f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -24,6 +24,10 @@ #include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + + +using namespace std::chrono_literals; // NOLINT namespace nav2_behavior_tree { @@ -66,7 +70,7 @@ class IsStoppedCondition : public BT::ConditionNode return { BT::InputPort("velocity_threshold", 0.01, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("duration_stopped", 1000, + BT::InputPort("duration_stopped", 1000ms, "Duration (ms) the velocity must remain below the threshold"), }; } diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 9dc629b18f9..f0b32d4c1c0 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -17,8 +17,6 @@ #include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" -using namespace std::chrono_literals; // NOLINT - namespace nav2_behavior_tree { @@ -27,7 +25,7 @@ IsStoppedCondition::IsStoppedCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), velocity_threshold_(0.01), - duration_stopped_(1000), + duration_stopped_(1000ms), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); @@ -42,6 +40,9 @@ IsStoppedCondition::~IsStoppedCondition() BT::NodeStatus IsStoppedCondition::tick() { + getInput("velocity_threshold", velocity_threshold_); + getInput("duration_stopped", duration_stopped_); + auto twist = odom_smoother_->getRawTwistStamped(); // if there is no timestamp, set it to now diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp index aaf1afe6cab..a94e4285fd9 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -34,6 +34,8 @@ class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture odom_smoother_ = std::make_shared(node_); config_->blackboard->set( "odom_smoother", odom_smoother_); // NOLINT + std::chrono::milliseconds duration = 100ms; + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; // to make the test faster bt_node_ = std::make_shared("is_stopped", *config_); } @@ -65,22 +67,17 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.header.stamp = time; odom_msg.twist.twist.linear.x = 1.0; odom_pub->publish(odom_msg); - odom_pub->publish(odom_msg); - std::this_thread::sleep_for(500ms); + std::this_thread::sleep_for(10ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); // Test RUNNING when robot is stopped but not long enough odom_msg.header.stamp = node_->now(); odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); - std::this_thread::sleep_for(500ms); + std::this_thread::sleep_for(90ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - // Test SUCCESS when robot is stopped for long enough - odom_msg.header.stamp = node_->now(); - odom_msg.twist.twist.linear.x = 0.001; - odom_pub->publish(odom_msg); - std::this_thread::sleep_for(1100ms); + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test FAILURE immediately after robot starts moving @@ -89,6 +86,23 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_pub->publish(odom_msg); std::this_thread::sleep_for(10ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test SUCCESS when robot is stopped for long with a back-dated timestamp + odom_msg.header.stamp = node_->now() - rclcpp::Duration(2, 0); + odom_msg.twist.twist.angular.z = 0; + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test SUCCESS when robot is stopped for long enough without a stamp for odometry + odom_msg.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // In the first tick, the timestamp is set + std::this_thread::sleep_for(110ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) From 5a4204f34e455da8d63115755bdf43855bd46ae6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:52:07 +0100 Subject: [PATCH 15/19] Add tests to odometry_utils Signed-off-by: Tony Najjar --- nav2_util/test/test_odometry_utils.cpp | 45 ++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 7d5b8bf9fe7..6871949303e 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -32,6 +32,34 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +TEST(OdometryUtils, test_uninitialized) +{ + auto node = std::make_shared("test_node"); + nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::TwistStamped twist_stamped_msg; + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_msg = odom_smoother.getRawTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getRawTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); +} + TEST(OdometryUtils, test_smoothed_velocity) { auto node = std::make_shared("test_node"); @@ -41,6 +69,7 @@ TEST(OdometryUtils, test_smoothed_velocity) nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::Twist twist_raw_msg; auto time = node->now(); @@ -67,9 +96,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 1.5); EXPECT_EQ(twist_msg.linear.y, 1.5); EXPECT_EQ(twist_msg.angular.z, 1.5); + EXPECT_EQ(twist_raw_msg.linear.x, 2.0); + EXPECT_EQ(twist_raw_msg.linear.y, 2.0); + EXPECT_EQ(twist_raw_msg.angular.z, 2.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2); odom_msg.twist.twist.linear.x = 3.0; @@ -81,9 +114,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 2.0); EXPECT_EQ(twist_msg.linear.y, 2.0); EXPECT_EQ(twist_msg.angular.z, 2.0); + EXPECT_EQ(twist_raw_msg.linear.x, 3.0); + EXPECT_EQ(twist_raw_msg.linear.y, 3.0); + EXPECT_EQ(twist_raw_msg.angular.z, 3.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45); odom_msg.twist.twist.linear.x = 4.0; @@ -95,9 +132,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 3.5); EXPECT_EQ(twist_msg.linear.y, 3.5); EXPECT_EQ(twist_msg.angular.z, 3.5); + EXPECT_EQ(twist_raw_msg.linear.x, 4.0); + EXPECT_EQ(twist_raw_msg.linear.y, 4.0); + EXPECT_EQ(twist_raw_msg.angular.z, 4.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0); odom_msg.twist.twist.linear.x = 5.0; @@ -109,7 +150,11 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 5.0); EXPECT_EQ(twist_msg.linear.y, 5.0); EXPECT_EQ(twist_msg.angular.z, 5.0); + EXPECT_EQ(twist_raw_msg.linear.x, 5.0); + EXPECT_EQ(twist_raw_msg.linear.y, 5.0); + EXPECT_EQ(twist_raw_msg.angular.z, 5.0); } From 0295ef95b13b9f90edb5f249997996527ff59b7c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:55:28 +0100 Subject: [PATCH 16/19] fix linting Signed-off-by: Tony Najjar --- .../test/plugins/condition/test_is_stopped.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp index a94e4285fd9..26ae239a180 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -34,8 +34,9 @@ class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture odom_smoother_ = std::make_shared(node_); config_->blackboard->set( "odom_smoother", odom_smoother_); // NOLINT + // shorten duration_stopped from default to make the test faster std::chrono::milliseconds duration = 100ms; - config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; // to make the test faster + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; bt_node_ = std::make_shared("is_stopped", *config_); } @@ -77,7 +78,7 @@ TEST_F(IsStoppedTestFixture, test_behavior) std::this_thread::sleep_for(90ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // Test SUCCESS when robot is stopped for long enough - std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test FAILURE immediately after robot starts moving @@ -92,7 +93,7 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.twist.twist.angular.z = 0; odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); - std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test SUCCESS when robot is stopped for long enough without a stamp for odometry @@ -100,7 +101,8 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); std::this_thread::sleep_for(10ms); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // In the first tick, the timestamp is set + // In the first tick, the timestamp is set + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); std::this_thread::sleep_for(110ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } From e4a4d38f634cef034d712aac8542677201c0036d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 13 Dec 2024 12:54:08 +0100 Subject: [PATCH 17/19] Fix typos Signed-off-by: Tony Najjar --- Doxyfile | 4 +- doc/requirements/requirements.md | 4 +- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 2 +- nav2_amcl/include/nav2_amcl/pf/pf.hpp | 2 +- .../include/nav2_amcl/sensors/laser/laser.hpp | 12 +-- nav2_amcl/src/amcl_node.cpp | 2 +- nav2_amcl/src/pf/pf.c | 4 +- nav2_amcl/src/pf/pf_draw.c | 2 +- .../nav2_behavior_tree/bt_action_server.hpp | 2 +- .../include/nav2_behavior_tree/bt_utils.hpp | 2 +- .../action/drive_on_heading_action.hpp | 2 +- .../condition/is_path_valid_condition.hpp | 2 +- .../plugins/condition/is_stuck_condition.hpp | 2 +- nav2_behavior_tree/nav2_tree_nodes.xml | 12 +-- .../action/drive_on_heading_action.cpp | 6 +- .../test_remove_in_collision_goals_action.cpp | 10 +-- nav2_behaviors/README.md | 2 +- .../include/nav2_behaviors/timed_behavior.hpp | 6 +- nav2_behaviors/plugins/assisted_teleop.cpp | 2 +- nav2_bringup/README.md | 2 +- nav2_bringup/launch/localization_launch.py | 2 +- nav2_bringup/launch/navigation_launch.py | 2 +- nav2_bringup/launch/tb3_simulation_launch.py | 2 +- nav2_bringup/launch/tb4_simulation_launch.py | 2 +- .../include/nav2_collision_monitor/circle.hpp | 2 +- .../nav2_collision_monitor/polygon.hpp | 2 +- .../velocity_polygon.hpp | 6 +- .../launch/collision_detector_node.launch.py | 2 +- .../launch/collision_monitor_node.launch.py | 2 +- .../src/collision_monitor_node.cpp | 2 +- nav2_collision_monitor/src/source.cpp | 2 +- .../test/collision_monitor_node_test.cpp | 16 ++-- nav2_collision_monitor/test/polygons_test.cpp | 4 +- .../test/velocity_polygons_test.cpp | 4 +- .../launch/parse_multirobot_pose.py | 2 +- .../nav2_constrained_smoother/utils.hpp | 2 +- .../test/test_constrained_smoother.cpp | 2 +- .../plugins/pose_progress_checker.hpp | 2 +- .../plugins/simple_goal_checker.hpp | 2 +- .../plugins/simple_progress_checker.hpp | 2 +- .../plugins/stopped_goal_checker.hpp | 2 +- nav2_controller/src/controller_server.cpp | 2 +- nav2_core/include/nav2_core/behavior.hpp | 2 +- nav2_core/include/nav2_core/controller.hpp | 2 +- .../include/nav2_core/global_planner.hpp | 2 +- nav2_costmap_2d/README.md | 2 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 2 +- .../costmap_filters/binary_filter.hpp | 2 +- .../costmap_filters/keepout_filter.hpp | 2 +- .../costmap_filters/speed_filter.hpp | 2 +- .../nav2_costmap_2d/inflation_layer.hpp | 2 +- .../nav2_costmap_2d/layered_costmap.hpp | 2 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 4 +- .../plugins/range_sensor_layer.cpp | 2 +- nav2_costmap_2d/plugins/voxel_layer.cpp | 4 +- nav2_costmap_2d/src/costmap_2d.cpp | 6 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- .../test/integration/costmap_tester.cpp | 2 +- .../test/integration/inflation_tests.cpp | 4 +- .../test/integration/range_tests.cpp | 4 +- .../integration/test_costmap_subscriber.cpp | 12 +-- nav2_costmap_2d/test/module_tests.cpp | 6 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 4 +- .../test/unit/binary_filter_test.cpp | 2 +- nav2_docking/README.md | 4 +- .../opennav_docking/docking_server.hpp | 2 +- .../include/opennav_docking/pose_filter.hpp | 2 +- .../opennav_docking/simple_charging_dock.hpp | 2 +- .../simple_non_charging_dock.hpp | 2 +- .../opennav_docking/test/test_pose_filter.cpp | 2 +- .../opennav_docking_core/charging_dock.hpp | 2 +- .../non_charging_dock.hpp | 2 +- .../include/costmap_queue/costmap_queue.hpp | 4 +- .../include/dwb_critics/map_grid.hpp | 4 +- .../dwb_critics/obstacle_footprint.hpp | 2 +- .../include/dwb_critics/rotate_to_goal.hpp | 2 +- .../dwb_critics/src/goal_dist.cpp | 2 +- .../dwb_critics/src/map_grid.cpp | 4 +- .../dwb_critics/src/path_dist.cpp | 2 +- .../test/obstacle_footprint_test.cpp | 2 +- .../dwb_plugins/kinematic_parameters.hpp | 2 +- .../dwb_plugins/one_d_velocity_iterator.hpp | 2 +- nav2_graceful_controller/README.md | 2 +- .../lifecycle_manager.hpp | 4 +- .../nav2_loopback_sim/loopback_simulator.py | 2 +- .../test/component/test_map_saver_node.cpp | 6 +- .../critics/path_follow_critic.hpp | 2 +- .../nav2_mppi_controller/optimizer.hpp | 2 +- .../tools/parameters_handler.hpp | 4 +- .../nav2_mppi_controller/tools/utils.hpp | 12 +-- .../src/critics/path_angle_critic.cpp | 8 +- .../src/critics/path_follow_critic.cpp | 12 +-- nav2_mppi_controller/src/optimizer.cpp | 8 +- nav2_mppi_controller/src/path_handler.cpp | 4 +- .../test/controller_state_transition_test.cpp | 2 +- .../test/parameter_handler_test.cpp | 28 +++--- .../test/trajectory_visualizer_tests.cpp | 90 +++++++++---------- nav2_mppi_controller/test/utils_test.cpp | 2 +- nav2_msgs/srv/LoadMap.srv | 2 +- .../include/nav2_navfn_planner/navfn.hpp | 2 +- .../nav2_navfn_planner/navfn_planner.hpp | 2 +- nav2_planner/src/planner_server.cpp | 4 +- .../README.md | 2 +- .../regulated_pure_pursuit_controller.hpp | 4 +- .../src/regulated_pure_pursuit_controller.cpp | 2 +- .../test/test_regulated_pp.cpp | 6 +- .../include/nav2_rviz_plugins/utils.hpp | 2 +- nav2_rviz_plugins/src/docking_panel.cpp | 2 +- nav2_rviz_plugins/src/utils.cpp | 2 +- .../nav2_simple_commander/demo_inspection.py | 2 +- .../nav2_simple_commander/demo_picking.py | 4 +- .../nav2_simple_commander/demo_security.py | 2 +- .../nav2_simple_commander/line_iterator.py | 2 +- .../test/test_line_iterator.py | 2 +- nav2_smac_planner/README.md | 2 +- .../include/nav2_smac_planner/a_star.hpp | 6 +- .../nav2_smac_planner/analytic_expansion.hpp | 2 +- .../include/nav2_smac_planner/node_2d.hpp | 4 +- .../include/nav2_smac_planner/node_hybrid.hpp | 4 +- .../nav2_smac_planner/node_lattice.hpp | 4 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 +- .../smac_planner_lattice.hpp | 2 +- .../include/nav2_smac_planner/smoother.hpp | 2 +- .../nav2_smac_planner/thirdparty/robin_hood.h | 8 +- .../lattice_primitives/README.md | 6 +- .../lattice_primitives/lattice_generator.py | 4 +- .../tests/trajectory_visualizer.py | 2 +- .../trajectory_generator.py | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/node_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smoother.cpp | 4 +- nav2_smac_planner/test/test_nodelattice.cpp | 12 +-- .../include/nav2_smoother/simple_smoother.hpp | 2 +- .../include/nav2_smoother/smoother_utils.hpp | 4 +- .../src/gps_navigation/tester.py | 2 +- .../src/system/test_multi_robot_launch.py | 2 +- nav2_system_tests/src/updown/README.md | 2 +- nav2_system_tests/src/updown/updownresults.py | 6 +- .../src/waypoint_follower/tester.py | 2 +- .../theta_star_planner.hpp | 2 +- .../include/nav2_util/lifecycle_utils.hpp | 4 +- nav2_util/include/nav2_util/node_utils.hpp | 2 +- .../include/nav2_util/odometry_utils.hpp | 2 +- .../nav2_util/simple_action_server.hpp | 2 +- .../include/nav2_util/validate_messages.hpp | 6 +- nav2_util/test/test_lifecycle_cli_node.cpp | 4 +- .../test/test_velocity_smoother.cpp | 2 +- nav2_voxel_grid/src/voxel_grid.cpp | 2 +- .../plugins/wait_at_waypoint.hpp | 2 +- .../waypoint_follower.hpp | 2 +- .../plugins/photo_at_waypoint.cpp | 2 +- tools/smoother_benchmarking/README.md | 2 +- tools/smoother_benchmarking/process_data.py | 2 +- 155 files changed, 312 insertions(+), 312 deletions(-) diff --git a/Doxyfile b/Doxyfile index aa34c7acb22..7fbf66c55a9 100644 --- a/Doxyfile +++ b/Doxyfile @@ -1451,8 +1451,8 @@ EXT_LINKS_IN_WINDOW = NO FORMULA_FONTSIZE = 10 -# Use the FORMULA_TRANPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are not +# Use the FORMULA_TRANSPARENT tag to determine whether or not the images +# generated for formulas are transparent ONGs. Transparent ONGs are not # supported properly for IE 6.0, but are supported on all modern browsers. # # Note that when changing this option you need to delete any form_*.png files in diff --git a/doc/requirements/requirements.md b/doc/requirements/requirements.md index 2cdf3f3f1c2..03382cdefba 100644 --- a/doc/requirements/requirements.md +++ b/doc/requirements/requirements.md @@ -199,7 +199,7 @@ ME005 | Mission Execution.Command Sequencing | 1 | The Mission Execution module ME006 | Mission Execution.Logging | 1 | The Mission Execution module SHOULD log its activity. | In case of forensic analysis of a safety event, for example. ME007 | Mission Execution.Feedback.Inputs.Error Recovery | 1 | Upon receipt of a downstream failure (unable to execute the Navigation Command), the Mission Execution module SHOULD attempt to recover and continue execution of the mission. ME008 | Mission Execution.Feedback.Outputs.Progress Notification | 1 | The Mission Execution module SHALL provide progress notifications on the execution of the mission. | Intermediate steps of interest. -ME009 | Mission Execution.Feedback.Outputs.Mission Completed | 1 | Upon successfull completion of the mission, the Mission Execution module SHALL output a corresponding notification. +ME009 | Mission Execution.Feedback.Outputs.Mission Completed | 1 | Upon successful completion of the mission, the Mission Execution module SHALL output a corresponding notification. ME010 | Mission Execution.Feedback.Outputs.Mission Canceled | 1 | Upon receiving a cancellation command and cancelling the mission, the Mission Execution module SHALL output a corresponding notification. ME011 | Mission Execution.Feedback.Outputs.Mission Failure | 1 | If the Mission Execution module is unable to execute the mission, it MUST output a failure notification. | This would be received by the user-level interface and could necessitate user intervention, such as having a remote operating center where the remote operator "rescues" the robot. ME012 | Mission Execution.Safe State Upon Failure | 1 | If the Mission Execution module is unable to execute the mission, it MUST direct the robot to a safe state. | The failure could be for a variety of reasons - sensor failures, algorithmic failure, a collision, etc. @@ -267,7 +267,7 @@ There are a few support modules and subsystems that are not part of the Navigati ### 2.4.1 Mapping -The map data format should be capable of describing typical indoor and outdoor environments encoutered by the robots. +The map data format should be capable of describing typical indoor and outdoor environments encountered by the robots. Id | Handle | Priority | Description | Notes -- | ------ | -------- | ----------- | ----- diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 0dd6166324e..2c971bc741c 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -112,7 +112,7 @@ class AmclNode : public nav2_util::LifecycleNode typedef struct { double weight; // Total weight (weights sum to 1) - pf_vector_t pf_pose_mean; // Mean of pose esimate + pf_vector_t pf_pose_mean; // Mean of pose estimate pf_matrix_t pf_pose_cov; // Covariance of pose estimate } amcl_hyp_t; diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index c4759d06b48..50aeca31a97 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf.hpp @@ -145,7 +145,7 @@ pf_t * pf_alloc( // Free an existing filter void pf_free(pf_t * pf); -// Initialize the filter using a guassian +// Initialize the filter using a gaussian void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov); // Initialize the filter using some model diff --git a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp index 8381e9b54b5..0d0f2756b8a 100644 --- a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp +++ b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp @@ -52,7 +52,7 @@ class Laser * @brief Run a sensor update on laser * @param pf Particle filter to use * @param data Laser data to use - * @return if it was succesful + * @return if it was successful */ virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0; @@ -123,7 +123,7 @@ class BeamModel : public Laser * @brief Run a sensor update on laser * @param pf Particle filter to use * @param data Laser data to use - * @return if it was succesful + * @return if it was successful */ bool sensorUpdate(pf_t * pf, LaserData * data); @@ -153,7 +153,7 @@ class LikelihoodFieldModel : public Laser * @brief Run a sensor update on laser * @param pf Particle filter to use * @param data Laser data to use - * @return if it was succesful + * @return if it was successful */ bool sensorUpdate(pf_t * pf, LaserData * data); @@ -162,7 +162,7 @@ class LikelihoodFieldModel : public Laser * @brief Perform the update function * @param data Laser data to use * @param pf Particle filter to use - * @return if it was succesful + * @return if it was successful */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); }; @@ -187,7 +187,7 @@ class LikelihoodFieldModelProb : public Laser * @brief Run a sensor update on laser * @param pf Particle filter to use * @param data Laser data to use - * @return if it was succesful + * @return if it was successful */ bool sensorUpdate(pf_t * pf, LaserData * data); @@ -196,7 +196,7 @@ class LikelihoodFieldModelProb : public Laser * @brief Perform the update function * @param data Laser data to use * @param pf Particle filter to use - * @return if it was succesful + * @return if it was successful */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); bool do_beamskip_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 5bf17738bdc..a590eee1f11 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1584,7 +1584,7 @@ AmclNode::initServices() void AmclNode::initOdometry() { - // TODO(mjeronimo): We should handle persistance of the last known pose of the robot. We could + // TODO(mjeronimo): We should handle persistence of the last known pose of the robot. We could // then read that pose here and initialize using that. // When pausing and resuming, remember the last robot pose so we don't start at 0:0 again diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 9d1f5a82892..980b4f9d979 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -67,7 +67,7 @@ pf_t * pf_alloc( // the max error between the true distribution and the estimated // distribution. [z] is the upper standard normal quantile for (1 - // p), where p is the probability that the error on the estimated - // distrubition will be less than [err]. + // distribution will be less than [err]. pf->pop_err = 0.01; pf->pop_z = 3; pf->dist_threshold = 0.5; @@ -123,7 +123,7 @@ void pf_free(pf_t * pf) free(pf); } -// Initialize the filter using a guassian +// Initialize the filter using a gaussian void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov) { int i; diff --git a/nav2_amcl/src/pf/pf_draw.c b/nav2_amcl/src/pf/pf_draw.c index b8e833f5135..f22c5671ef5 100644 --- a/nav2_amcl/src/pf/pf_draw.c +++ b/nav2_amcl/src/pf/pf_draw.c @@ -70,7 +70,7 @@ void pf_draw_samples(pf_t * pf, rtk_fig_t * fig, int max_samples) } -// Draw the hitogram (kd tree) +// Draw the histogram (kd tree) void pf_draw_hist(pf_t * pf, rtk_fig_t * fig) { pf_sample_set_t * set; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index e98db267210..295a4d98412 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -218,7 +218,7 @@ class BtActionServer // The blackboard shared by all of the nodes in the tree BT::Blackboard::Ptr blackboard_; - // The XML file that cointains the Behavior Tree to create + // The XML file that contains the Behavior Tree to create std::string current_bt_xml_filename_; std::string default_bt_xml_filename_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 91e3e241b91..0591f100b3b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -238,7 +238,7 @@ T1 deconflictPortAndParamFrame( /** * @brief Try reading an import port first, and if that doesn't work * fallback to reading directly the blackboard. - * The blackboard must be passed explitly because config() is private in BT.CPP 4.X + * The blackboard must be passed explicitly because config() is private in BT.CPP 4.X * * @param bt_node node * @param blackboard the blackboard ovtained with node->config().blackboard diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 67730ac51cd..e4feb921082 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -86,7 +86,7 @@ class DriveOnHeadingAction : public BtActionNode::SharedPtr client_; - // The timeout value while waiting for a responce from the + // The timeout value while waiting for a response from the // is path valid service std::chrono::milliseconds server_timeout_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index e532822d42a..24229fb85f1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp @@ -101,7 +101,7 @@ class IsStuckCondition : public BT::ConditionNode // Calculated states double current_accel_; - // Robot specific paramters + // Robot specific parameters double brake_accel_limit_; }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 5eae2516f74..cf48b4c5e32 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -1,7 +1,7 @@ @@ -171,31 +171,31 @@ Name of the topic to receive planner selection commands Default planner of the planner selector - Name of the selected planner received from the topic subcription + Name of the selected planner received from the topic subscription Name of the topic to receive controller selection commands Default controller of the controller selector - Name of the selected controller received from the topic subcription + Name of the selected controller received from the topic subscription Name of the topic to receive smoother selection commands Default smoother of the smoother selector - Name of the selected smoother received from the topic subcription + Name of the selected smoother received from the topic subscription Name of the topic to receive goal checker selection commands Default goal checker of the controller selector - Name of the selected goal checker received from the topic subcription + Name of the selected goal checker received from the topic subscription Name of the topic to receive progress checker selection commands Default progress checker of the controller selector - Name of the selected progress checker received from the topic subcription + Name of the selected progress checker received from the topic subscription diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index c5fa780a4c9..a8b90f8158e 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -25,7 +25,7 @@ DriveOnHeadingAction::DriveOnHeadingAction( const std::string & action_name, const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf), - initalized_(false) + initialized_(false) { } @@ -47,12 +47,12 @@ void DriveOnHeadingAction::initialize() goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); goal_.disable_collision_checks = disable_collision_checks; - initalized_ = true; + initialized_ = true; } void DriveOnHeadingAction::on_tick() { - if (!initalized_) { + if (!initialized_) { initialize(); } } diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index 626d0b1b15d..a9fd10dfc1f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -25,10 +25,10 @@ #include "utils/test_behavior_tree_fixture.hpp" -class RemoveInCollisionGoalsSucessService : public TestService +class RemoveInCollisionGoalsSuccessService : public TestService { public: - RemoveInCollisionGoalsSucessService() + RemoveInCollisionGoalsSuccessService() : TestService("/global_costmap/get_cost_global_costmap") {} @@ -113,7 +113,7 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test { tree_.reset(); } - static std::shared_ptr success_server_; + static std::shared_ptr success_server_; static std::shared_ptr failure_server_; protected: @@ -126,7 +126,7 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr; BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr; -std::shared_ptr +std::shared_ptr RemoveInCollisionGoalsTestFixture::success_server_ = nullptr; std::shared_ptr RemoveInCollisionGoalsTestFixture::failure_server_ = nullptr; @@ -237,7 +237,7 @@ int main(int argc, char ** argv) // initialize service and spin on new thread RemoveInCollisionGoalsTestFixture::success_server_ = - std::make_shared(); + std::make_shared(); std::thread success_server_thread([]() { rclcpp::spin(RemoveInCollisionGoalsTestFixture::success_server_); }); diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md index 7c87ae91a37..be0ee32461f 100644 --- a/nav2_behaviors/README.md +++ b/nav2_behaviors/README.md @@ -6,7 +6,7 @@ The package defines: - A `TimedBehavior` template which is used as a base class to implement specific timed behavior action server - but not required. - The `Backup`, `DriveOnHeading`, `Spin` and `Wait` behaviors. -The only required class a behavior must derive from is the `nav2_core/behavior.hpp` class, which implements the pluginlib interface the behavior server will use to dynamically load your behavior. The `nav2_behaviors/timed_behavior.hpp` derives from this class and implements a generic action server for a timed behavior behavior (e.g. calls an implmentation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `timed_behavior.hpp` helps in managing the complexity to simplify new behavior development, described more below. +The only required class a behavior must derive from is the `nav2_core/behavior.hpp` class, which implements the pluginlib interface the behavior server will use to dynamically load your behavior. The `nav2_behaviors/timed_behavior.hpp` derives from this class and implements a generic action server for a timed behavior behavior (e.g. calls an implementation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `timed_behavior.hpp` helps in managing the complexity to simplify new behavior development, described more below. The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface. diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 2e560fb7218..23a3640bcd5 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -198,7 +198,7 @@ class TimedBehavior : public nav2_core::Behavior std::string global_frame_; std::string robot_base_frame_; double transform_tolerance_; - rclcpp::Duration elasped_time_{0, 0}; + rclcpp::Duration elapsed_time_{0, 0}; // Clock rclcpp::Clock::SharedPtr clock_; @@ -236,7 +236,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { - elasped_time_ = clock_->now() - start_time; + elapsed_time_ = clock_->now() - start_time; // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping if (action_server_->is_preempt_requested()) { RCLCPP_ERROR( @@ -253,7 +253,7 @@ class TimedBehavior : public nav2_core::Behavior if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = elasped_time_; + result->total_elapsed_time = elapsed_time_; onActionCompletion(result); action_server_->terminate_all(result); return; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 9e85229a3c1..d53d8fd0d58 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -82,7 +82,7 @@ void AssistedTeleop::onActionCompletion(std::shared_ptrcurrent_teleop_duration = elasped_time_; + feedback_->current_teleop_duration = elapsed_time_; action_server_->publish_feedback(feedback_); rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index da45591352b..2e9b5b65e8a 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -35,7 +35,7 @@ ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={ #### Unique -There are two robots including name and intitial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up. +There are two robots including name and initial pose are hard-coded in the launch script. Two separated unique robots are required params file (`nav2_multirobot_params_1.yaml`, `nav2_multirobot_params_2.yaml`) for each robot to bring up. If you want to bringup more than two robots, you should modify the `unique_multi_tb3_simulation_launch.py` script. diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 42afb5c2cb5..909b81eb3e1 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -103,7 +103,7 @@ def generate_launch_description(): declare_container_name_cmd = DeclareLaunchArgument( 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition', + description='the name of container that nodes will load in if use composition', ) declare_use_respawn_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index a40b86327e3..987c398992c 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -108,7 +108,7 @@ def generate_launch_description(): declare_container_name_cmd = DeclareLaunchArgument( 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition', + description='the name of container that nodes will load in if use composition', ) declare_use_respawn_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 532d8ea24fa..5f8659f95b7 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -198,7 +198,7 @@ def generate_launch_description(): }.items(), ) # The SDF file for the world is a xacro file because we wanted to - # conditionally load the SceneBroadcaster plugin based on wheter we're + # conditionally load the SceneBroadcaster plugin based on whether we're # running in headless mode. But currently, the Gazebo command line doesn't # take SDF strings for worlds, so the output of xacro needs to be saved into # a temporary file and passed to Gazebo. diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 0765ceb4341..5930a1b73b8 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -200,7 +200,7 @@ def generate_launch_description(): ) # The SDF file for the world is a xacro file because we wanted to - # conditionally load the SceneBroadcaster plugin based on wheter we're + # conditionally load the SceneBroadcaster plugin based on whether we're # running in headless mode. But currently, the Gazebo command line doesn't # take SDF strings for worlds, so the output of xacro needs to be saved into # a temporary file and passed to Gazebo. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index a468be7c880..2476b17d886 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -27,7 +27,7 @@ namespace nav2_collision_monitor { /** - * @brief Circle shape implementaiton. + * @brief Circle shape implementation. * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot * while for APPROACH model it represents robot footprint. */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 6bb3b5dd011..b81571765b9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -249,7 +249,7 @@ class Polygon /** * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...] * @param poly_string Input String containing the verteceis of the polygon - * @param polygon Output Point vector with all the vertecies of the polygon + * @param polygon Output Point vector with all the vertices of the polygon * @return True if all parameters were obtained or false in failure case */ bool getPolygonFromString(std::string & poly_string, std::vector & polygon); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp index e4c65bebad5..d93ebd931d2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp @@ -52,7 +52,7 @@ class VelocityPolygon : public Polygon virtual ~VelocityPolygon(); /** - * @brief Overriden getParameters function for VelocityPolygon parameters + * @brief Overridden getParameters function for VelocityPolygon parameters * @param polygon_sub_topic Not used in VelocityPolygon * @param polygon_pub_topic Output name of polygon publishing topic * @param footprint_topic Not used in VelocityPolygon @@ -63,14 +63,14 @@ class VelocityPolygon : public Polygon std::string & /*footprint_topic*/) override; /** - * @brief Overriden updatePolygon function for VelocityPolygon + * @brief Overridden updatePolygon function for VelocityPolygon * @param cmd_vel_in Robot twist command input */ void updatePolygon(const Velocity & cmd_vel_in) override; protected: /** - * @brief Custom struc to store the parameters of the sub-polygon + * @brief Custom struct to store the parameters of the sub-polygon * @param poly_ The points of the sub-polygon * @param velocity_polygon_name_ The name of the sub-polygon * @param linear_min_ The minimum linear velocity diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index c16bec55a4d..548aca4e2bb 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): declare_container_name_cmd = DeclareLaunchArgument( 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition', + description='the name of container that nodes will load in if use composition', ) configured_params = RewrittenYaml( diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index b00b43b2aa1..3dd9d01a0d4 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): declare_container_name_cmd = DeclareLaunchArgument( 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition', + description='the name of container that nodes will load in if use composition', ) configured_params = ParameterFile( diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2b0991c9941..c336c59782c 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -582,7 +582,7 @@ bool CollisionMonitor::processApproach( // Obtain time before a collision const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity); if (collision_time >= 0.0) { - // If collision will occurr, reduce robot speed + // If collision will occur, reduce robot speed const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); const Velocity safe_vel = velocity * change_ratio; // Check that currently calculated velocity is safer than diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index c7aafd06ace..e6e10b08639 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -64,7 +64,7 @@ void Source::getCommonParameters(std::string & source_topic) nav2_util::declare_parameter_if_not_declared( node, source_name_ + ".topic", - rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner + rclcpp::ParameterValue("scan")); // Set default topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); nav2_util::declare_parameter_if_not_declared( diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 1e8c0bf013a..0de57bd895e 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1231,7 +1231,7 @@ TEST_F(Tester, testSourceTimeoutOverride) // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s double change_ratio = (1.5 / 3.0) / TIME_BEFORE_COLLISION; // Range configured but not published, range source should be considered invalid - // but as we set the source_timeout of the Range source to 0.0, its validity check is overidden + // but as we set the source_timeout of the Range source to 0.0, its validity check is overridden ASSERT_NEAR( cmd_vel_out_->linear.x, 3.0 * change_ratio, 3.0 * SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); @@ -1530,12 +1530,12 @@ TEST_F(Tester, testVelocityPolygonStop) // 1. Forward: 0 -> 0.5 m/s // 2. Backward: 0 -> -0.5 m/s setCommonParameters(); - addPolygon("VelocityPoylgon", VELOCITY_POLYGON, 1.0, "stop"); - addPolygonVelocitySubPolygon("VelocityPoylgon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0); - addPolygonVelocitySubPolygon("VelocityPoylgon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0); - setPolygonVelocityVectors("VelocityPoylgon", {"Forward", "Backward"}); + addPolygon("VelocityPolygon", VELOCITY_POLYGON, 1.0, "stop"); + addPolygonVelocitySubPolygon("VelocityPolygon", "Forward", 0.0, 0.5, 0.0, 1.0, 4.0); + addPolygonVelocitySubPolygon("VelocityPolygon", "Backward", -0.5, 0.0, 0.0, 1.0, 2.0); + setPolygonVelocityVectors("VelocityPolygon", {"Forward", "Backward"}); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"VelocityPoylgon"}, {POINTCLOUD_NAME}); + setVectors({"VelocityPolygon"}, {POINTCLOUD_NAME}); rclcpp::Time curr_time = cm_->now(); // Start Collision Monitor node @@ -1562,7 +1562,7 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPoylgon"); + ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon"); // 3. Switch to Backward velocity polygon // Obstacle is far away from Backward velocity polygon @@ -1585,7 +1585,7 @@ TEST_F(Tester, testVelocityPolygonStop) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, STOP); - ASSERT_EQ(action_state_->polygon_name, "VelocityPoylgon"); + ASSERT_EQ(action_state_->polygon_name, "VelocityPolygon"); // Stop Collision Monitor node cm_->stop(); diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index a0dbf9fcc96..7273f90857c 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -738,7 +738,7 @@ TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) polygon_->getPolygon(poly); ASSERT_EQ(poly.size(), 0u); - // Publush polygon in different frame and make shure that it was set correctly + // Publish polygon in different frame and make sure that it was set correctly test_node_->publishPolygon(BASE2_FRAME_ID, true); ASSERT_TRUE(waitPolygon(500ms, poly)); ASSERT_EQ(poly.size(), 4u); @@ -778,7 +778,7 @@ TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame) polygon_->getPolygon(poly); ASSERT_EQ(poly.size(), 0u); - // Publush polygon in incorrect frame and check that polygon was not updated + // Publish polygon in incorrect frame and check that polygon was not updated test_node_->publishPolygon("incorrect_frame", true); ASSERT_FALSE(waitPolygon(100ms, poly)); } diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 8b9ecec6d75..bde3d9989ad 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -434,13 +434,13 @@ TEST_F(Tester, testVelocityPolygonOutOfRangeVelocity) ASSERT_EQ(poly.size(), 0u); - // Publish out of range cmd_vel(linear) and check that polygon is still emtpy + // Publish out of range cmd_vel(linear) and check that polygon is still empty nav2_collision_monitor::Velocity vel{0.6, 0.0, 0.0}; velocity_polygon_->updatePolygon(vel); ASSERT_FALSE(waitPolygon(500ms, poly)); ASSERT_EQ(poly.size(), 0u); - // Publish out of range cmd_vel(rotation) and check that polygon is still emtpy + // Publish out of range cmd_vel(rotation) and check that polygon is still empty vel = {0.3, 0.0, 1.5}; velocity_polygon_->updatePolygon(vel); ASSERT_FALSE(waitPolygon(500ms, poly)); diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py index bb1725d36de..55cf7093deb 100644 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -33,7 +33,7 @@ def __init__(self, target_argument: Text): `target_argument` shall be 'robots'. Then, this will parse a string value for `robots` argument. - Each robot name which is corresponding to namespace and pose of it will be separted by `;`. + Each robot name which is corresponding to namespace and pose of it will be separated by `;`. The pose consists of x, y and yaw with YAML format. :param: target argument name to parse diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp index fb4e2aa1307..dd754fa7a4c 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp @@ -112,7 +112,7 @@ inline Eigen::Matrix tangentDir( return result; } - // tangent is prependicular to (pt - center) + // tangent is perpendicular to (pt - center) // Note: not determining + or - direction here, this should be handled at the caller side return Eigen::Matrix(center[1] - pt[1], pt[0] - center[0]); } diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 1ca6461e9a9..c4bb7e52740 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -378,7 +378,7 @@ class SmootherTest : public ::testing::Test { auto output = input; for (size_t i = 1; i < input.size() - 1; i++) { - // add offset prependicular to path + // add offset perpendicular to path Eigen::Vector2d direction = (input[i + 1].block<2, 1>(0, 0) - input[i - 1].block<2, 1>(0, 0)).normalized(); output[i].block<2, 1>( diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp index 3db219eaae0..48eb580c964 100644 --- a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp @@ -56,7 +56,7 @@ class PoseProgressChecker : public SimpleProgressChecker std::string plugin_name_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index e9091db9ae8..8bb3d9a949f 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -81,7 +81,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker std::string plugin_name_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index b86a7018e57..f7b9707711a 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -71,7 +71,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker std::string plugin_name_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index d7586db6f31..e2f6ee0a465 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -73,7 +73,7 @@ class StoppedGoalChecker : public SimpleGoalChecker std::string plugin_name_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 93e66539392..57003871a4e 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -249,7 +249,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) return nav2_util::CallbackReturn::FAILURE; } - // Set subscribtion to the speed limiting topic + // Set subscription to the speed limiting topic speed_limit_sub_ = create_subscription( speed_limit_topic, rclcpp::QoS(10), std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1)); diff --git a/nav2_core/include/nav2_core/behavior.hpp b/nav2_core/include/nav2_core/behavior.hpp index 43ef9adf3c4..ee18146473c 100644 --- a/nav2_core/include/nav2_core/behavior.hpp +++ b/nav2_core/include/nav2_core/behavior.hpp @@ -71,7 +71,7 @@ class Behavior virtual void activate() = 0; /** - * @brief Method to deactive Behavior and any threads involved in execution. + * @brief Method to deactivate Behavior and any threads involved in execution. */ virtual void deactivate() = 0; diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index b01381a0414..f9adaef0ac1 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -88,7 +88,7 @@ class Controller virtual void activate() = 0; /** - * @brief Method to deactive planner and any threads involved in execution. + * @brief Method to deactivate planner and any threads involved in execution. */ virtual void deactivate() = 0; diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index 0eff064a5d2..691acaa2340 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -63,7 +63,7 @@ class GlobalPlanner virtual void activate() = 0; /** - * @brief Method to deactive planner and any threads involved in execution. + * @brief Method to deactivate planner and any threads involved in execution. */ virtual void deactivate() = 0; diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index d6c60ea915f..a794f6430ff 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -1,6 +1,6 @@ # Nav2 Costmap_2d -The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. +The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal notable changes necessary due to support in ROS2. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://docs.nav2.org/tutorials/index.html) and [first-time setup guides](https://docs.nav2.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 3d71c501ece..3f514e9ad26 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -421,7 +421,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode OnSetParametersCallbackHandle::SharedPtr dyn_params_handler; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp index 0a813607836..86a7e73669c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp @@ -109,7 +109,7 @@ class BinaryFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; - std::string global_frame_; // Frame of currnet layer (master_grid) + std::string global_frame_; // Frame of current layer (master_grid) double base_, multiplier_; // Filter values higher than this threshold, diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index 2d7d9bf4aa4..e2225196d72 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -101,7 +101,7 @@ class KeepoutFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; - std::string global_frame_; // Frame of currnet layer (master_grid) + std::string global_frame_; // Frame of current layer (master_grid) }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index c9fdf9f0fa7..cbce0e8767d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -103,7 +103,7 @@ class SpeedFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; - std::string global_frame_; // Frame of currnet layer (master_grid) + std::string global_frame_; // Frame of current layer (master_grid) double base_, multiplier_; bool percentage_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 0ae010e94b8..b75ac555c98 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -185,7 +185,7 @@ class InflationLayer : public Layer typedef std::recursive_mutex mutex_t; /** - * @brief Get the mutex of the inflation inforamtion + * @brief Get the mutex of the inflation information */ mutex_t * getMutex() { diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp index eb1beaa11a7..15c94ee7796 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp @@ -216,7 +216,7 @@ class LayeredCostmap // combined_costmap_ is a final costmap where all results produced by plugins and filters (if any) // to be merged. // The separation is aimed to avoid interferences of work between plugins and filters. - // primay_costmap_ and combined_costmap_ have the same sizes, origins and default values. + // primary_costmap_ and combined_costmap_ have the same sizes, origins and default values. Costmap2D primary_costmap_, combined_costmap_; std::string global_frame_; diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 7021bfb4595..51c018ae427 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -646,7 +646,7 @@ ObstacleLayer::raytraceFreespace( return; } - // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later double origin_x = origin_x_, origin_y = origin_y_; double map_end_x = origin_x + size_x_ * resolution_; double map_end_y = origin_y + size_y_ * resolution_; @@ -663,7 +663,7 @@ ObstacleLayer::raytraceFreespace( double wx = *iter_x; double wy = *iter_y; - // now we also need to make sure that the enpoint we're raytracing + // now we also need to make sure that the endpoint we're raytracing // to isn't off the costmap and scale if necessary double a = wx - ox; double b = wy - oy; diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index dc28d4f4342..a7992726a98 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -523,7 +523,7 @@ void RangeSensorLayer::updateCosts( void RangeSensorLayer::reset() { - RCLCPP_DEBUG(logger_, "Reseting range sensor layer..."); + RCLCPP_DEBUG(logger_, "Resetting range sensor layer..."); deactivate(); resetMaps(); was_reset_ = true; diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index c4344d9f0e7..afd1ad06459 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -319,7 +319,7 @@ void VoxelLayer::raytraceFreespace( sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_y(*clearing_endpoints_, "y"); sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_z(*clearing_endpoints_, "z"); - // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_y = origin_y_ + getSizeInMetersY(); double map_end_z = origin_z_ + getSizeInMetersZ(); @@ -422,7 +422,7 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) cell_oy = static_cast((new_origin_y - origin_y_) / resolution_); // compute the associated world coordinates for the origin cell - // beacuase we want to keep things grid-aligned + // because we want to keep things grid-aligned double new_grid_ox, new_grid_oy; new_grid_ox = origin_x_ + cell_ox * resolution_; new_grid_oy = origin_y_ + cell_oy * resolution_; diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 9ddbcf5e9d6..bfa29e66e3f 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -206,7 +206,7 @@ bool Costmap2D::copyWindow( Costmap2D & Costmap2D::operator=(const Costmap2D & map) { - // check for self assignement + // check for self assignment if (this == &map) { return *this; } @@ -555,9 +555,9 @@ bool Costmap2D::saveMap(std::string file_name) } fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff); - for (unsigned int iy = 0; iy < size_y_; iy++) { + for (unsigned int it = 0; it < size_y_; iy++) { for (unsigned int ix = 0; ix < size_x_; ix++) { - unsigned char cost = getCost(ix, iy); + unsigned char cost = getCost(ix, it); fprintf(fp, "%d ", cost); } fprintf(fp, "\n"); diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index bc1b14da786..10c616d1df5 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -111,7 +111,7 @@ Costmap2DPublisher::Costmap2DPublisher( Costmap2DPublisher::~Costmap2DPublisher() {} -// TODO(bpwilcox): find equivalent/workaround to ros::SingleSubscriberPublishr +// TODO(bpwilcox): find equivalent/workaround to ros::SingleSubscriberPublisher /* void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& pub) { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index a35a1786bb4..0551d1625e5 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -425,7 +425,7 @@ Costmap2DROS::getParameters() filter_types_[i] = nav2_util::get_plugin_type_param(node, filter_names_[i]); } - // 2. The map publish frequency cannot be 0 (to avoid a divde-by-zero) + // 2. The map publish frequency cannot be 0 (to avoid a divide-by-zero) if (map_publish_frequency_ > 0) { publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); } else { diff --git a/nav2_costmap_2d/test/integration/costmap_tester.cpp b/nav2_costmap_2d/test/integration/costmap_tester.cpp index 59676e23596..66f80d11bb1 100644 --- a/nav2_costmap_2d/test/integration/costmap_tester.cpp +++ b/nav2_costmap_2d/test/integration/costmap_tester.cpp @@ -115,7 +115,7 @@ void CostmapTester::compareCells( if (cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE) { // if the cell is a lethal obstacle, - // then we know that all its neighbors should have equal or slighlty less cost + // then we know that all its neighbors should have equal or slightly less cost unsigned char expected_lowest_cost = 0; EXPECT_TRUE( neighbor_cost >= expected_lowest_cost || diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index b88c3736886..816805a9f7a 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -264,7 +264,7 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u); } -TEST_F(TestNode, testInflationInUnkown) +TEST_F(TestNode, testInflationInUnknown) { std::vector parameters; // Set cost_scaling_factor parameter to 1.0 for inflation layer @@ -299,7 +299,7 @@ TEST_F(TestNode, testInflationInUnkown) EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 4u); } -TEST_F(TestNode, testInflationAroundUnkown) +TEST_F(TestNode, testInflationAroundUnknown) { auto inflation_radius = 4.1; std::vector parameters; diff --git a/nav2_costmap_2d/test/integration/range_tests.cpp b/nav2_costmap_2d/test/integration/range_tests.cpp index 1ee5380b7a2..4541ee16e7e 100644 --- a/nav2_costmap_2d/test/integration/range_tests.cpp +++ b/nav2_costmap_2d/test/integration/range_tests.cpp @@ -187,7 +187,7 @@ TEST_F(TestNode, testClearingAtMaxRange) { } // Testing fixed scan with robot forward motion -TEST_F(TestNode, testProbabalisticModelForward) { +TEST_F(TestNode, testProbabilisticModelForward) { geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node_->now(); transform.header.frame_id = "frame"; @@ -240,7 +240,7 @@ TEST_F(TestNode, testProbabalisticModelForward) { } // Testing fixed motion with downward movement -TEST_F(TestNode, testProbabalisticModelDownward) { +TEST_F(TestNode, testProbabilisticModelDownward) { geometry_msgs::msg::TransformStamped transform; transform.header.stamp = node_->now(); transform.header.frame_id = "frame"; diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 849edf282c7..d7d23e8a52d 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -127,7 +127,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) bool always_send_full_costmap = true; std::vector> expectedCostmaps; - std::vector> recievedCostmaps; + std::vector> receivedCostmaps; auto costmapPublisher = std::make_shared( node, costmapToSend.get(), "", topicName, always_send_full_costmap); @@ -145,13 +145,13 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) rclcpp::spin_some(node->get_node_base_interface()); - recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + receivedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); } ASSERT_EQ(fullCostmapMsgCount, mapChanges.size()); ASSERT_EQ(updateCostmapMsgCount, 0); - ASSERT_EQ(expectedCostmaps, recievedCostmaps); + ASSERT_EQ(expectedCostmaps, receivedCostmaps); costmapPublisher->on_deactivate(); } @@ -161,7 +161,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) bool always_send_full_costmap = false; std::vector> expectedCostmaps; - std::vector> recievedCostmaps; + std::vector> receivedCostmaps; auto costmapPublisher = std::make_shared( node, costmapToSend.get(), "", topicName, always_send_full_costmap); @@ -179,13 +179,13 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) rclcpp::spin_some(node->get_node_base_interface()); - recievedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); + receivedCostmaps.emplace_back(getCurrentCharMapFromSubscriber()); } ASSERT_EQ(fullCostmapMsgCount, 1); ASSERT_EQ(updateCostmapMsgCount, mapChanges.size() - 1); - ASSERT_EQ(expectedCostmaps, recievedCostmaps); + ASSERT_EQ(expectedCostmaps, receivedCostmaps); costmapPublisher->on_deactivate(); } diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 7a5fe811e41..5f7ea14d192 100644 --- a/nav2_costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -162,7 +162,7 @@ TEST(costmap, testResetForStaticMap) { } ASSERT_EQ(hitCount, 36); - // Veriy that we have 64 non-leathal + // Veriy that we have 64 non-lethal hitCount = 0; for (unsigned int i = 0; i < 10; ++i) { for (unsigned int j = 0; j < 10; ++j) { @@ -356,7 +356,7 @@ TEST(costmap, testWindowCopy) { ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6); ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6); - // check that we actually get the windo that we expect + // check that we actually get the window that we expect for (unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i) { for (unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j) { ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2)); @@ -1092,7 +1092,7 @@ TEST(costmap, testRaytracing2) { } /** - * Within a certian radius of the robot, the cost map most propagate obstacles. This + * Within a certain radius of the robot, the cost map most propagate obstacles. This * is to avoid a case where a hit on a far obstacle clears inscribed radius around a * near one. */ diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 3cdc10aecb7..291ffb062f9 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -3,8 +3,8 @@ target_link_libraries(collision_footprint_test nav2_costmap_2d_core ) -ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) -target_link_libraries(costmap_convesion_test +ament_add_gtest(costmap_conversion_test costmap_conversion_test.cpp) +target_link_libraries(costmap_conversion_test nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index fa7fed4e107..e1fda3bbbe1 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -675,7 +675,7 @@ void TestNode::testResetFilter() binary_state = waitBinaryState(); verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state); - // Reset binary filter and check its state was resetted to default + // Reset binary filter and check its state was reset to default binary_filter_->resetFilter(); binary_state = waitBinaryState(); ASSERT_TRUE(binary_state != nullptr); diff --git a/nav2_docking/README.md b/nav2_docking/README.md index 2a9ba502ae9..f5259338c76 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -167,7 +167,7 @@ return true if `isCharging` returns true. The `SimpleChargingDock` and `SimpleNonChargingDock` plugins are examples with many common options which may be fully functional for some robots. -`getStagingPose` applys a parameterized translational and rotational offset to the dock pose to obtain the staging pose. +`getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose. `getRefinedPose` can be used in two ways. 1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. @@ -194,7 +194,7 @@ For debugging purposes, there are several publishers which can be used with RVIZ | Parameter | Description | Type | Default | |------------------------------|---------------------------------------------------------|--------|-----------| | controller_frequency | Control frequency (Hz) for vision-control loop | double | 50.0 | -| initial_perception_timeout | Timeout (s) to wait to obtain intial perception of the dock | double | 5.0 | +| initial_perception_timeout | Timeout (s) to wait to obtain initial perception of the dock | double | 5.0 | | wait_charge_timeout | Timeout (s) to wait to see if charging starts after docking | double | 5.0 | | dock_approach_timeout | timeout (s) to attempt vision-control approach loop | double | 30.0 | | undock_linear_tolerance | Tolerance (m) to exit the undocking control loop at staging pose | double | 0.05 | diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 9e03c5b154c..cd2ad0c5803 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -95,7 +95,7 @@ class DockingServer : public nav2_util::LifecycleNode /** * @brief Wait for charging to begin. * @param dock Dock instance, used to query isCharging(). - * @returns True if charging successfully started within alloted time. + * @returns True if charging successfully started within allotted time. */ bool waitForCharge(Dock * dock); diff --git a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp index 99f8359b5e1..c18bed48adc 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp @@ -30,7 +30,7 @@ class PoseFilter /** * @brief Create a pose filter instance. * @param coef Filtering coefficient. Valid range is 0-1, where 0 means take the new measurement - * @param timeout If time between measurments exceeds this value, take the new measurement. + * @param timeout If time between measurements exceeds this value, take the new measurement. */ PoseFilter(double coef, double timeout); diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 49adf4124c9..c9b46e4a400 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -61,7 +61,7 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock virtual void activate() {} /** - * @brief Method to deactive Behavior and any threads involved in execution. + * @brief Method to deactivate Behavior and any threads involved in execution. */ virtual void deactivate() {} diff --git a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp index 0f258289448..cecb5299150 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp @@ -61,7 +61,7 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock virtual void activate() {} /** - * @brief Method to deactive Behavior and any threads involved in execution. + * @brief Method to deactivate Behavior and any threads involved in execution. */ virtual void deactivate() {} diff --git a/nav2_docking/opennav_docking/test/test_pose_filter.cpp b/nav2_docking/opennav_docking/test/test_pose_filter.cpp index 2997a21c5aa..f010c9c0108 100644 --- a/nav2_docking/opennav_docking/test/test_pose_filter.cpp +++ b/nav2_docking/opennav_docking/test/test_pose_filter.cpp @@ -43,7 +43,7 @@ TEST(PoseFilterTests, FilterTests) // Update filter geometry_msgs::msg::PoseStamped pose = filter.update(meas1); - // Header frame_id is inconsistent, so pose = measurment + // Header frame_id is inconsistent, so pose = measurement EXPECT_NEAR(pose.pose.position.x, 1.0, 0.0001); EXPECT_NEAR(pose.pose.position.y, 3.0, 0.0001); EXPECT_NEAR(pose.pose.position.z, 5.0, 0.0001); diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index 886fde2fa7a..902323ae805 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -60,7 +60,7 @@ class ChargingDock virtual void activate() = 0; /** - * @brief Method to deactive Behavior and any threads involved in execution. + * @brief Method to deactivate Behavior and any threads involved in execution. */ virtual void deactivate() = 0; diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp index 9951b19dff9..c0efc0d0cc9 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/non_charging_dock.hpp @@ -58,7 +58,7 @@ class NonChargingDock : public ChargingDock virtual void activate() = 0; /** - * @brief Method to deactive Behavior and any threads involved in execution. + * @brief Method to deactivate Behavior and any threads involved in execution. */ virtual void deactivate() = 0; diff --git a/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp b/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp index 66f9f84afce..0ac987cf069 100644 --- a/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp +++ b/nav2_dwb_controller/costmap_queue/include/costmap_queue/costmap_queue.hpp @@ -96,7 +96,7 @@ class CellData * how far each cell is from an obstacle, and is also used in a number of Trajectory cost functions. * * It is implemented with a queue. The standard operation is to enqueueCell the original set, and then - * retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell + * retrieve the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell * returns an object that contains the coordinates of this cell and the origin cell, as well as * the distance between them. By default, the Euclidean distance is used for ordering, but passing in * manhattan=true to the constructor will use the Manhattan distance. @@ -138,7 +138,7 @@ class CostmapQueue : public MapBasedQueue /** * @brief Check to see if we should add this cell to the queue. Always true unless overridden. * @param cell The cell to check - * @return True, unless overriden + * @return True, unless overridden */ virtual bool validCellToQueue(const CellData & /*cell*/) {return true;} /** diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp index 611aad46445..df6f77e7725 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp @@ -121,14 +121,14 @@ class MapGridCritic : public dwb_core::TrajectoryCritic }; /** - * @brief Clear the queuDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore + * @brief Clear the queueDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore */ void reset() override; /** * @brief Go through the queue and set the cells to the Manhattan distance from their parents */ - void propogateManhattanDistances(); + void propagateManhattanDistances(); std::shared_ptr queue_; nav2_costmap_2d::Costmap2D * costmap_; diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp index aa75cbc39af..a859c392e50 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/obstacle_footprint.hpp @@ -60,7 +60,7 @@ Footprint getOrientedFootprint( * efficiency. This is valid if the obstacles in the local costmap are inflated. * * A more robust class could check every cell within the robot's footprint without inflating the obstacles, - * at some computational cost. That is left as an excercise to the reader. + * at some computational cost. That is left as an exercise to the reader. */ class ObstacleFootprintCritic : public BaseObstacleCritic { diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp index 4198ddeb766..405dbc14723 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/rotate_to_goal.hpp @@ -59,7 +59,7 @@ namespace dwb_critics * score trajectories that have linear movement as invalid and score the rest based on the result of the * scoreRotation method * - * The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance + * The scoreRotation method can be overridden, but the default behavior is to return the shortest angular distance * between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter. * * If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory, * which is the same as DWA's behavior. This is the default. diff --git a/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp b/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp index bb0fce406ae..b70f94353df 100644 --- a/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp +++ b/nav2_dwb_controller/dwb_critics/src/goal_dist.cpp @@ -57,7 +57,7 @@ bool GoalDistCritic::prepare( cell_values_[index] = 0.0; queue_->enqueueCell(local_goal_x, local_goal_y); - propogateManhattanDistances(); + propagateManhattanDistances(); return true; } diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp index 030e77cd637..6dae0889da0 100644 --- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp +++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp @@ -60,7 +60,7 @@ void MapGridCritic::onInit() costmap_ = costmap_ros_->getCostmap(); queue_ = std::make_shared(*costmap_, *this); - // Always set to true, but can be overriden by subclasses + // Always set to true, but can be overridden by subclasses stop_on_failure_ = true; auto node = node_.lock(); @@ -105,7 +105,7 @@ void MapGridCritic::reset() std::fill(cell_values_.begin(), cell_values_.end(), unreachable_score_); } -void MapGridCritic::propogateManhattanDistances() +void MapGridCritic::propagateManhattanDistances() { while (!queue_->isEmpty()) { costmap_queue::CellData cell = queue_->getNextCell(); diff --git a/nav2_dwb_controller/dwb_critics/src/path_dist.cpp b/nav2_dwb_controller/dwb_critics/src/path_dist.cpp index be8896b5999..3ba6a173436 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_dist.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_dist.cpp @@ -85,7 +85,7 @@ bool PathDistCritic::prepare( return false; } - propogateManhattanDistances(); + propagateManhattanDistances(); return true; } diff --git a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp index d3e18e2acbe..5c400c47d5a 100644 --- a/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/obstacle_footprint_test.cpp @@ -71,7 +71,7 @@ geometry_msgs::msg::Point rotate_origin(geometry_msgs::msg::Point p, double angl return p; } -// Auxilary function to create a Point with given x and y values. +// Auxiliary function to create a Point with given x and y values. geometry_msgs::msg::Point getPoint(double x, double y) { geometry_msgs::msg::Point p; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index a87376e718c..d462e26f538 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -156,7 +156,7 @@ class KinematicsHandler // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp index 10a4aab379a..87f2ae6e17e 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.hpp @@ -48,7 +48,7 @@ const double EPSILON = 1E-5; * * @param v0 Initial velocity * @param accel The acceleration rate - * @param decel The decceleration rate + * @param decel The deceleration rate * @param dt Delta time - amount of time to project into the future * @param target target velocity * @return The velocity dt seconds after v0. diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 24f1b851e7e..0154550682e 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -4,7 +4,7 @@ The graceful motion controller implements a controller based on the works of Jon See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law -The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: +The smooth control law is a pose-following kinematic control law that generates a smooth and comfortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: * The egocentric polar coordinates of the motion target (r, phi, delta) with respect to the robot frame. * A slow subsystem which describes the position of the robot. * A fast subsystem which describes the steering angle of the robot. diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 0728df7d071..a42350d5c06 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -80,7 +80,7 @@ class LifecycleManager : public rclcpp::Node * @brief Lifecycle node manager callback function * @param request_header Header of the service request * @param request Service request - * @param reponse Service response + * @param response Service response */ void managerCallback( const std::shared_ptr request_header, @@ -91,7 +91,7 @@ class LifecycleManager : public rclcpp::Node * state. * @param request_header Header of the request * @param request Service request - * @param reponse Service response + * @param response Service response */ void isActiveCallback( const std::shared_ptr request_header, diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 4000ddfb16e..6f75ef442cc 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -41,7 +41,7 @@ This is a loopback simulator that replaces a physics simulator to create a frictionless, inertialess, and collisionless simulation environment. It accepts cmd_vel messages and publishes odometry & TF messages based on the -cumulative velocities received to mimick global localization and simulation. +cumulative velocities received to mimic global localization and simulation. It also accepts initialpose messages to set the initial pose of the robot to place anywhere. """ diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp index df36f94408d..f4b0069ef79 100644 --- a/nav2_map_server/test/component/test_map_saver_node.cpp +++ b/nav2_map_server/test/component/test_map_saver_node.cpp @@ -117,7 +117,7 @@ TEST_F(MapSaverTestFixture, SaveMap) RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); ASSERT_TRUE(client->wait_for_service()); - // 1. Send valid save_map serivce request + // 1. Send valid save_map service request req->map_topic = "map"; req->map_url = path(g_tmp_dir) / path(g_valid_map_name); req->image_format = "png"; @@ -146,7 +146,7 @@ TEST_F(MapSaverTestFixture, SaveMapDefaultParameters) RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); ASSERT_TRUE(client->wait_for_service()); - // 1. Send save_map serivce request with default parameters + // 1. Send save_map service request with default parameters req->map_topic = ""; req->map_url = path(g_tmp_dir) / path(g_valid_map_name); req->image_format = ""; @@ -176,7 +176,7 @@ TEST_F(MapSaverTestFixture, SaveMapInvalidParameters) RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); ASSERT_TRUE(client->wait_for_service()); - // 1. Trying to send save_map serivce request with different sets of parameters + // 1. Trying to send save_map service request with different sets of parameters // In case of map is expected to be saved correctly, verify it req->map_topic = "invalid_map"; req->map_url = path(g_tmp_dir) / path(g_valid_map_name); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index c06fba5b896..1ccd08c32fe 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -27,7 +27,7 @@ namespace mppi::critics * @class mppi::critics::ConstraintCritic * @brief Critic objective function for following the path approximately * To allow for deviation from path in case of dynamic obstacles. Path Align - * is what aligns the trajectories to the path more or less precisely, if desireable. + * is what aligns the trajectories to the path more or less precisely, if desirable. * A higher weight here with an offset > 1 will accelerate the samples to full speed * faster and push the follow point further ahead, creating some shortcutting. */ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 6e1c271d94d..a985da7cb75 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -226,7 +226,7 @@ class Optimizer bool isHolonomic() const; /** - * @brief Using control frequence and time step size, determine if trajectory + * @brief Using control frequencies and time step size, determine if trajectory * offset should be used to populate initial state of the next cycle */ void setOffset(double controller_frequency); diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp index 43bcc0b540c..4445186c24c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp @@ -77,7 +77,7 @@ class ParametersHandler std::vector parameters); /** - * @brief Get an object to retreive parameters + * @brief Get an object to retrieve parameters * @param ns Namespace to get parameters within * @return Parameter getter object */ @@ -119,7 +119,7 @@ class ParametersHandler /** * @brief register a function to be called when setting a parameter * - * The callback funciton is expected to behave as follows. + * The callback function is expected to behave as follows. * Successful parameter changes should not interfere with * the result parameter. * Unsuccessful parameter changes should set the result.successful = false diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 4ad0dbfbd78..72fbaeb6011 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -204,7 +204,7 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) * @brief Check if the robot pose is within the Goal Checker's tolerances to goal * @param global_checker Pointer to the goal checker * @param robot Pose of robot - * @param path Path to retreive goal pose from + * @param path Path to retrieve goal pose from * @return bool If robot is within goal checker tolerances to the goal */ inline bool withinPositionGoalTolerance( @@ -240,7 +240,7 @@ inline bool withinPositionGoalTolerance( * @brief Check if the robot pose is within tolerance to the goal * @param pose_tolerance Pose tolerance to use * @param robot Pose of robot - * @param path Path to retreive goal pose from + * @param path Path to retrieve goal pose from * @return bool If robot is within tolerance to the goal */ inline bool withinPositionGoalTolerance( @@ -284,11 +284,11 @@ auto normalize_angles(const T & angles) * @brief shortest_angular_distance * * Given 2 angles, this returns the shortest angular - * difference. The inputs and ouputs are of course radians. + * difference. The inputs and outputs are of course radians. * * The result * would always be -pi <= result <= pi. Adding the result - * to "from" will always get you an equivelent angle to "to". + * to "from" will always get you an equivalent angle to "to". * @param from Start angle * @param to End angle * @return Shortest distance between angles @@ -303,7 +303,7 @@ auto shortest_angular_distance( /** * @brief Evaluate furthest point idx of data.path which is - * nearset to some trajectory in data.trajectories + * nearest to some trajectory in data.trajectories * @param data Data to use * @return Idx of furthest path point reached by a set of trajectories */ @@ -552,7 +552,7 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) float ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; - // Checking for the existance of cusp, in the path, using the dot product. + // Checking for the existence of cusp, in the path, using the dot product. float dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0f) { return idx + 1; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index eee54a6be32..e1e049a5257 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -70,12 +70,12 @@ void PathAngleCritic::score(CriticData & data) } utils::setPathFurthestPointIfNotSet(data); - auto offseted_idx = std::min( + auto offsetted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1); - const float goal_x = data.path.x(offseted_idx); - const float goal_y = data.path.y(offseted_idx); - const float goal_yaw = data.path.yaws(offseted_idx); + const float goal_x = data.path.x(offsetted_idx); + const float goal_y = data.path.y(offsetted_idx); + const float goal_yaw = data.path.yaws(offsetted_idx); const geometry_msgs::msg::Pose & pose = data.state.pose.pose; switch (mode_) { diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index fc8f0ff7f64..eacbb56efd5 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -44,21 +44,21 @@ void PathFollowCritic::score(CriticData & data) utils::setPathCostsIfNotSet(data, costmap_ros_); const size_t path_size = data.path.x.shape(0) - 1; - auto offseted_idx = std::min( + auto offsetted_idx = std::min( *data.furthest_reached_path_point + offset_from_furthest_, path_size); // Drive to the first valid path point, in case of dynamic obstacles on path // we want to drive past it, not through it bool valid = false; - while (!valid && offseted_idx < path_size - 1) { - valid = (*data.path_pts_valid)[offseted_idx]; + while (!valid && offsetted_idx < path_size - 1) { + valid = (*data.path_pts_valid)[offsetted_idx]; if (!valid) { - offseted_idx++; + offsetted_idx++; } } - const auto path_x = data.path.x(offseted_idx); - const auto path_y = data.path.y(offseted_idx); + const auto path_x = data.path.x(offsetted_idx); + const auto path_y = data.path.y(offsetted_idx); const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 9490691b6e1..d5c111217af 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -408,12 +408,12 @@ void Optimizer::updateControlSequence() auto && costs_normalized = costs_ - xt::amin(costs_, immediate); auto && exponents = xt::eval(xt::exp(-1 / settings_.temperature * costs_normalized)); auto && softmaxes = xt::eval(exponents / xt::sum(exponents, immediate)); - auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis())); + auto && softmaxes_extended = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis())); - xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate); - xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate); + xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extended, 0, immediate); + xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extended, 0, immediate); if (is_holo) { - xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); + xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extended, 0, immediate); } applyControlSequenceConstraints(); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index ddf2b403b68..254062eb297 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -75,7 +75,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); unsigned int mx, my; - // Find the furthest relevent pose on the path to consider within costmap + // Find the furthest relevant pose on the path to consider within costmap // bounds // Transforming it to the costmap frame in the same loop for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end; @@ -120,7 +120,7 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( nav_msgs::msg::Path PathHandler::transformPath( const geometry_msgs::msg::PoseStamped & robot_pose) { - // Find relevent bounds of path to use + // Find relevant bounds of path to use geometry_msgs::msg::PoseStamped global_pose = transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp index e003859c627..dc8f49a2a3a 100644 --- a/nav2_mppi_controller/test/controller_state_transition_test.cpp +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -32,7 +32,7 @@ class RosLockGuard }; RosLockGuard g_rclcpp; -// Tests basic transition from configure->active->process->deactive->cleanup +// Tests basic transition from configure->active->process->deactivate->cleanup TEST(ControllerStateTransitionTest, ControllerNotFail) { diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index 27ef61d90c7..0c2475b0e37 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -131,18 +131,18 @@ TEST(ParameterHandlerTest, GetSystemParamsTest) // Get parameters in global namespace and in subnamespaces ParametersHandler handler(node); - auto getParamer = handler.getParamGetter(""); + auto getParameter = handler.getParamGetter(""); bool p1 = false; int p2 = 0; - getParamer(p1, "param1", false); - getParamer(p2, "ns.param2", 0); + getParameter(p1, "param1", false); + getParameter(p2, "ns.param2", 0); EXPECT_EQ(p1, true); EXPECT_EQ(p2, 7); // Get parameters in subnamespaces using name semantics of getter - auto getParamer2 = handler.getParamGetter("ns"); + auto getParameter2 = handler.getParamGetter("ns"); p2 = 0; - getParamer2(p2, "param2", 0); + getParameter2(p2, "param2", 0); EXPECT_EQ(p2, 7); } @@ -156,10 +156,10 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) handler.start(); // Get parameters and check they have initial values - auto getParamer = handler.getParamGetter(""); + auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParamer(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -201,10 +201,10 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) handler.start(); // Get parameters and check they have initial values - auto getParamer = handler.getParamGetter(""); + auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParamer(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -247,7 +247,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) handler.start(); // Set verbose true to get more information about bad parameter usage - auto getParamer = handler.getParamGetter(""); + auto getParameter = handler.getParamGetter(""); auto rec_param = std::make_shared( node->get_node_base_interface(), node->get_node_topics_interface(), node->get_node_graph_interface(), @@ -269,9 +269,9 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) // Try to access some parameters that have not been declared int p1 = 0, p2 = 0; - EXPECT_THROW(getParamer(p1, "not_declared", 8, ParameterType::Dynamic), + EXPECT_THROW(getParameter(p1, "not_declared", 8, ParameterType::Dynamic), rclcpp::exceptions::InvalidParameterValueException); - EXPECT_THROW(getParamer(p2, "not_declared2", 9, ParameterType::Static), + EXPECT_THROW(getParameter(p2, "not_declared2", 9, ParameterType::Static), rclcpp::exceptions::InvalidParameterValueException); // Try to set some parameters that have not been declared via the service client diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 2b7c8e0a906..ea151ba8863 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -47,14 +47,14 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) { auto node = std::make_shared("my_node"); auto parameters_handler = std::make_unique(node); - nav_msgs::msg::Path recieved_path; + nav_msgs::msg::Path received_path; nav_msgs::msg::Path pub_path; pub_path.header.frame_id = "fake_frame"; pub_path.poses.resize(5); auto my_sub = node->create_subscription( "transformed_global_plan", 10, - [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + [&](const nav_msgs::msg::Path msg) {received_path = msg;}); TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); @@ -62,8 +62,8 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) vis.visualize(pub_path); rclcpp::spin_some(node->get_node_base_interface()); - EXPECT_EQ(recieved_path.poses.size(), 5u); - EXPECT_EQ(recieved_path.header.frame_id, "fake_frame"); + EXPECT_EQ(received_path.poses.size(), 5u); + EXPECT_EQ(received_path.header.frame_id, "fake_frame"); } TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) @@ -71,10 +71,10 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) auto node = std::make_shared("my_node"); auto parameters_handler = std::make_unique(node); - visualization_msgs::msg::MarkerArray recieved_msg; + visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( "/trajectories", 10, - [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); // optimal_trajectory empty, should fail to publish xt::xtensor optimal_trajectory; @@ -87,7 +87,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); - EXPECT_EQ(recieved_msg.markers.size(), 0u); + EXPECT_EQ(received_msg.markers.size(), 0u); // Now populated with content, should publish optimal_trajectory = xt::ones({20, 2}); @@ -97,34 +97,34 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) rclcpp::spin_some(node->get_node_base_interface()); // Should have 20 trajectory points in the map frame - EXPECT_EQ(recieved_msg.markers.size(), 20u); - EXPECT_EQ(recieved_msg.markers[0].header.frame_id, "fkmap"); + EXPECT_EQ(received_msg.markers.size(), 20u); + EXPECT_EQ(received_msg.markers[0].header.frame_id, "fkmap"); // Check IDs are properly populated - EXPECT_EQ(recieved_msg.markers[0].id, 0); - EXPECT_EQ(recieved_msg.markers[1].id, 1); - EXPECT_EQ(recieved_msg.markers[10].id, 10); + EXPECT_EQ(received_msg.markers[0].id, 0); + EXPECT_EQ(received_msg.markers[1].id, 1); + EXPECT_EQ(received_msg.markers[10].id, 10); // Check poses are correct - EXPECT_EQ(recieved_msg.markers[0].pose.position.x, 1); - EXPECT_EQ(recieved_msg.markers[0].pose.position.y, 1); - EXPECT_EQ(recieved_msg.markers[0].pose.position.z, 0.06); + EXPECT_EQ(received_msg.markers[0].pose.position.x, 1); + EXPECT_EQ(received_msg.markers[0].pose.position.y, 1); + EXPECT_EQ(received_msg.markers[0].pose.position.z, 0.06); // Check that scales are rational - EXPECT_EQ(recieved_msg.markers[0].scale.x, 0.03); - EXPECT_EQ(recieved_msg.markers[0].scale.y, 0.03); - EXPECT_EQ(recieved_msg.markers[0].scale.z, 0.07); + EXPECT_EQ(received_msg.markers[0].scale.x, 0.03); + EXPECT_EQ(received_msg.markers[0].scale.y, 0.03); + EXPECT_EQ(received_msg.markers[0].scale.z, 0.07); - EXPECT_EQ(recieved_msg.markers[19].scale.x, 0.07); - EXPECT_EQ(recieved_msg.markers[19].scale.y, 0.07); - EXPECT_EQ(recieved_msg.markers[19].scale.z, 0.09); + EXPECT_EQ(received_msg.markers[19].scale.x, 0.07); + EXPECT_EQ(received_msg.markers[19].scale.y, 0.07); + EXPECT_EQ(received_msg.markers[19].scale.z, 0.09); // Check that the colors are rational - for (unsigned int i = 0; i != recieved_msg.markers.size() - 1; i++) { - EXPECT_LT(recieved_msg.markers[i].color.g, recieved_msg.markers[i + 1].color.g); - EXPECT_LT(recieved_msg.markers[i].color.b, recieved_msg.markers[i + 1].color.b); - EXPECT_EQ(recieved_msg.markers[i].color.r, recieved_msg.markers[i + 1].color.r); - EXPECT_EQ(recieved_msg.markers[i].color.a, recieved_msg.markers[i + 1].color.a); + for (unsigned int i = 0; i != received_msg.markers.size() - 1; i++) { + EXPECT_LT(received_msg.markers[i].color.g, received_msg.markers[i + 1].color.g); + EXPECT_LT(received_msg.markers[i].color.b, received_msg.markers[i + 1].color.b); + EXPECT_EQ(received_msg.markers[i].color.r, received_msg.markers[i + 1].color.r); + EXPECT_EQ(received_msg.markers[i].color.a, received_msg.markers[i + 1].color.a); } } @@ -133,10 +133,10 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) auto node = std::make_shared("my_node"); auto parameters_handler = std::make_unique(node); - visualization_msgs::msg::MarkerArray recieved_msg; + visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( "/trajectories", 10, - [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); models::Trajectories candidate_trajectories; candidate_trajectories.x = xt::ones({200, 12}); @@ -152,7 +152,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) rclcpp::spin_some(node->get_node_base_interface()); // 40 * 4, for 5 trajectory steps + 3 point steps - EXPECT_EQ(recieved_msg.markers.size(), 160u); + EXPECT_EQ(received_msg.markers.size(), 160u); } TEST(TrajectoryVisualizerTests, VisOptimalPath) @@ -163,10 +163,10 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) cmd_stamp.sec = 5; cmd_stamp.nanosec = 10; - nav_msgs::msg::Path recieved_path; + nav_msgs::msg::Path received_path; auto my_sub = node->create_subscription( "optimal_trajectory", 10, - [&](const nav_msgs::msg::Path msg) {recieved_path = msg;}); + [&](const nav_msgs::msg::Path msg) {received_path = msg;}); // optimal_trajectory empty, should fail to publish xt::xtensor optimal_trajectory; @@ -178,7 +178,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) vis.visualize(bogus_path); rclcpp::spin_some(node->get_node_base_interface()); - EXPECT_EQ(recieved_path.poses.size(), 0u); + EXPECT_EQ(received_path.poses.size(), 0u); // Now populated with content, should publish optimal_trajectory.resize({20, 2}); @@ -192,27 +192,27 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) rclcpp::spin_some(node->get_node_base_interface()); // Should have a 20 points path in the map frame and with same stamp than velocity command - EXPECT_EQ(recieved_path.poses.size(), 20u); - EXPECT_EQ(recieved_path.header.frame_id, "fkmap"); - EXPECT_EQ(recieved_path.header.stamp.sec, cmd_stamp.sec); - EXPECT_EQ(recieved_path.header.stamp.nanosec, cmd_stamp.nanosec); + EXPECT_EQ(received_path.poses.size(), 20u); + EXPECT_EQ(received_path.header.frame_id, "fkmap"); + EXPECT_EQ(received_path.header.stamp.sec, cmd_stamp.sec); + EXPECT_EQ(received_path.header.stamp.nanosec, cmd_stamp.nanosec); tf2::Quaternion quat; - for (unsigned int i = 0; i != recieved_path.poses.size() - 1; i++) { + for (unsigned int i = 0; i != received_path.poses.size() - 1; i++) { // Poses should be in map frame too - EXPECT_EQ(recieved_path.poses[i].header.frame_id, "fkmap"); + EXPECT_EQ(received_path.poses[i].header.frame_id, "fkmap"); // Check positions are correct - EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast(i)); - EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast(i)); - EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.06); + EXPECT_EQ(received_path.poses[i].pose.position.x, static_cast(i)); + EXPECT_EQ(received_path.poses[i].pose.position.y, static_cast(i)); + EXPECT_EQ(received_path.poses[i].pose.position.z, 0.06); // Check orientations are correct quat.setRPY(0., 0., optimal_trajectory(i, 2)); auto expected_orientation = tf2::toMsg(quat); - EXPECT_EQ(recieved_path.poses[i].pose.orientation.x, expected_orientation.x); - EXPECT_EQ(recieved_path.poses[i].pose.orientation.y, expected_orientation.y); - EXPECT_EQ(recieved_path.poses[i].pose.orientation.z, expected_orientation.z); - EXPECT_EQ(recieved_path.poses[i].pose.orientation.w, expected_orientation.w); + EXPECT_EQ(received_path.poses[i].pose.orientation.x, expected_orientation.x); + EXPECT_EQ(received_path.poses[i].pose.orientation.y, expected_orientation.y); + EXPECT_EQ(received_path.poses[i].pose.orientation.z, expected_orientation.z); + EXPECT_EQ(received_path.poses[i].pose.orientation.w, expected_orientation.w); } } diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 4c939bd72b0..2adf1c59463 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -359,7 +359,7 @@ TEST(UtilsTests, SmootherTest) savitskyGolayFilter(noisey_sequence, history, settings); - // Check history is propogated backward + // Check history is propagated backward EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02); EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02); EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02); diff --git a/nav2_msgs/srv/LoadMap.srv b/nav2_msgs/srv/LoadMap.srv index 3b9caaad470..a1898aa36a8 100644 --- a/nav2_msgs/srv/LoadMap.srv +++ b/nav2_msgs/srv/LoadMap.srv @@ -3,7 +3,7 @@ # Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml string map_url --- -# Result code defintions +# Result code definitions uint8 RESULT_SUCCESS=0 uint8 RESULT_MAP_DOES_NOT_EXIST=1 uint8 RESULT_INVALID_MAP_DATA=2 diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index 966a8458521..7f863baf984 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -261,7 +261,7 @@ class NavFn /** * @brief Calculates the path for at mose cycles * @param n The maximum number of cycles to run for - * @return The lenght of the path found, 0 if none + * @return The length of the path found, 0 if none */ int calcPath(int n, int * st = NULL); diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index c1cebb1bef0..5a1fb19fb99 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -227,7 +227,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ccce9813544..6359721e348 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -141,7 +141,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_WARN( get_logger(), "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency); + " than 0.0 to turn on duration overrun warning messages", expected_planner_frequency); max_planner_duration_ = 0.0; } @@ -730,7 +730,7 @@ PlannerServer::dynamicParametersCallback(std::vector paramete RCLCPP_WARN( get_logger(), "The expected planner frequency parameter is %.4f Hz. The value should to be greater" - " than 0.0 to turn on duration overrrun warning messages", parameter.as_double()); + " than 0.0 to turn on duration overrun warning messages", parameter.as_double()); max_planner_duration_ = 0.0; } } diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index acdd0eb1ed9..619d53619a9 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -151,7 +151,7 @@ controller_server: | `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | | `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | -Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed gage. ## Notes to users diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 18941f70824..df3e5ea587e 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -135,7 +135,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Whether robot should rotate to rough path heading * @param carrot_pose current lookahead point - * @param angle_to_path Angle of robot output relatie to carrot marker + * @param angle_to_path Angle of robot output relative to carrot marker * @param x_vel_sign Velocoty sign (forward or backward) * @return Whether should rotate to path heading */ @@ -154,7 +154,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief Create a smooth and kinematically smoothed rotation command * @param linear_vel linear velocity * @param angular_vel angular velocity - * @param angle_to_path Angle of robot output relatie to carrot marker + * @param angle_to_path Angle of robot output relative to carrot marker * @param curr_speed the current robot speed */ void rotateToHeading( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index edcd6db5583..896a1ab45af 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -506,7 +506,7 @@ double RegulatedPurePursuitController::findVelocitySignChange( double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y - transformed_plan.poses[pose_id].pose.position.y; - /* Checking for the existance of cusp, in the path, using the dot product + /* Checking for the existence of cusp, in the path, using the dot product and determine it's distance from the robot. If there is no cusp in the path, then just determine the distance to the goal location. */ const double dot_prod = (oa_x * ab_x) + (oa_y * ab_y); diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 38cc195bc61..77ecac687af 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -260,7 +260,7 @@ INSTANTIATE_TEST_SUITE_P( 1.0, {1.0, 0.0} }, - // Origin to hte negative X axis + // Origin to the negative X axis CircleSegmentIntersectionParam{ {0.0, 0.0}, {-2.0, 0.0}, @@ -405,7 +405,7 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) { EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON); EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON); - // 2 poses bck + // 2 poses back path.poses.clear(); path.poses.resize(2); path.poses[0].pose.position.x = -2.0; @@ -646,7 +646,7 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // ctrl->resetVelocityRegulationScaling(); // curvature = 0.0; - // min changable cost + // min changeable cost // pose_cost = 1; // linear_vel = 0.5; // curr_speed.linear.x = 0.5; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp index 1f286060f3c..ba52b009372 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp @@ -27,7 +27,7 @@ namespace nav2_rviz_plugins { /** - * @brief Load the avaialble plugins into the combo box + * @brief Load the available plugins into the combo box * @param node The node to use for loading the plugins * @param server_failed if the server failed to load the plugins, false otherwise * @param server_name The name of the server to load plugins for diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index e43f718ebee..bc8b3c34288 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -257,7 +257,7 @@ DockingPanel::DockingPanel(QWidget * parent) } }); - // Conect buttons with functions + // Connect buttons with functions QObject::connect( use_dock_id_checkbox_, &QCheckBox::stateChanged, this, &DockingPanel::dockIdCheckbox); } diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 96cf7784bf1..0053e404683 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -45,7 +45,7 @@ void pluginLoader( } // Loading the plugins into the combo box - // If server unavaialble, let the combo box be empty + // If server unavailable, let the combo box be empty if (server_unavailable) { return; } diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 8e5ff073940..ddb1c14afab 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -79,7 +79,7 @@ def main(): navigator.followWaypoints(inspection_points) # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) - # Simply the current waypoint ID for the demonstation + # Simply the current waypoint ID for the demonstration i = 0 while not navigator.isTaskComplete(): i += 1 diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index d0975d14466..f6785f982b2 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -44,7 +44,7 @@ def main(): - # Recieved virtual request for picking item at Shelf A and bring to + # Received virtual request for picking item at Shelf A and bring to # worker at the pallet jack 7 for shipping. This request would # contain the shelf ID ('shelf_A') and shipping destination ('frieght_bay_3') #################### @@ -86,7 +86,7 @@ def main(): # Do something during our route # (e.x. queue up future tasks or detect person for fine-tuned positioning) - # Simply print information for workers on the robot's ETA for the demonstation + # Simply print information for workers on the robot's ETA for the demonstration i = 0 while not navigator.isTaskComplete(): i += 1 diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 38cc2653c88..2ff66f73115 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -72,7 +72,7 @@ def main(): navigator.goThroughPoses(route_poses) # Do something during our route (e.x. AI detection on camera images for anomalies) - # Simply print ETA for the demonstation + # Simply print ETA for the demonstration i = 0 while not navigator.isTaskComplete(): i += 1 diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py index 9f6c092570a..179c5d1da1b 100644 --- a/nav2_simple_commander/nav2_simple_commander/line_iterator.py +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -144,7 +144,7 @@ def getX0(self): return self.x0_ def getY0(self): - """Get the ordinate of the intial point.""" + """Get the ordinate of the initial point.""" return self.y0_ def getX1(self): diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 8bfb91588b1..3ec64bc5212 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -21,7 +21,7 @@ class TestLineIterator(unittest.TestCase): def test_type_error(self): - # Test if a type error raised when passing invalid arguements types + # Test if a type error raised when passing invalid arguments types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') def test_value_error(self): diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 49ab250123f..5afe2238e1d 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -199,7 +199,7 @@ Before addressing the section below, make sure you have an appropriately set max In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. -Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. +Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversable, but not quite. From the starting location, that gap yields the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index a3ae1f6c7b2..2184dbf4477 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -102,7 +102,7 @@ class AStarAlgorithm /** * @brief Creating path from given costmap, start, and goal - * @param path Reference to a vector of indicies of generated path + * @param path Reference to a vector of indices of generated path * @param num_iterations Reference to number of iterations to create plan * @param tolerance Reference to tolerance in costmap nodes * @param cancel_checker Function to check if the task has been canceled @@ -162,7 +162,7 @@ class AStarAlgorithm /** * @brief Get maximum number of on-approach iterations after within threshold - * @return Reference to Maximum on-appraoch iterations parameter + * @return Reference to Maximum on-approach iterations parameter */ int & getOnApproachMaxIterations(); @@ -231,7 +231,7 @@ class AStarAlgorithm inline bool areInputsValid(); /** - * @brief Clear hueristic queue of nodes to search + * @brief Clear heuristic queue of nodes to search */ inline void clearQueue(); diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index c4d3deccda7..6be84d1c109 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -116,7 +116,7 @@ class AnalyticExpansion /** * @brief Takes an expanded nodes to clean up, if necessary, of any state - * information that may be poluting it from a prior search iteration + * information that may be polluting it from a prior search iteration * @param expanded_nodes Expanded node to clean up from search */ void cleanNode(const NodePtr & nodes); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 2ef090fe498..b18ba0043ea 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -68,7 +68,7 @@ class Node2D /** * @brief operator== for comparisons * @param Node2D right hand side node reference - * @return If cell indicies are equal + * @return If cell indices are equal */ bool operator==(const Node2D & rhs) { @@ -258,7 +258,7 @@ class Node2D /** * @brief Set the starting pose for planning, as a node index - * @param path Reference to a vector of indicies of generated path + * @param path Reference to a vector of indices of generated path * @return whether the path was able to be backtraced */ bool backtracePath(CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index cf6a9a6e89e..e8bf083e64f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -196,7 +196,7 @@ class NodeHybrid /** * @brief operator== for comparisons * @param NodeHybrid right hand side node reference - * @return If cell indicies are equal + * @return If cell indices are equal */ bool operator==(const NodeHybrid & rhs) { @@ -460,7 +460,7 @@ class NodeHybrid /** * @brief Set the starting pose for planning, as a node index - * @param path Reference to a vector of indicies of generated path + * @param path Reference to a vector of indices of generated path * @return whether the path was able to be backtraced */ bool backtracePath(CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index f3e3ff37f9a..fe5af4efec7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -140,7 +140,7 @@ class NodeLattice /** * @brief operator== for comparisons * @param NodeLattice right hand side node reference - * @return If cell indicies are equal + * @return If cell indices are equal */ bool operator==(const NodeLattice & rhs) { @@ -402,7 +402,7 @@ class NodeLattice /** * @brief Set the starting pose for planning, as a node index - * @param path Reference to a vector of indicies of generated path + * @param path Reference to a vector of indices of generated path * @return whether the path was able to be backtraced */ bool backtracePath(CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index f1653569440..a4b7ed180bb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -91,7 +91,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner protected: /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 57e225f2876..74be801ad2b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -90,7 +90,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner protected: /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 89ecc0b0926..00503d4d293 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -138,7 +138,7 @@ class Smoother * @brief Set the field value for a given dimension * @param msg Current pose to sample * @param dim Dimension ID of interest - * @param value to set the dimention to for the pose + * @param value to set the dimension to for the pose */ inline void setFieldByDim( geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h index 978a3728a8e..5aa4d530753 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -185,7 +185,7 @@ static Counts& counts() { # define ROBIN_HOOD_UNLIKELY(condition) __builtin_expect(condition, 0) #endif -// detect if native wchar_t type is availiable in MSVC +// detect if native wchar_t type is available in MSVC #ifdef _MSC_VER # ifdef _NATIVE_WCHAR_T_DEFINED # define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 @@ -196,7 +196,7 @@ static Counts& counts() { # define ROBIN_HOOD_PRIVATE_DEFINITION_HAS_NATIVE_WCHART() 1 #endif -// detect if MSVC supports the pair(std::piecewise_construct_t,...) consructor being constexpr +// detect if MSVC supports the pair(std::piecewise_construct_t,...) constructor being constexpr #ifdef _MSC_VER # if _MSC_VER <= 1900 # define ROBIN_HOOD_PRIVATE_DEFINITION_BROKEN_CONSTEXPR() 1 @@ -891,7 +891,7 @@ struct WrapKeyEqual : public T { // // * Node: either a DataNode that directly has the std::pair as member, // or a DataNode with a pointer to std::pair. Which DataNode representation to use -// depends on how fast the swap() operation is. Heuristically, this is automatically choosen +// depends on how fast the swap() operation is. Heuristically, this is automatically chosen // based on sizeof(). there are always 2^n Nodes. // // * info: Each Node in the map has a corresponding info byte, so there are 2^n info bytes. @@ -1502,7 +1502,7 @@ class Table // Creates an empty hash map. Nothing is allocated yet, this happens at the first insert. // This tremendously speeds up ctor & dtor of a map that never receives an element. The - // penalty is payed at the first insert, and not before. Lookup of this empty map works + // penalty is paid at the first insert, and not before. Lookup of this empty map works // because everybody points to DummyInfoByte::b. parameter bucket_count is dictated by the // standard, but we can ignore it. explicit Table( diff --git a/nav2_smac_planner/lattice_primitives/README.md b/nav2_smac_planner/lattice_primitives/README.md index 0d9b6982faf..cfdebe36aa4 100644 --- a/nav2_smac_planner/lattice_primitives/README.md +++ b/nav2_smac_planner/lattice_primitives/README.md @@ -130,11 +130,11 @@ The image shows how the angular headings are generated. The same idea can be ext 2. Find the intersection point I of lines 1 and 2 -3. Calculate the distance beween I and the origin (let this be d1). Also calculate the distance between I and the end point (let this be d2) +3. Calculate the distance between I and the origin (let this be d1). Also calculate the distance between I and the end point (let this be d2) 4. If d1 and d2 are equal then proceed to step 5. Otherwise, create intermediate points for each line that are min(d1, d2) distance away along the lines from I. So there should be an intermediate point on line 1 and an intermediate point on line 2. One of these intermediate points should align with either the origin or the end point by nature of how the distance was calculated. -5. Create perpindicular lines for line 1 and line 2 that pass through the respective intermediate points. The intersection of these perpindicular lines is the centre of the circle whose arc represents the curved portion of the trajectory. +5. Create perpendicular lines for line 1 and line 2 that pass through the respective intermediate points. The intersection of these perpendicular lines is the centre of the circle whose arc represents the curved portion of the trajectory. 6. Finally, if needed append straight segments to the path to ensure we start at the origin and end at the end point. @@ -159,7 +159,7 @@ The lattice generator is generally based on the generation of the control set as 4. Steps 2-3 are repeated for the next wavefront which is a grid resolution step further away from the origin. -5. Steps 1-4 are repeated untill all trajectories are being removed. The generator will continue for a few more wavefront steps until N wavefronts have been searched with no new trajectories. At this point the generator terminates and returns the computed minimal set. +5. Steps 1-4 are repeated until all trajectories are being removed. The generator will continue for a few more wavefront steps until N wavefronts have been searched with no new trajectories. At this point the generator terminates and returns the computed minimal set. - The number N is the stopping_threshold parameter diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index 185cc0f839f..a2b0340cb9b 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -31,7 +31,7 @@ class LatticeGenerator: Handles all the logic for computing the minimal control set. Computes the minimal control set for a vehicle given its parameters. - Includes handling the propogating and searching along wavefronts as + Includes handling the propagating and searching along wavefronts as well as determining if a trajectory is part of the minimal set based on previously added trajectories. """ @@ -169,7 +169,7 @@ def _is_minimal_trajectory( self, trajectory: Trajectory, prior_end_poses: index.Rtree ) -> bool: """ - Determine wheter a trajectory is a minimal trajectory. + Determine whether a trajectory is a minimal trajectory. Uses an RTree for speedup. diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py index 9187d0b1e46..fb6fa2c7b42 100644 --- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -19,7 +19,7 @@ to ensure that the x, y, and yaw values are correct. This is mainly used for debugging when making changes to parts of the code. However, if you would like to see how each trajectory in your -ouput file looks then you can run this script. +output file looks then you can run this script. """ import json diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index bd47d055cdf..5fa7d638962 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -474,7 +474,7 @@ def _calculate_trajectory_params( x1, y1 = arc_start_point x2, y2 = arc_end_point - # Find intersection point of the perpindicular lines of line 1 and 2 + # Find intersection point of the perpendicular lines of line 1 and 2 # that pass through arc start and arc end point respectively if m1 == 0: # If line 1 has gradient 0 then it is the x-axis. diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 73084859da7..ec0ccc31f02 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -110,7 +110,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // The analytic expansion can short-cut near obstacles when closer to a goal // So, we can attempt to refine it more by increasing the possible radius // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traveral cost estimation. + // a scoring function similar to that used in traversal cost estimation. auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { if (expansion.size() < 2) { return std::numeric_limits::max(); diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 0ec6da7a739..c1a7a236077 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -87,7 +87,7 @@ void HybridMotionTable::initDubin( // 2) chord length must be greater than sqrt(2) to leave current cell // 3) maximum curvature must be respected, represented by minimum turning angle // Thusly: - // On circle of radius minimum turning angle, we need select motion primatives + // On circle of radius minimum turning angle, we need select motion primitives // with chord length > sqrt(2) and be an increment of our bin size // // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 728d9910fd9..1dfdebfbfc6 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -115,7 +115,7 @@ void SmacPlanner2D::configure( _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), true /*for 2D, most use radius*/, - 0.0 /*for 2D cost at inscribed isn't relevent*/); + 0.0 /*for 2D cost at inscribed isn't relevant*/); // Initialize A* template _a_star = std::make_unique>(_motion_model, _search_info); diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index 66e0d58446c..298efa51e2f 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -247,7 +247,7 @@ std::vector Smoother::findDirectionalPathSegments(const nav_msgs::m double ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; - // Checking for the existance of cusp, in the path, using the dot product. + // Checking for the existence of cusp, in the path, using the dot product. double dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0) { curr_segment.end = idx; @@ -255,7 +255,7 @@ std::vector Smoother::findDirectionalPathSegments(const nav_msgs::m curr_segment.start = idx; } - // Checking for the existance of a differential rotation in place. + // Checking for the existence of a differential rotation in place. double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 98a921d1757..35912cd3d5b 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -53,8 +53,8 @@ TEST(NodeLatticeTest, parser_test) nav2_smac_planner::MotionPose pose; json jsonMetaData = j["lattice_metadata"]; - json jsonPrimatives = j["primitives"]; - json jsonPose = jsonPrimatives[0]["poses"][0]; + json jsonPrimitives = j["primitives"]; + json jsonPose = jsonPrimitives[0]["poses"][0]; nav2_smac_planner::fromJsonToMetaData(jsonMetaData, metaData); @@ -67,10 +67,10 @@ TEST(NodeLatticeTest, parser_test) EXPECT_EQ(metaData.motion_model, std::string("ackermann")); std::vector myPrimitives; - for (unsigned int i = 0; i < jsonPrimatives.size(); ++i) { - nav2_smac_planner::MotionPrimitive newPrimative; - nav2_smac_planner::fromJsonToMotionPrimitive(jsonPrimatives[i], newPrimative); - myPrimitives.push_back(newPrimative); + for (unsigned int i = 0; i < jsonPrimitives.size(); ++i) { + nav2_smac_planner::MotionPrimitive newPrimitive; + nav2_smac_planner::fromJsonToMotionPrimitive(jsonPrimitives[i], newPrimitive); + myPrimitives.push_back(newPrimitive); } // Checks for parsing primitives diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 2e8fea4a945..da0f98bf965 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -114,7 +114,7 @@ class SimpleSmoother : public nav2_core::Smoother * @brief Set the field value for a given dimension * @param msg Current pose to sample * @param dim Dimension ID of interest - * @param value to set the dimention to for the pose + * @param value to set the dimension to for the pose */ inline void setFieldByDim( geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp index 67d7296353f..c7a45f383c0 100644 --- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -67,7 +67,7 @@ inline std::vector findDirectionalPathSegments( double ab_y = path.poses[idx + 1].pose.position.y - path.poses[idx].pose.position.y; - // Checking for the existance of cusp, in the path, using the dot product. + // Checking for the existence of cusp, in the path, using the dot product. double dot_product = (oa_x * ab_x) + (oa_y * ab_y); if (dot_product < 0.0) { curr_segment.end = idx; @@ -75,7 +75,7 @@ inline std::vector findDirectionalPathSegments( curr_segment.start = idx; } - // Checking for the existance of a differential rotation in place. + // Checking for the existence of a differential rotation in place. double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 56064aa9ace..88d663a57a0 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -191,7 +191,7 @@ def main(argv=sys.argv[1:]): == ComputePathToPose.Result().GOAL_OUTSIDE_MAP ) - # stop on failure test with bogous waypoint + # stop on failure test with bogus waypoint test.setStopFailureParam(True) bwps = [[55.944831, -3.186998], [55.929834, -3.184343], [55.944782, -3.187060]] test.setWaypoints(bwps) diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 87c7bb671e0..612c050a043 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): output='screen', ) - # Define commands for spawing the robots into Gazebo + # Define commands for spawning the robots into Gazebo spawn_robots_cmds = [] for robot in robots: spawn_robots_cmds.append( diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index ff84569c3ad..43564543e1e 100644 --- a/nav2_system_tests/src/updown/README.md +++ b/nav2_system_tests/src/updown/README.md @@ -12,7 +12,7 @@ If the test passes, you should see this comment in the output: [test_updown-13] [INFO] [test_updown]: **************************************************** TEST PASSED! ``` -To run the test in a loop 1000x, run the `test_updown_reliablity` script and log the output: +To run the test in a loop 1000x, run the `test_updown_reliability` script and log the output: ``` ./test_updown_reliability |& tee /tmp/updown.log ``` diff --git a/nav2_system_tests/src/updown/updownresults.py b/nav2_system_tests/src/updown/updownresults.py index 6025094be1d..de38c51c8bf 100755 --- a/nav2_system_tests/src/updown/updownresults.py +++ b/nav2_system_tests/src/updown/updownresults.py @@ -14,8 +14,8 @@ # limitations under the License. -# To use this script, run the `test_updown_reliablity` script and log the output -# > ./test_updown_reliablity |& tee /tmp/updown.log +# To use this script, run the `test_updown_reliability` script and log the output +# > ./test_updown_reliability |& tee /tmp/updown.log # When the test is completed, pipe the log to this script to get a summary of the # results # > ./updownresults.py < /tmp/updown.log @@ -59,7 +59,7 @@ def main(): if 'The system is active' in line: bringup_successful = True - if 'The system has been sucessfully shut down' in line: + if 'The system has been successfully shut down' in line: shutdown_successful = True print('Number of tests: ', test_count) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index b5df396006f..d60406063c5 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -242,7 +242,7 @@ def main(argv=sys.argv[1:]): == ComputePathToPose.Result().GOAL_OUTSIDE_MAP ) - # stop on failure test with bogous waypoint + # stop on failure test with bogus waypoint test.setStopFailureParam(True) bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]] test.setWaypoints(bwps) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 09f87142196..1c7296f3ee1 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -100,7 +100,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner const double & dist_bw_points); /** - * @brief Callback executed when a paramter change is detected + * @brief Callback executed when a parameter change is detected * @param parameters list of changed parameters */ rcl_interfaces::msg::SetParametersResult diff --git a/nav2_util/include/nav2_util/lifecycle_utils.hpp b/nav2_util/include/nav2_util/lifecycle_utils.hpp index 91517753d49..a270aa4e7ed 100644 --- a/nav2_util/include/nav2_util/lifecycle_utils.hpp +++ b/nav2_util/include/nav2_util/lifecycle_utils.hpp @@ -40,7 +40,7 @@ void startup_lifecycle_nodes( /// Transition the given lifecycle nodes to the ACTIVATED state in order. /** - * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2" + * \param[in] nodes A ':' separated list of node names. eg. "/node1:/node2" */ void startup_lifecycle_nodes( const std::string & nodes, @@ -67,7 +67,7 @@ void reset_lifecycle_nodes( /// Transition the given lifecycle nodes to the UNCONFIGURED state in order. /** - * \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2" + * \param[in] nodes A ':' separated list of node names. eg. "/node1:/node2" */ void reset_lifecycle_nodes( const std::string & nodes, diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 99570224add..5cae40170d8 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -31,7 +31,7 @@ namespace nav2_util * purpose. However, only alphanumeric characters and '_' are allowed in node * names. This function replaces any invalid character with a '_' * - * \param[in] potential_node_name Potential name but possibly with invalid charaters. + * \param[in] potential_node_name Potential name but possibly with invalid characters. * \return A copy of the input string but with non-alphanumeric characters replaced with '_' */ std::string sanitize_node_name(const std::string & potential_node_name); diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index ab9132b5ffe..8f184b1ae36 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -35,7 +35,7 @@ namespace nav2_util /** * @class OdomSmoother - * Wrapper for getting smooth odometry readings using a simple moving avergae. + * Wrapper for getting smooth odometry readings using a simple moving average. * Subscribes to the topic with a mutex. */ class OdomSmoother diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 342f930ab9f..8de024ab05f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -286,7 +286,7 @@ class SimpleActionServer } /** - * @brief Deactive action server + * @brief Deactivate action server */ void deactivate() { diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp index 58594f1ecd5..315f05fee05 100644 --- a/nav2_util/include/nav2_util/validate_messages.hpp +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -25,14 +25,14 @@ // @brief Validation Check -// Check recieved message is safe or not for the nav2-system +// Check received message is safe or not for the nav2-system // For each msg-type known in nav2, we could check it as following: // if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.") // // Workflow of validateMsg(): -// if here's a sub-msg-type in the recieved msg, +// if here's a sub-msg-type in the received msg, // the content of sub-msg would be checked as sub-msg-type -// then, check the whole recieved msg. +// then, check the whole received msg. // // Following conditions are involved in check: // 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on diff --git a/nav2_util/test/test_lifecycle_cli_node.cpp b/nav2_util/test/test_lifecycle_cli_node.cpp index 50a458cb1fd..48982c6b820 100644 --- a/nav2_util/test/test_lifecycle_cli_node.cpp +++ b/nav2_util/test/test_lifecycle_cli_node.cpp @@ -79,7 +79,7 @@ class RclCppFixture RclCppFixture g_rclcppfixture; -TEST(LifeycleCLI, fails_no_node_name) +TEST(LifecycleCLI, fails_no_node_name) { Handle handle; auto rc = system("ros2 run nav2_util lifecycle_bringup"); @@ -94,7 +94,7 @@ TEST(LifeycleCLI, fails_no_node_name) SUCCEED(); } -TEST(LifeycleCLI, succeeds_node_name) +TEST(LifecycleCLI, succeeds_node_name) { Handle handle; auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli"); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index a5d34149bc4..793bc436b2c 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -458,7 +458,7 @@ TEST(VelocitySmootherTest, testapplyConstraintsPositiveToPositiveDecel) smoother->configure(state); double no_eta = 1.0; - // Test asymetric accel/decel use cases + // Test asymmetric accel/decel use cases double accel = 0.1; double decel = -1.0; double dv_decel = -decel / 20.0; diff --git a/nav2_voxel_grid/src/voxel_grid.cpp b/nav2_voxel_grid/src/voxel_grid.cpp index e4055448b38..b2359dd7e75 100644 --- a/nav2_voxel_grid/src/voxel_grid.cpp +++ b/nav2_voxel_grid/src/voxel_grid.cpp @@ -213,7 +213,7 @@ VoxelStatus VoxelGrid::getVoxelColumn( return MARKED; } - // check if the number of unkown bits qualifies the col as unknown + // check if the number of unknown bits qualifies the col as unknown if (!bitsBelowThreshold(unknown_bits, unknown_threshold)) { return UNKNOWN; } diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index cbd27002464..69498aa267a 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -49,7 +49,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor /** * @brief declares and loads parameters used (waypoint_pause_duration_) * - * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param parent parent node that plugin will be created within (waypoint_follower in this case) * @param plugin_name */ void initialize( diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 8dd0d70cd77..1b77e1dfea4 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -122,7 +122,7 @@ class WaypointFollower : public nav2_util::LifecycleNode /** * @brief Templated function to perform internal logic behind waypoint following, * Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so. - * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details. + * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback functions for details. * * @tparam T action_server * @tparam V feedback diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 296f8f41de0..ff878f4d390 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -122,7 +122,7 @@ bool PhotoAtWaypoint::processAtWaypoint( cv::imwrite(full_path_image_path.c_str(), curr_frame_mat); RCLCPP_INFO( logger_, - "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index); + "Photo has been taken successfully at waypoint %i", curr_waypoint_index); } catch (const std::exception & e) { RCLCPP_ERROR( logger_, diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index d5334a51bdd..d1f870094b3 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -2,7 +2,7 @@ This experiment runs a set with randomly generated goals for objective benchmarking. -Bechmarking scripts require the following python packages to be installed: +Benchmarking scripts require the following python packages to be installed: ``` pip install transforms3d diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py index 4a422bc0037..9aab2c9f997 100644 --- a/tools/smoother_benchmarking/process_data.py +++ b/tools/smoother_benchmarking/process_data.py @@ -251,7 +251,7 @@ def main(): path_lengths = np.asarray(path_lengths) total_paths = len(paths) - # [planner, smoothers] path lenghth in a row + # [planner, smoothers] path length in a row path_lengths.resize((int(total_paths / methods_num), methods_num)) # [planner, smoothers] path length in a column path_lengths = path_lengths.transpose() From ff0d6981a54824b8006e681fd1efe70aec32d371 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 13 Dec 2024 12:58:52 +0100 Subject: [PATCH 18/19] fix Signed-off-by: Tony Najjar --- nav2_common/nav2_common/launch/parse_multirobot_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_common/nav2_common/launch/parse_multirobot_pose.py b/nav2_common/nav2_common/launch/parse_multirobot_pose.py index 55cf7093deb..9207cad5f79 100644 --- a/nav2_common/nav2_common/launch/parse_multirobot_pose.py +++ b/nav2_common/nav2_common/launch/parse_multirobot_pose.py @@ -33,7 +33,7 @@ def __init__(self, target_argument: Text): `target_argument` shall be 'robots'. Then, this will parse a string value for `robots` argument. - Each robot name which is corresponding to namespace and pose of it will be separated by `;`. + Each robot name which corresponds to namespace and pose of it will be separated by `;`. The pose consists of x, y and yaw with YAML format. :param: target argument name to parse From 0bc9ccd5258fc7f5edc7dc17e3eafffba6bdeee7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 13 Dec 2024 13:23:32 +0100 Subject: [PATCH 19/19] revert not typo Signed-off-by: Tony Najjar --- nav2_costmap_2d/src/costmap_2d.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index bfa29e66e3f..0302801297b 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -555,9 +555,9 @@ bool Costmap2D::saveMap(std::string file_name) } fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff); - for (unsigned int it = 0; it < size_y_; iy++) { + for (unsigned int iy = 0; iy < size_y_; iy++) { for (unsigned int ix = 0; ix < size_x_; ix++) { - unsigned char cost = getCost(ix, it); + unsigned char cost = getCost(ix, iy); fprintf(fp, "%d ", cost); } fprintf(fp, "\n");