diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 2975c39f80..074f39ef54 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.8 + 1.1.9

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 589030f123..cb9333efbb 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.8 + 1.1.9 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index febaa7de64..1f7226e6aa 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -46,7 +46,7 @@ GoalUpdater::GoalUpdater( sub_option.callback_group = callback_group_; goal_sub_ = node_->create_subscription( goal_updater_topic, - 10, + rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); } @@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_some(); - if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { - goal = last_goal_received_; + if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); + } } setOutput("output_goal", goal); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 341192757f..95bb3e48b5 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -95,6 +95,8 @@ TEST_F(GoalUpdaterTestFixture, test_tick) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -102,35 +104,92 @@ TEST_F(GoalUpdaterTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { - tree_->rootNode()->executeTick(); - } - + // tick tree without publishing updated goal and get updated_goal + tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); +} - EXPECT_EQ(updated_goal, goal); +TEST_F(GoalUpdaterTestFixture, test_older_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + auto goal_updater_pub = + node_->create_publisher("goal_update", 10); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - goal_to_update.header.stamp = node_->now(); + goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goal_updater_pub->publish(goal_to_update); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; + config_->blackboard->get("updated_goal", updated_goal); + + // expect to succeed and not update goal + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(updated_goal, goal); +} + +TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) +{ + // create tree + std::string xml_txt = + R"( + + + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); - auto start = node_->now(); - while ((node_->now() - start).seconds() < 0.5) { - tree_->rootNode()->executeTick(); - goal_updater_pub->publish(goal_to_update); + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); - rclcpp::spin_some(node_); - } + // publish updated_goal older than goal + geometry_msgs::msg::PoseStamped goal_to_update_1; + goal_to_update_1.header.stamp = node_->now(); + goal_to_update_1.pose.position.x = 2.0; + + geometry_msgs::msg::PoseStamped goal_to_update_2; + goal_to_update_2.header.stamp = node_->now(); + goal_to_update_2.pose.position.x = 3.0; + goal_updater_pub->publish(goal_to_update_1); + goal_updater_pub->publish(goal_to_update_2); + tree_->rootNode()->executeTick(); + geometry_msgs::msg::PoseStamped updated_goal; config_->blackboard->get("updated_goal", updated_goal); - EXPECT_NE(updated_goal, goal); - EXPECT_EQ(updated_goal, goal_to_update); + // expect to succeed + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + // expect to update goal with latest goal update + EXPECT_EQ(updated_goal, goal_to_update_2); } int main(int argc, char ** argv) diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp similarity index 100% rename from nav2_behaviors/plugins/assisted_teleop.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp diff --git a/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp similarity index 100% rename from nav2_behaviors/plugins/back_up.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/back_up.hpp diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp similarity index 100% rename from nav2_behaviors/plugins/drive_on_heading.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp diff --git a/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp similarity index 100% rename from nav2_behaviors/plugins/spin.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp diff --git a/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp similarity index 100% rename from nav2_behaviors/plugins/wait.hpp rename to nav2_behaviors/include/nav2_behaviors/plugins/wait.hpp diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index d7942a1893..6f6494b755 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.8 + 1.1.9 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index defdfaf5a7..2882773bc0 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -14,7 +14,7 @@ #include -#include "assisted_teleop.hpp" +#include "nav2_behaviors/plugins/assisted_teleop.hpp" #include "nav2_util/node_utils.hpp" namespace nav2_behaviors diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 90a69a686e..61246d0509 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "back_up.hpp" +#include "nav2_behaviors/plugins/back_up.hpp" namespace nav2_behaviors { diff --git a/nav2_behaviors/plugins/drive_on_heading.cpp b/nav2_behaviors/plugins/drive_on_heading.cpp index 53525b3bb6..9b44f0bacb 100644 --- a/nav2_behaviors/plugins/drive_on_heading.cpp +++ b/nav2_behaviors/plugins/drive_on_heading.cpp @@ -14,7 +14,7 @@ // limitations under the License. #include -#include "drive_on_heading.hpp" +#include "nav2_behaviors/plugins/drive_on_heading.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior) diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index a76fe38f52..b7df8b3654 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -18,7 +18,7 @@ #include #include -#include "spin.hpp" +#include "nav2_behaviors/plugins/spin.hpp" #include "tf2/utils.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 236f712201..5100ab01b0 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -15,7 +15,7 @@ #include #include -#include "wait.hpp" +#include "nav2_behaviors/plugins/wait.hpp" namespace nav2_behaviors { diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9f123b81eb..29194acad5 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -71,7 +71,8 @@ def generate_launch_description(): namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', - remappings=[('/tf', 'tf'), + remappings=[('/map', 'map'), + ('/tf', 'tf'), ('/tf_static', 'tf_static'), ('/goal_pose', 'goal_pose'), ('/clicked_point', 'clicked_point'), diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index c239f0036d..6b018950c4 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.8 + 1.1.9 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index c4ba88031d..cdba8c07f1 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.8 + 1.1.9 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 82727af5e2..bc22714ef5 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -204,8 +204,15 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) void NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) { + geometry_msgs::msg::PoseStamped current_pose; + nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance); + RCLCPP_INFO( - logger_, "Begin navigating from current location to (%.2f, %.2f)", + logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)", + current_pose.pose.position.x, current_pose.pose.position.y, goal->pose.pose.position.x, goal->pose.pose.position.y); // Reset state for new action feedback diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2dc2995856..2aed9920d0 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -27,6 +27,7 @@ #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index d80a6ff0e0..eb750b4e60 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.8 + 1.1.9 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 939555bf3d..69523e3c14 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -148,6 +148,12 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + process({msg->linear.x, msg->linear.y, msg->angular.z}); } diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 7c12a4bd52..ae15bd488c 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -78,7 +78,7 @@ void Range::getData( // Ignore data, if its range is out of scope of range sensor abilities if (data_->range < data_->min_range || data_->range > data_->max_range) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", source_name_.c_str(), data_->range, data_->min_range, data_->max_range); diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 3b29fdda59..2887fa5503 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -128,17 +128,21 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): yaml[key] = rewrite_val break key = yaml_key_list.pop(0) - yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) - + if isinstance(yaml, list): + yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val) + else: + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) return yaml def substitute_keys(self, yaml, key_rewrites): if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: + for key in list(yaml.keys()): + val = yaml[key] + if key in key_rewrites: new_key = key_rewrites[key] yaml[new_key] = yaml[key] del yaml[key] + if isinstance(val, dict): self.substitute_keys(val, key_rewrites) def getYamlLeafKeys(self, yamlData): diff --git a/nav2_common/package.xml b/nav2_common/package.xml index e10aae9dda..d215166a49 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.8 + 1.1.9 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 762ae72db3..df52455957 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.8 + 1.1.9 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index a9a686981a..d3ae1711be 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.8 + 1.1.9 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index dfc71ee64f..c5a5fcae00 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.8 + 1.1.9 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey 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 5f68dcaed1..48d126577d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -178,7 +178,6 @@ class InflationLayer : public Layer */ void onFootprintChanged() override; -private: /** * @brief Lookup pre-computed distances * @param mx The x coordinate of the current cell diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 71cd4654a9..a4383f1477 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -128,7 +128,7 @@ class RangeSensorLayer : public CostmapLayer */ void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); -private: +protected: /** * @brief Processes all sensors into the costmap buffered from callbacks */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index aec68fac1f..dd1a65cef0 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -123,7 +123,7 @@ class StaticLayer : public CostmapLayer */ virtual void matchSize(); -private: +protected: /** * @brief Get parameters of layer */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 14baefcbdd..888e401439 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -133,7 +133,6 @@ class VoxelLayer : public ObstacleLayer */ virtual void resetMaps(); -private: /** * @brief Use raycasting between 2 points to clear freespace */ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 1b571b2745..daf0c64fab 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.8 + 1.1.9 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index ff6aa1d5d0..3ca44c2a78 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -75,7 +75,7 @@ void CostmapFilter::onInitialize() // Get parameters node->get_parameter(name_ + "." + "enabled", enabled_); filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); - double transform_tolerance; + double transform_tolerance {}; node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index bc4debf1be..881883872c 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.8 + 1.1.9 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index b08ab9a730..93228c13f5 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.8 + 1.1.9 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 3f74ce58a1..025990ae33 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.8 + 1.1.9 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7778d8ebf6..4ebe4e1404 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.8 + 1.1.9 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 9abbd2499f..f26bfef251 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.8 + 1.1.9 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index da5861c10e..b3e77825da 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.8 + 1.1.9 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 09c4e745a4..5561c30bdf 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.8 + 1.1.9 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index d541ce92b7..417e2e7d62 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.8 + 1.1.9 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 65572c482d..c1ec76b748 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.8 + 1.1.9 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 35d39d5f57..94ea2640c8 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.8 + 1.1.9 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index f025075d17..951ffb1562 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -66,6 +66,9 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | + | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | #### Ackermann Motion Model | Parameter | Type | Definition | @@ -83,14 +86,14 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 3.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.40. Minimal distance between robot and goal above which angle goal cost considered. | + | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | #### Goal Critic | Parameter | Type | Definition | | -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 1.0. Distance between robot and goal above which goal cost starts being considered | + | threshold_to_consider | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered | #### Obstacles Critic @@ -111,20 +114,22 @@ This process is then repeated a number of times and returns a converged solution | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path align cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered | | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | - + | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | #### Path Angle Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 2.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which path angle cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered | | offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. | | max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered | + | forward_preference | bool | Default true. Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains *no* particular preference one way or another. | + #### Path Follow Critic | Parameter | Type | Definition | @@ -132,14 +137,14 @@ This process is then repeated a number of times and returns a converged solution | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | - | threshold_to_consider | float | Default 0.4. Distance between robot and goal above which path follow cost stops being considered | + | threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | - | threshold_to_consider | double | Default 0.4. Distance between robot and goal above which prefer forward cost stops being considered | + | threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered | #### Twirling Critic @@ -186,17 +191,17 @@ controller_server: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 1.0 + threshold_to_consider: 1.4 GoalAngleCritic: enabled: true cost_power: 1 cost_weight: 3.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 PreferForwardCritic: enabled: true cost_power: 1 cost_weight: 5.0 - threshold_to_consider: 0.4 + threshold_to_consider: 0.5 ObstaclesCritic: enabled: true cost_power: 1 @@ -212,21 +217,23 @@ controller_server: cost_weight: 14.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 3 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 offset_from_furthest: 20 + use_path_orientations: false PathFollowCritic: enabled: true cost_power: 1 cost_weight: 5.0 offset_from_furthest: 5 - threshold_to_consider: 0.6 + threshold_to_consider: 1.4 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 - threshold_to_consider: 0.40 + threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 + forward_preference: true # TwirlingCritic: # enabled: true # twirling_cost_power: 1 @@ -255,7 +262,7 @@ If you don't require path following behavior (e.g. just want to follow a goal po As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. -The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. +The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker. The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 62d6bd1042..8fb8fb688a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 7618afb9a4..a549995557 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -49,6 +49,7 @@ class PathAlignCritic : public CriticFunction int trajectory_point_step_{0}; float threshold_to_consider_{0}; float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 92130dceb3..6bf412eb25 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -48,6 +48,8 @@ class PathAngleCritic : public CriticFunction float threshold_to_consider_{0}; size_t offset_from_furthest_{0}; + bool reversing_allowed_{true}; + bool forward_preference_{true}; unsigned int power_{0}; float weight_{0}; 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 12317c7b61..c06fba5b89 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 @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index 2fbf4edac7..9f0803a23a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -123,10 +125,18 @@ class PathHandler const geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Prune global path to only interesting portions + * @brief Prune a path to only interesting portions + * @param plan Plan to prune * @param end Final path iterator */ - void pruneGlobalPlan(const PathIterator end); + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); std::string name_; std::shared_ptr costmap_; @@ -134,11 +144,16 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; + double inversion_xy_tolerance_{0.2}; + double inversion_yaw_tolerance{0.4}; + bool enforce_path_inversion_{false}; + unsigned int inversion_locale_{0u}; }; } // namespace mppi 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 7319e318f0..93c1aae106 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -416,15 +417,25 @@ inline void setPathCostsIfNotSet( * @param pose pose * @param point_x Point to find angle relative to X axis * @param point_y Point to find angle relative to Y axis + * @param forward_preference If reversing direction is valid * @return Angle between two points */ -inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +inline double posePointAngle( + const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference) { double pose_x = pose.position.x; double pose_y = pose.position.y; double pose_yaw = tf2::getYaw(pose.orientation); double yaw = atan2(point_y - pose_y, point_x - pose_x); + + // If no preference for forward, return smallest angle either in heading or 180 of heading + if (!forward_preference) { + return std::min( + abs(angles::shortest_angular_distance(yaw, pose_yaw)), + abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + } + return abs(angles::shortest_angular_distance(yaw, pose_yaw)); } @@ -599,6 +610,59 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + 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. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index d1e5b68de5..017957da52 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.8 + 1.1.9 nav2_mppi_controller Aleksei Budyakov Steve Macenski diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index fc26f89d11..4dc2bcf1de 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -28,15 +28,14 @@ void ConstraintCritic::initialize() logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); - float vx_max, vy_max, vx_min, vy_min; + float vx_max, vy_max, vx_min; getParentParam(vx_max, "vx_max", 0.5); getParentParam(vy_max, "vy_max", 0.0); getParentParam(vx_min, "vx_min", -0.35); - getParentParam(vy_min, "vy_min", 0.0); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index c196871327..62bfdf0676 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -24,7 +24,7 @@ void GoalAngleCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0); - getParam(threshold_to_consider_, "threshold_to_consider", 0.40); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 859a9c3f5c..e38a98ed6b 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,13 +18,15 @@ namespace mppi::critics { +using xt::evaluation_strategy::immediate; + void GoalCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); - getParam(threshold_to_consider_, "threshold_to_consider", 1.0); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4); RCLCPP_INFO( logger_, "GoalCritic instantiated with %d power and %f weight.", @@ -47,14 +50,14 @@ void GoalCritic::score(CriticData & data) const auto goal_x = data.path.x(goal_idx); const auto goal_y = data.path.y(goal_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); + const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); + const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); auto dists = xt::sqrt( - xt::pow(last_x - goal_x, 2) + - xt::pow(last_y - goal_y, 2)); + xt::pow(traj_x - goal_x, 2) + + xt::pow(traj_y - goal_y, 2)); - data.costs += xt::pow(std::move(dists) * weight_, power_); + data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 44e629ddc0..fa98d56973 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -31,6 +31,17 @@ void ObstaclesCritic::initialize() collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + RCLCPP_INFO( logger_, "ObstaclesCritic instantiated with %d power and %f / %f weights. " @@ -184,7 +195,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) collision_checker_.worldToMap(x, y, x_i, y_i); cost = collision_checker_.pointCost(x_i, y_i); - if (consider_footprint_ && cost >= possibly_inscribed_cost_) { + if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); collision_cost.using_footprint = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index b95b96a04a..2585193ade 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -34,7 +34,8 @@ void PathAlignCritic::initialize() getParam(trajectory_point_step_, "trajectory_point_step", 4); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); RCLCPP_INFO( logger_, @@ -71,9 +72,11 @@ void PathAlignCritic::score(CriticData & data) const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points const size_t batch_size = T_x.shape(0); const size_t time_steps = T_x.shape(1); @@ -85,18 +88,27 @@ void PathAlignCritic::score(CriticData & data) return; } + float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; + double min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + for (size_t t = 0; t < batch_size; ++t) { - float summed_dist = 0; + summed_dist = 0; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - double min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; + min_dist_sq = std::numeric_limits::max(); + min_s = 0; // Find closest path segment to the trajectory point for (size_t s = 0; s < path_segments_count - 1; s++) { xt::xtensor_fixed> P; - float dx = P_x(s) - T_x(t, p); - float dy = P_y(s) - T_y(t, p); - float dist_sq = dx * dx + dy * dy; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; min_s = s; diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a588bbe291..f09554bf18 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,22 +22,37 @@ namespace mppi::critics void PathAngleCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + float vx_min; + getParentParam(vx_min, "vx_min", -0.35); + if (fabs(vx_min) < 1e-6) { // zero + reversing_allowed_ = false; + } else if (vx_min < 0.0) { // reversing possible + reversing_allowed_ = true; + } + auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 2.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); getParam( max_angle_to_furthest_, "max_angle_to_furthest", 1.2); + getParam( + forward_preference_, + "forward_preference", true); + if (!reversing_allowed_) { + forward_preference_ = true; + } RCLCPP_INFO( logger_, - "PathAngleCritic instantiated with %d power and %f weight.", - power_, weight_); + "PathAngleCritic instantiated with %d power and %f weight. Reversing %s", + power_, weight_, reversing_allowed_ ? "allowed." : "not allowed."); } void PathAngleCritic::score(CriticData & data) @@ -58,17 +74,28 @@ void PathAngleCritic::score(CriticData & data) const float goal_x = xt::view(data.path.x, offseted_idx); const float goal_y = xt::view(data.path.y, offseted_idx); - if (utils::posePointAngle(data.state.pose.pose, goal_x, goal_y) < max_angle_to_furthest_) { + if (utils::posePointAngle( + data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_) + { return; } - const auto yaws_between_points = xt::atan2( + auto yaws_between_points = xt::atan2( goal_y - data.trajectories.y, goal_x - data.trajectories.x); - const auto yaws = + + auto yaws = xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + if (reversing_allowed_ && !forward_preference_) { + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); + } else { + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + } } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index e440bd0b3e..01ecb91728 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -26,7 +26,7 @@ void PathFollowCritic::initialize() getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 1.4); getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0); diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 5cea014bbc..18b6900177 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -24,7 +24,7 @@ void PreferForwardCritic::initialize() getParam(weight_, "cost_weight", 5.0); getParam( threshold_to_consider_, - "threshold_to_consider", 0.40f); + "threshold_to_consider", 0.5); RCLCPP_INFO( logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_); diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 5127917e28..cfefb3b6d3 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,6 +37,12 @@ void PathHandler::initialize( getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + if (enforce_path_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + inversion_locale_ = 0u; + } } std::pair @@ -43,12 +51,13 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( { using nav2_util::geometry_utils::euclidean_distance; - auto begin = global_plan_.poses.begin(); + auto begin = global_plan_up_to_inversion_.poses.begin(); // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -63,7 +72,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_.poses.end(), prune_distance_); + 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 @@ -95,12 +104,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw std::runtime_error("Received plan with zero length"); } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw std::runtime_error( "Unable to transform robot pose into global plan's frame"); } @@ -116,7 +125,15 @@ nav_msgs::msg::Path PathHandler::transformPath( transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - pruneGlobalPlan(lower_bound); + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw std::runtime_error("Resulting plan has 0 poses in it."); @@ -156,13 +173,32 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_path_inversion_) { + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} -void PathHandler::pruneGlobalPlan(const PathIterator end) +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) { - global_plan_.poses.erase(global_plan_.poses.begin(), end); + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + double distance = std::hypot( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + double angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } } // namespace mppi diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index e79e4fe241..a1a42aa776 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -417,9 +417,9 @@ TEST(CriticTests, PathFollowCritic) // Scoring testing // provide state poses and path close within positional tolerances - state.pose.pose.position.x = 1.0; + state.pose.pose.position.x = 2.0; path.reset(6); - path.x(5) = 0.85; + path.x(5) = 1.7; critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 20e055392f..e6a992f482 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler PathHandlerWrapper() : PathHandler() {} - void pruneGlobalPlanWrapper(const PathIterator end) + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { - return pruneGlobalPlan(end); + return prunePlan(path, end); } double getMaxCostmapDistWrapper() @@ -66,6 +66,21 @@ class PathHandlerWrapper : public PathHandler { return transformToGlobalPlanFrame(pose); } + + void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) + { + global_plan_up_to_inversion_ = path; + } + + bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) + { + return isWithinInversionTolerances(robot_pose); + } + + nav_msgs::msg::Path & getInvertedPath() + { + return global_plan_up_to_inversion_; + } }; TEST(PathHandlerTests, GetAndPrunePath) @@ -82,7 +97,7 @@ TEST(PathHandlerTests, GetAndPrunePath) EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(it); + handler.pruneGlobalPlanWrapper(rtn_path, it); auto rtn2_path = handler.getPath(); EXPECT_EQ(rtn2_path.poses.size(), 6u); } @@ -131,10 +146,10 @@ TEST(PathHandlerTests, TestBounds) handler.setPath(path); auto [transformed_plan, closest] = handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - auto & path_in = handler.getPath(); - EXPECT_EQ(closest - path_in.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(closest); - auto & path_pruned = handler.getPath(); + auto & path_inverted = handler.getInvertedPath(); + EXPECT_EQ(closest - path_inverted.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(path_inverted, closest); + auto & path_pruned = handler.getInvertedPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); } @@ -189,3 +204,46 @@ TEST(PathHandlerTests, TestTransforms) auto final_path = handler.transformPath(robot_pose); EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); } + +TEST(PathHandlerTests, TestInversionToleranceChecks) +{ + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(i); + path.poses.push_back(pose); + } + path.poses.back().pose.orientation.w = 1; + + PathHandlerWrapper handler; + handler.setGlobalPlanUpToInversion(path); + + // Not near (0,0) + geometry_msgs::msg::PoseStamped robot_pose; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Exactly on top of it + robot_pose.pose.position.x = 9; + robot_pose.pose.orientation.w = 1.0; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Laterally of it + robot_pose.pose.position.y = 9; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.z = 0.8509035; + robot_pose.pose.orientation.w = 0.525322; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled within tolerances + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.w = 0.9961947; + robot_pose.pose.orientation.z = 0.0871558; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Offset spatially + off angled but both within tolerances + robot_pose.pose.position.x = 9.10; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 37acd78e87..8e6c2e4e50 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -195,7 +195,14 @@ TEST(UtilsTests, AnglesTests) pose.position.y = 0.0; pose.orientation.w = 1.0; double point_x = 1.0, point_y = 0.0; - EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); + bool forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = false; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + point_x = -1.0; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6); + forward_preference = true; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6); } TEST(UtilsTests, FurthestAndClosestReachedPoint) @@ -362,3 +369,66 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(utils::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +} diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index f38fb405b1..49b585af2a 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.8 + 1.1.9 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index d957d1ee6e..2b5ca37009 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.8 + 1.1.9 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 4083e291b6..18873edabd 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.8 + 1.1.9 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5edc284ed2..23e7cc44b5 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.8 + 1.1.9 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 6a44e9029b..f97c366224 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.8 + 1.1.9 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5436f3d46f..20200ebeef 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.8 + 1.1.9 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 085bf014b2..b39fe34809 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -275,7 +275,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition); auto options = rclcpp::NodeOptions().arguments( - {"--ros-args --remap __node:=navigation_dialog_action_client"}); + {"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"}); client_node_ = std::make_shared("_", options); client_nav_ = std::make_shared( diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index c87e718cfc..830d297706 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.8 + 1.1.9 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index e6520f4bd2..5933a6dc92 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -14,6 +14,7 @@ #include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ #define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ @@ -34,11 +35,13 @@ class GridCollisionChecker * for use when regular bin intervals are appropriate * @param costmap The costmap to collision check against * @param num_quantizations The number of quantizations to precompute footprint + * @param node Node to extract clock and logger from * orientations for to speed up collision checking */ GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations); + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker @@ -117,6 +120,8 @@ class GridCollisionChecker bool footprint_is_radius_; std::vector angles_; double possible_inscribed_cost_{-1}; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 524aa418db..0ab37485fc 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.8 + 1.1.9 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index c0cbfb98fa..20d288809d 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -19,9 +19,15 @@ namespace nav2_smac_planner GridCollisionChecker::GridCollisionChecker( nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations) + unsigned int num_quantizations, + rclcpp_lifecycle::LifecycleNode::SharedPtr node) : FootprintCollisionChecker(costmap) { + if (node) { + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + // Convert number of regular bins into angles float bin_size = 2 * M_PI / static_cast(num_quantizations); angles_.reserve(num_quantizations); @@ -104,7 +110,17 @@ bool GridCollisionChecker::inCollision( static_cast(x), static_cast(y)); if (footprint_cost_ < possible_inscribed_cost_) { - return false; + if (possible_inscribed_cost_ > 0) { + return false; + } else { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 1000, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } } // If its inscribed, in collision, or unknown in the middle, diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 606f6861ef..333bf20c8b 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -31,7 +31,7 @@ using std::placeholders::_1; SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -108,7 +108,7 @@ void SmacPlanner2D::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/); + _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), true /*for 2D, most use radius*/, diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ee614c7b94..c4c3019afc 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -32,7 +32,7 @@ using std::placeholders::_1; SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -182,7 +182,7 @@ void SmacPlannerHybrid::configure( } // Initialize collision checker - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -525,6 +525,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _lookup_table_dim += 1.0; } + auto node = _node.lock(); + // Re-Initialize A* template if (reinit_a_star) { _a_star = std::make_unique>(_motion_model, _search_info); @@ -540,7 +542,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize costmap downsampler if (reinit_downsampler) { if (_downsample_costmap && _downsampling_factor > 1) { - auto node = _node.lock(); std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); _costmap_downsampler->on_configure( @@ -550,7 +551,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize collision checker if (reinit_collision_checker) { - _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node); _collision_checker.setFootprint( _costmap_ros->getRobotFootprint(), _costmap_ros->getUseRadius(), @@ -559,7 +560,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para // Re-Initialize smoother if (reinit_smoother) { - auto node = _node.lock(); SmootherParams params; params.get(node, _name); _smoother = std::make_unique(params); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b7e22ee1f9..248ef1f218 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -31,7 +31,7 @@ using rcl_interfaces::msg::ParameterType; SmacPlannerLattice::SmacPlannerLattice() : _a_star(nullptr), - _collision_checker(nullptr, 1), + _collision_checker(nullptr, 1, nullptr), _smoother(nullptr), _costmap(nullptr) { @@ -169,7 +169,7 @@ void SmacPlannerLattice::configure( // increments causing "wobbly" checks that could cause larger robots to virtually show collisions // in valid configurations. This approximation helps to bound orientation error for all checks // in exchange for slight inaccuracies in the collision headings in terminal search states. - _collision_checker = GridCollisionChecker(_costmap, 72u); + _collision_checker = GridCollisionChecker(_costmap, 72u, node); _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius(), diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 72a6ba2968..05dfb0796f 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -39,6 +39,7 @@ RclCppFixture g_rclcppfixture; TEST(AStarTest, test_a_star_2d) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; nav2_smac_planner::AStarAlgorithm a_star( nav2_smac_planner::MotionModel::TWOD, info); @@ -62,7 +63,7 @@ TEST(AStarTest, test_a_star_2d) // functional case testing std::unique_ptr checker = - std::make_unique(costmapA, 1); + std::make_unique(costmapA, 1, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); @@ -122,6 +123,7 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -152,7 +154,7 @@ TEST(AStarTest, test_a_star_se2) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -178,6 +180,7 @@ TEST(AStarTest, test_a_star_se2) TEST(AStarTest, test_a_star_lattice) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.05; info.non_straight_penalty = 1.05; @@ -213,7 +216,7 @@ TEST(AStarTest, test_a_star_lattice) } std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing @@ -225,7 +228,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 49u); + EXPECT_GT(path.size(), 47u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -239,6 +242,7 @@ TEST(AStarTest, test_a_star_lattice) TEST(AStarTest, test_se2_single_pose_path) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -263,7 +267,7 @@ TEST(AStarTest, test_se2_single_pose_path) new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, size_theta); + std::make_unique(costmapA, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // functional case testing diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 40e73c82ad..84f2d5b39d 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -19,11 +19,21 @@ #include "gtest/gtest.h" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_util/lifecycle_node.hpp" using namespace nav2_costmap_2d; // NOLINT +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(collision_footprint, test_basic) { + auto node = std::make_shared("testA"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); geometry_msgs::msg::Point p1; @@ -41,7 +51,7 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -51,9 +61,10 @@ TEST(collision_footprint, test_basic) TEST(collision_footprint, test_point_cost) { + auto node = std::make_shared("testB"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); @@ -65,9 +76,10 @@ TEST(collision_footprint, test_point_cost) TEST(collision_footprint, test_world_to_map) { + auto node = std::make_shared("testC"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); nav2_costmap_2d::Footprint footprint; collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); @@ -90,6 +102,7 @@ TEST(collision_footprint, test_world_to_map) TEST(collision_footprint, test_footprint_at_pose_with_movement) { + auto node = std::make_shared("testD"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254); for (unsigned int i = 40; i <= 60; ++i) { @@ -113,7 +126,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); @@ -132,6 +145,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) TEST(collision_footprint, test_point_and_line_cost) { + auto node = std::make_shared("testE"); nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.10000, 0, 0.0, 128.0); @@ -153,7 +167,7 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node); collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index f3af941c4b..eede15fa84 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -34,9 +34,10 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { + auto node = std::make_shared("test"); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -101,6 +102,7 @@ TEST(Node2DTest, test_node_2d) TEST(Node2DTest, test_node_2d_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; unsigned int size_x = 10u; unsigned int size_y = 10u; @@ -122,7 +124,7 @@ TEST(Node2DTest, test_node_2d_neighbors) nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index e304b97e42..16d6278bad 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -35,6 +35,7 @@ RclCppFixture g_rclcppfixture; TEST(NodeHybridTest, test_node_hybrid) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -56,7 +57,7 @@ TEST(NodeHybridTest, test_node_hybrid) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction @@ -135,6 +136,7 @@ TEST(NodeHybridTest, test_node_hybrid) TEST(NodeHybridTest, test_obstacle_heuristic) { + auto node = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 0.1; info.non_straight_penalty = 1.1; @@ -169,7 +171,7 @@ TEST(NodeHybridTest, test_obstacle_heuristic) } } std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid testA(0); @@ -245,6 +247,7 @@ TEST(NodeHybridTest, test_node_debin_neighbors) TEST(NodeHybridTest, test_node_reeds_neighbors) { + auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; info.change_penalty = 1.2; info.non_straight_penalty = 1.4; @@ -284,7 +287,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(&costmapA, 72); + std::make_unique(&costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); std::function neighborGetter = diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 61479e6e1b..16674d9dad 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -22,9 +22,18 @@ #include "nav2_smac_planner/node_lattice.hpp" #include "gtest/gtest.h" #include "ament_index_cpp/get_package_share_directory.hpp" +#include "nav2_util/lifecycle_node.hpp" using json = nlohmann::json; +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + TEST(NodeLatticeTest, parser_test) { std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); @@ -164,6 +173,7 @@ TEST(NodeLatticeTest, test_node_lattice_conversions) TEST(NodeLatticeTest, test_node_lattice) { + auto node = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -207,7 +217,7 @@ TEST(NodeLatticeTest, test_node_lattice) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test node valid and cost @@ -241,6 +251,7 @@ TEST(NodeLatticeTest, test_node_lattice) TEST(NodeLatticeTest, test_get_neighbors) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -271,7 +282,7 @@ TEST(NodeLatticeTest, test_get_neighbors) nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( 10, 10, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmapA, 72); + std::make_unique(costmapA, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); std::function neighborGetter = @@ -291,6 +302,7 @@ TEST(NodeLatticeTest, test_get_neighbors) TEST(NodeLatticeTest, test_node_lattice_custom_footprint) { + auto lnode = std::make_shared("test"); std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_smac_planner"); std::string filePath = pkg_share_dir + @@ -321,7 +333,7 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D( 40, 40, 0.05, 0.0, 0.0, 0); std::unique_ptr checker = - std::make_unique(costmap, 72); + std::make_unique(costmap, 72, lnode); // Make some custom asymmetrical footprint nav2_costmap_2d::Footprint footprint; diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index d7d27f1d20..acf709c282 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -95,7 +95,7 @@ TEST(SmootherTest, test_full_smoother) a_star.initialize( false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); std::unique_ptr checker = - std::make_unique(costmap, size_theta); + std::make_unique(costmap, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // Create A* search to smooth diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index bc54f7b45c..f361b621db 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.8 + 1.1.9 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index 8dbc5416db..5bbbf399fa 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -226,21 +226,16 @@ class DummySmootherServer : public nav2_smoother::SmootherServer }; // Define a test class to hold the context for the tests - -class SmootherConfigTest : public ::testing::Test -{ -}; - class SmootherTest : public ::testing::Test { -protected: - SmootherTest() {SetUp();} +public: + SmootherTest() {} ~SmootherTest() {} - void SetUp() + void SetUp() override { - node_lifecycle_ = - std::make_shared( + node_ = + std::make_shared( "LifecycleSmootherTestNode", rclcpp::NodeOptions()); smoother_server_ = std::make_shared(); @@ -255,10 +250,10 @@ class SmootherTest : public ::testing::Test smoother_server_->activate(); client_ = rclcpp_action::create_client( - node_lifecycle_->get_node_base_interface(), - node_lifecycle_->get_node_graph_interface(), - node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "smooth_path"); + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "smooth_path"); std::cout << "Setup complete." << std::endl; } @@ -267,6 +262,9 @@ class SmootherTest : public ::testing::Test smoother_server_->deactivate(); smoother_server_->cleanup(); smoother_server_->shutdown(); + smoother_server_.reset(); + client_.reset(); + node_.reset(); } bool sendGoal( @@ -294,7 +292,7 @@ class SmootherTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); - if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != + if (rclcpp::spin_until_future_complete(node_, future_goal) != rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; @@ -318,12 +316,12 @@ class SmootherTest : public ::testing::Test std::cout << "Getting async result..." << std::endl; auto future_result = client_->async_get_result(goal_handle_); std::cout << "Waiting on future..." << std::endl; - rclcpp::spin_until_future_complete(node_lifecycle_, future_result); + rclcpp::spin_until_future_complete(node_, future_result); std::cout << "future received!" << std::endl; return future_result.get(); } - std::shared_ptr node_lifecycle_; + std::shared_ptr node_; std::shared_ptr smoother_server_; std::shared_ptr> client_; std::shared_ptr> goal_handle_; @@ -390,7 +388,7 @@ TEST_F(SmootherTest, testingCollisionCheckDisabled) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -405,7 +403,7 @@ TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -420,7 +418,7 @@ TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) { auto smoother_server = std::make_shared(); auto state = smoother_server->configure(); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index fb121d0673..bfdee60c71 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.8 + 1.1.9 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index c6554db039..7f0ca4cafd 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.8 + 1.1.9 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index baead03cba..e653e39a25 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -20,6 +20,8 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" @@ -107,6 +109,13 @@ bool getTransform( const std::shared_ptr tf_buffer, tf2::Transform & tf2_transform); +/** + * @brief Validates a twist message contains no nans or infs + * @param msg Twist message to validate + * @return True if valid, false if contains unactionable values + */ +bool validateTwist(const geometry_msgs::msg::Twist & msg); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 3b5e246ef7..861f323f24 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.8 + 1.1.9 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 37ab261230..20791e8293 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -15,6 +15,7 @@ // limitations under the License. #include +#include #include #include "nav2_util/robot_utils.hpp" @@ -141,4 +142,33 @@ bool getTransform( return true; } +bool validateTwist(const geometry_msgs::msg::Twist & msg) +{ + if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) { + return false; + } + + if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) { + return false; + } + + if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) { + return false; + } + + if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) { + return false; + } + + if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) { + return false; + } + + if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) { + return false; + } + + return true; +} + } // end namespace nav2_util diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index bd277515ab..d8922868fb 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/robot_utils.hpp" #include "tf2_ros/transform_listener.h" @@ -32,3 +33,28 @@ TEST(RobotUtils, LookupExceptionError) global_pose.header.frame_id = "base_link"; ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); } + +TEST(RobotUtils, validateTwist) +{ + geometry_msgs::msg::Twist msg; + EXPECT_TRUE(nav2_util::validateTwist(msg)); + + msg.linear.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.x = 1; + msg.linear.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.linear.y = 1; + msg.linear.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + + msg.linear.z = 1; + msg.angular.x = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.x = 1; + msg.angular.y = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); + msg.angular.y = 1; + msg.angular.z = NAN; + EXPECT_FALSE(nav2_util::validateTwist(msg)); +} diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index c3e7835c6b..a059e74076 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -25,6 +25,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/odometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_velocity_smoother { diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 986ba2be89..d0009942e7 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.8 + 1.1.9 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index aa5fbec783..bc101fd732 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,11 +74,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); - for (unsigned int i = 0; i != max_decels_.size(); i++) { + for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - RCLCPP_WARN( - get_logger(), - "Positive values set of deceleration! These should be negative to slow down!"); + throw std::runtime_error( + "Positive values set of deceleration! These should be negative to slow down!"); + } + if (max_accels_[i] < 0.0) { + throw std::runtime_error( + "Negative values set of acceleration! These should be positive to speed up!"); + } + if (min_velocities_[i] > max_velocities_[i]) { + throw std::runtime_error( + "Min velocities are higher than max velocities!"); } } @@ -174,6 +181,12 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + // If message contains NaN or Inf, ignore + if (!nav2_util::validateTwist(*msg)) { + RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); + return; + } + command_ = msg; last_command_time_ = now(); } @@ -352,8 +365,24 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } else if (name == "min_velocity") { min_velocities_ = parameter.as_double_array(); } else if (name == "max_accel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] < 0.0) { + RCLCPP_WARN( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + result.successful = false; + } + } max_accels_ = parameter.as_double_array(); } else if (name == "max_decel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + result.successful = false; + } + } max_decels_ = parameter.as_double_array(); } else if (name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index a1b2f5454a..c58e20b22c 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams) EXPECT_THROW(smoother->configure(state), std::runtime_error); } +TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) +{ + auto smoother = + std::make_shared(); + + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; + std::vector bad_test_min_vel{10.0, 10.0, 10.0}; + std::vector bad_test_max_vel{-10.0, -10.0, -10.0}; + + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); + rclcpp_lifecycle::State state; + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); +} + TEST(VelocitySmootherTest, testDynamicParameter) { auto smoother = @@ -613,6 +635,8 @@ TEST(VelocitySmootherTest, testDynamicParameter) std::vector min_accel{0.0, 0.0, 0.0}; std::vector deadband{0.0, 0.0, 0.0}; std::vector bad_test{0.0, 0.0}; + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("smoothing_frequency", 100.0), @@ -662,6 +686,16 @@ TEST(VelocitySmootherTest, testDynamicParameter) rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); EXPECT_FALSE(results.get().successful); + // Test invalid accel / decel + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_accel", bad_test_accel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_decel", bad_test_decel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + // test full state after major changes smoother->deactivate(state); smoother->cleanup(state); diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index f92a2166c1..d19fe4815b 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.8 + 1.1.9 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index adc360bb39..c1a88efec7 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.8 + 1.1.9 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index c74a2ae1a5..43315f5da6 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.8 + 1.1.9 ROS2 Navigation Stack