From ef0d773ae999985e4688e209c5cc8ff19bab11c2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 2 Jan 2024 19:10:39 -0800 Subject: [PATCH 01/13] Create CONTRIBUTING.md Signed-off-by: Steve Macenski --- CONTRIBUTING.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000000..cfba094dac --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). From 7647ba77d1b7bbba866e481ddd58f172736f8fef Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 4 Jan 2024 12:38:42 -0500 Subject: [PATCH 02/13] footprint checks (#4030) * footprint checks Signed-off-by: gg * lint fix Signed-off-by: gg --------- Signed-off-by: gg --- .../include/nav2_planner/planner_server.hpp | 3 ++ nav2_planner/src/planner_server.cpp | 37 +++++++++++++++---- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 2413e2eb3f..7d2c9d03f0 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_core/global_planner.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/planner_exceptions.hpp" namespace nav2_planner @@ -248,6 +249,8 @@ class PlannerServer : public nav2_util::LifecycleNode std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + collision_checker_; // Publishers for the path rclcpp_lifecycle::LifecyclePublisher::SharedPtr plan_publisher_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index d087cbef75..cb2e87b746 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -52,7 +52,6 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Declare this node's parameters declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 1.0); - declare_parameter("action_server_result_timeout", 10.0); get_parameter("planner_plugins", planner_ids_); @@ -86,6 +85,12 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); + if (!costmap_ros_->getUseRadius()) { + collision_checker_ = + std::make_unique>( + costmap_); + } + // Launch a thread to run the costmap node costmap_thread_ = std::make_unique(costmap_ros_); @@ -644,16 +649,34 @@ void PlannerServer::isPathValid( std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; unsigned int my = 0; + + bool use_radius = costmap_ros_->getUseRadius(); + + unsigned int cost = nav2_costmap_2d::FREE_SPACE; for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) { - costmap_->worldToMap( - request->path.poses[i].pose.position.x, - request->path.poses[i].pose.position.y, mx, my); - unsigned int cost = costmap_->getCost(mx, my); + auto & position = request->path.poses[i].pose.position; + if (use_radius) { + if (costmap_->worldToMap(position.x, position.y, mx, my)) { + cost = costmap_->getCost(mx, my); + } else { + cost = nav2_costmap_2d::LETHAL_OBSTACLE; + } + } else { + nav2_costmap_2d::Footprint footprint = costmap_ros_->getRobotFootprint(); + auto theta = tf2::getYaw(request->path.poses[i].pose.orientation); + cost = static_cast(collision_checker_->footprintCostAtPose( + position.x, position.y, theta, footprint)); + } - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + if (use_radius && + (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) { response->is_valid = false; + break; + } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) { + response->is_valid = false; + break; } } } From 305f7e039f1183b94981ec6a7f294f0ed7b89d07 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 4 Jan 2024 18:14:46 -0500 Subject: [PATCH 03/13] Is path valid doc (#4032) * docs Signed-off-by: gg * update Signed-off-by: gg --------- Signed-off-by: gg --- nav2_planner/src/planner_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index cb2e87b746..af28b1b1eb 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -644,7 +644,8 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; From bc76734348e8a4d5429debd496fd1e7d35a97ae1 Mon Sep 17 00:00:00 2001 From: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Date: Fri, 5 Jan 2024 18:44:55 +0100 Subject: [PATCH 04/13] Direction change index computation during initialization (#4026) * modified direction change index computation point prior the direction change index was being computed in ever getNeighbor call, now its done at parse time. Signed-off-by: Anil Kumar Chavali * changed computation point of direction_change_index * moved computation to getMotionPrimitives function * updated getMotionPrimitives function to return index * updated node lattice tests with modified function * removed obsolete code Signed-off-by: Anil Kumar Chavali * updated docstring for getMotionPrimitives func. Signed-off-by: Anil Kumar Chavali * linting fixes using ament_uncrustify Signed-off-by: Anil Kumar Chavali --------- Signed-off-by: Anil Kumar Chavali --- .../nav2_smac_planner/node_lattice.hpp | 5 ++++- nav2_smac_planner/src/node_lattice.cpp | 22 +++++++++---------- nav2_smac_planner/test/test_nodelattice.cpp | 15 +++++++++---- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 4fe6a284cf..577cc28e19 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -67,9 +67,12 @@ struct LatticeMotionTable /** * @brief Get projections of motion models * @param node Ptr to NodeLattice + * @param Reference direction change index * @return A set of motion poses */ - MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node); + MotionPrimitivePtrs getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index); /** * @brief Get file metadata needed diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c852a96dd3..dfdf47ff4b 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -111,7 +111,9 @@ void LatticeMotionTable::initMotionModel( } } -MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node) +MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives( + const NodeLattice * node, + unsigned int & direction_change_index) { MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta]; MotionPrimitivePtrs primitive_projection_list; @@ -119,6 +121,9 @@ MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * primitive_projection_list.push_back(&prims_at_heading[i]); } + // direction change index + direction_change_index = static_cast(primitive_projection_list.size()); + if (allow_reverse_expansion) { // Find normalized heading bin of the reverse expansion double reserve_heading = node->pose.theta - (num_angle_quantization / 2); @@ -470,17 +475,12 @@ void NodeLattice::getNeighbors( bool backwards = false; NodePtr neighbor = nullptr; Coordinates initial_node_coords, motion_projection; - MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this); + unsigned int direction_change_index = 0; + MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives( + this, + direction_change_index); const float & grid_resolution = motion_table.lattice_metadata.grid_resolution; - unsigned int direction_change_idx = 1e9; - for (unsigned int i = 0; i != motion_primitives.size(); i++) { - if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) { - direction_change_idx = i; - break; - } - } - for (unsigned int i = 0; i != motion_primitives.size(); i++) { const MotionPose & end_pose = motion_primitives[i]->poses.back(); motion_projection.x = this->pose.x + (end_pose._x / grid_resolution); @@ -502,7 +502,7 @@ void NodeLattice::getNeighbors( // account in case the robot base footprint is asymmetric. backwards = false; angle = motion_projection.theta; - if (i >= direction_change_idx) { + if (i >= direction_change_index) { backwards = true; angle = motion_projection.theta - (motion_table.num_angle_quantization / 2); if (angle < 0) { diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index 16674d9dad..f64f022f71 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -120,9 +120,12 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info); nav2_smac_planner::NodeLattice aNode(0); + unsigned int direction_change_index = 0; aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0)); nav2_smac_planner::MotionPrimitivePtrs projections = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives( + &aNode, + direction_change_index); EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01); EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01); @@ -130,7 +133,9 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) EXPECT_NEAR( nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata( - filePath).grid_resolution, 0.05, 0.005); + filePath) + .grid_resolution, + 0.05, 0.005); } TEST(NodeLatticeTest, test_node_lattice_conversions) @@ -248,7 +253,6 @@ TEST(NodeLatticeTest, test_node_lattice) delete costmapA; } - TEST(NodeLatticeTest, test_get_neighbors) { auto lnode = std::make_shared("test"); @@ -356,9 +360,12 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint) node.pose.x = 20; node.pose.y = 20; node.pose.theta = 0; + + // initialize direction change index + unsigned int direction_change_index = 0; // Test that the node is valid though all motion primitives poses for custom footprint nav2_smac_planner::MotionPrimitivePtrs motion_primitives = - nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node); + nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node, direction_change_index); EXPECT_GT(motion_primitives.size(), 0u); for (unsigned int i = 0; i < motion_primitives.size(); i++) { EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true); From 133d1a90647688b4820d20f371fb1da6523078fc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 5 Jan 2024 17:46:14 -0800 Subject: [PATCH 05/13] adding github action lint (#4036) * adding github action lint * move linting file Signed-off-by: Steve Macenski * try all wildcard * stringify * removing copyright check --------- Signed-off-by: Steve Macenski --- .github/workflows/lint.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 0000000000..635086a522 --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,21 @@ +name: Lint +on: + pull_request: + +jobs: + ament_lint_general: + name: ament_${{ matrix.linter }} + runs-on: ubuntu-latest + container: + image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + strategy: + fail-fast: false + matrix: + linter: [xmllint, cpplint, uncrustify, pep257, flake8] + steps: + - uses: actions/checkout@v2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + linter: ${{ matrix.linter }} + distribution: rolling + package-name: "*" From 8e90477c8a7afbc9f55de7b571ad6a6a0f381c00 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 5 Jan 2024 18:16:41 -0800 Subject: [PATCH 06/13] debug visualizations for lattice planner (#4034) * debug visualizations for lattice planner Signed-off-by: Steve Macenski * fix a typo * fix test * Update planner_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_planner/src/planner_server.cpp | 4 +- nav2_smac_planner/README.md | 2 +- .../smac_planner_lattice.hpp | 6 ++ .../src/smac_planner_lattice.cpp | 70 ++++++++++++++++++- nav2_smac_planner/test/test_smac_hybrid.cpp | 1 + nav2_smac_planner/test/test_smac_lattice.cpp | 1 + 6 files changed, 80 insertions(+), 4 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index af28b1b1eb..9a7b7912ae 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -644,8 +644,8 @@ void PlannerServer::isPathValid( /** * The lethal check starts at the closest point to avoid points that have already been passed - * and may have become occupied. The method for collision detection is based on the shape of - * the footprint. + * and may have become occupied. The method for collision detection is based on the shape of + * the footprint. */ std::unique_lock lock(*(costmap_->getMutex())); unsigned int mx = 0; diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 5f437ce546..5eee580bc8 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,7 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. - debug_visualizations: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + debug_visualizations: True # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index f5207cd45c..ce8bafd2fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -27,6 +27,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" @@ -111,6 +112,11 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; double _lookup_table_size; + bool _debug_visualizations; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _expansions_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index d3ddf5bdef..392c75d619 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -126,6 +126,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".allow_reverse_expansion", rclcpp::ParameterValue(false)); node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion); + nav2_util::declare_parameter_if_not_declared( + node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".debug_visualizations", _debug_visualizations); _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = @@ -193,6 +196,12 @@ void SmacPlannerLattice::configure( _smoother->initialize(_metadata.min_turning_radius); } + if (_debug_visualizations) { + _expansions_publisher = node->create_publisher("expansions", 1); + _planned_footprints_publisher = node->create_publisher( + "planned_footprints", 1); + } + RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerLattice with " "maximum iterations %i, max on approach iterations %i, " @@ -208,6 +217,10 @@ void SmacPlannerLattice::activate() _logger, "Activating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_activate(); + if (_debug_visualizations) { + _expansions_publisher->on_activate(); + _planned_footprints_publisher->on_activate(); + } auto node = _node.lock(); // Add callback for dynamic parameters _dyn_params_handler = node->add_on_set_parameters_callback( @@ -220,6 +233,10 @@ void SmacPlannerLattice::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerLattice", _name.c_str()); _raw_plan_publisher->on_deactivate(); + if (_debug_visualizations) { + _expansions_publisher->on_deactivate(); + _planned_footprints_publisher->on_deactivate(); + } _dyn_params_handler.reset(); } @@ -282,11 +299,30 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::CoordinateVector path; int num_iterations = 0; std::string error; + std::unique_ptr>> expansions = nullptr; + if (_debug_visualizations) { + expansions = std::make_unique>>(); + } // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + path, num_iterations, + _tolerance / static_cast(_costmap->getResolution()), expansions.get())) { + if (_debug_visualizations) { + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + } + // Note: If the start is blocked only one iteration will occur before failure if (num_iterations == 1) { throw nav2_core::StartOccupied("Start occupied"); @@ -324,6 +360,38 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _raw_plan_publisher->publish(plan); } + if (_debug_visualizations) { + // Publish expansions for debug + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg_pose.orientation = getWorldOrientation(std::get<2>(e)); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + + // plot footprint path planned for debug + if (_planned_footprints_publisher->get_subscription_count() > 0) { + auto marker_array = std::make_unique(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); + } + + if (marker_array->markers.empty()) { + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + } + _planned_footprints_publisher->publish(std::move(marker_array)); + } + } + // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 3be1a9e897..aae4666edc 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -45,6 +45,7 @@ TEST(SmacTest, test_smac_se2) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 = std::make_shared("SmacSE2Test"); + nodeSE2->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 9ce99a46b1..b791f9961c 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -54,6 +54,7 @@ TEST(SmacTest, test_smac_lattice) { rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice = std::make_shared("SmacLatticeTest"); + nodeLattice->declare_parameter("test.debug_visualizations", rclcpp::ParameterValue(true)); std::shared_ptr costmap_ros = std::make_shared("global_costmap"); From 870b9536fa5572370c62155c96c7987dfc2e3468 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 8 Jan 2024 11:31:08 -0800 Subject: [PATCH 07/13] Update vision_opencv's branch for rolling Signed-off-by: Steve Macenski --- tools/underlay.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/underlay.repos b/tools/underlay.repos index 2e092f2517..96772eafd1 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -14,7 +14,7 @@ repositories: # ros-perception/vision_opencv: # type: git # url: https://github.com/ros-perception/vision_opencv.git - # version: ros2 + # version: rolling # ros/bond_core: # type: git # url: https://github.com/ros/bond_core.git From b4ea7f5843c0c33f9daa874b7c694d19d052cb70 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 8 Jan 2024 16:00:28 -0800 Subject: [PATCH 08/13] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/ch?= =?UTF-8?q?eckout=20from=202=20to=204=20(#4040)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/checkout](https://github.com/actions/checkout) from 2 to 4. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v2...v4) --- updated-dependencies: - dependency-name: actions/checkout dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/lint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 635086a522..8732b239d9 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -13,7 +13,7 @@ jobs: matrix: linter: [xmllint, cpplint, uncrustify, pep257, flake8] steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - uses: ros-tooling/action-ros-lint@v0.1 with: linter: ${{ matrix.linter }} From 5f7f83a12168b74d64c3f054de09f2eb1a23adc0 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 10 Jan 2024 11:54:55 -0800 Subject: [PATCH 09/13] linting for new lint bots catching issues not previously reported (#4044) * linting for new lint bots * moar * moar2 * try without dashes * more * more --- nav2_amcl/src/amcl_node.cpp | 10 +++--- nav2_bringup/params/nav2_params.yaml | 32 ++++++++++++++++--- nav2_collision_monitor/src/polygon.cpp | 1 - .../test/collision_detector_node_test.cpp | 4 +-- .../test/collision_monitor_node_test.cpp | 3 +- .../nav2_common/launch/rewritten_yaml.py | 1 + .../nav2_costmap_2d/costmap_2d_ros.hpp | 6 ++-- .../denoise/image_processing.hpp | 2 +- .../include/nav2_costmap_2d/footprint.hpp | 1 + .../nav2_costmap_2d/inflation_layer.hpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 8 +++-- .../integration/test_costmap_2d_publisher.cpp | 2 +- .../integration/test_costmap_subscriber.cpp | 18 +++++++++-- .../test/regression/order_layer.hpp | 6 ++-- .../test/regression/plugin_api_order.cpp | 4 +-- .../test/unit/denoise_layer_test.cpp | 4 +-- .../test/unit/image_processing_test.cpp | 4 +-- nav2_costmap_2d/test/unit/image_test.cpp | 2 +- .../test/unit/image_tests_helper.hpp | 6 ++-- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 4 ++- .../nav2_simple_commander/costmap_2d.py | 8 ----- .../nav2_simple_commander/robot_navigator.py | 1 + .../test/test_footprint_collision_checker.py | 1 + .../test/test_line_iterator.py | 1 + .../src/costmap_filters/tester_node.py | 1 + .../src/gps_navigation/tester.py | 1 + .../system/nav_through_poses_tester_node.py | 1 + .../src/system/nav_to_pose_tester_node.py | 1 + .../src/system_failure/tester_node.py | 1 + .../src/waypoint_follower/tester.py | 1 + 30 files changed, 90 insertions(+), 47 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index dd03b3be02..85dc80b39c 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1167,7 +1167,7 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha1_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha1 to be negative," @@ -1177,7 +1177,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha2_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha2 to be negative," @@ -1187,7 +1187,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha3_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha3 to be negative," @@ -1197,7 +1197,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha4_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha4 to be negative," @@ -1207,7 +1207,7 @@ AmclNode::dynamicParametersCallback( reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - //alpha restricted to be non-negative + // alpha restricted to be non-negative if (alpha5_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha5 to be negative," diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 5f0e6e775d..0e3427a17b 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -277,12 +277,34 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + GridBased: + plugin: "nav2_smac_planner/SmacPlannerLattice" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + debug_visualizations: true + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index f78f9291ef..53480a0974 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -409,7 +409,6 @@ bool Polygon::getParameters( // Do not need to proceed further, if "points" parameter is defined. // Static polygon will be used. return getPolygonFromString(poly_string, poly_); - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { RCLCPP_INFO( logger_, diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 1ac519111f..e36ef6e583 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -162,9 +162,9 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; - }; // Tester Tester::Tester() diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 242b9f72db..757956f3f8 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -195,7 +195,8 @@ class Tester : public ::testing::Test nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; // CollisionMonitor collision points markers - rclcpp::Subscription::SharedPtr collision_points_marker_sub_; + rclcpp::Subscription::SharedPtr + collision_points_marker_sub_; visualization_msgs::msg::MarkerArray::SharedPtr collision_points_marker_msg_; // Service client for setting CollisionMonitor parameters diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index c28dc88cc0..11aa77227e 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -20,6 +20,7 @@ class DictItemReference: + def __init__(self, dictionary, key): self.dictionary = dictionary self.dictKey = key diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index b1ddedceeb..69f2cfefa1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -77,7 +77,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @brief Constructor for the wrapper * @param options Additional options to control creation of the node. */ - Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Constructor for the wrapper, the node will @@ -399,12 +399,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode double resolution_{0}; std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; double transform_tolerance_{0}; ///< The timeout before transform errors double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors - bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node // Derived parameters bool use_radius_{false}; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 99d24240d2..3a2f6a77d2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -81,7 +81,7 @@ void morphologyOperation( using ShapeBuffer3x3 = std::array; // NOLINT inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); -} // namespace imgproc_impl +} // namespace imgproc_impl /** * @brief Perform morphological dilation diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp index 8a5fc206f1..6590a4682e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon.hpp" 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 aa0e0f13c2..0ce4b008d9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -131,7 +131,7 @@ class InflationLayer : public Layer /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() override {return false;} + bool isClearable() override {return false;} /** * @brief Reset this costmap diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 501f675736..2a36bab959 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -289,7 +289,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) // Check timeout if (now() > initial_transform_timeout_point) { RCLCPP_ERROR( - get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", + get_logger(), + "Failed to activate %s because " + "transform from %s to %s did not become available before timeout", get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); return nav2_util::CallbackReturn::FAILURE; @@ -523,7 +525,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->updateBounds(x0, xn, y0, yn); } @@ -535,7 +537,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - for (auto & layer_pub: layer_publishers_) { + for (auto & layer_pub : layer_publishers_) { layer_pub->publishCostmap(); } diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 790466f38b..68a9875ef6 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -162,7 +162,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map - ASSERT_TRUE(costmap_raw->data.size() == 100); + ASSERT_EQ(costmap_raw->data.size(), 100u); unsigned int i = 0; for (; i < 7; ++i) { EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 65fa120cd6..d674ee7669 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -1,3 +1,17 @@ +// Copyright (c) 2020 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include #include "rclcpp/rclcpp.hpp" @@ -119,7 +133,7 @@ TEST_F(TestCostmapSubscriberShould, handleFullCostmapMsgs) node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); - for (const auto & mapChange :mapChanges) { + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); } @@ -153,7 +167,7 @@ TEST_F(TestCostmapSubscriberShould, handleCostmapUpdateMsgs) node, costmapToSend.get(), "", topicName, always_send_full_costmap); costmapPublisher->on_activate(); - for (const auto & mapChange :mapChanges) { + for (const auto & mapChange : mapChanges) { for (const auto & observation : mapChange.observations) { costmapToSend->setCost(observation.x, observation.y, observation.cost); } diff --git a/nav2_costmap_2d/test/regression/order_layer.hpp b/nav2_costmap_2d/test/regression/order_layer.hpp index 35bb2aeabb..01955f0ba9 100644 --- a/nav2_costmap_2d/test/regression/order_layer.hpp +++ b/nav2_costmap_2d/test/regression/order_layer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ -#define NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#ifndef REGRESSION__ORDER_LAYER_HPP_ +#define REGRESSION__ORDER_LAYER_HPP_ #include "nav2_costmap_2d/layer.hpp" @@ -43,4 +43,4 @@ class OrderLayer : public nav2_costmap_2d::Layer } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__ORDER_LAYER_HPP_ +#endif // REGRESSION__ORDER_LAYER_HPP_ diff --git a/nav2_costmap_2d/test/regression/plugin_api_order.cpp b/nav2_costmap_2d/test/regression/plugin_api_order.cpp index c7baf34453..f7a00e90d2 100644 --- a/nav2_costmap_2d/test/regression/plugin_api_order.cpp +++ b/nav2_costmap_2d/test/regression/plugin_api_order.cpp @@ -16,8 +16,8 @@ #include #include -#include -#include +#include "gtest/gtest.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" TEST(CostmapPluginsTester, checkPluginAPIOrder) { diff --git a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp index 37c277f900..558fb26361 100644 --- a/nav2_costmap_2d/test/unit/denoise_layer_test.cpp +++ b/nav2_costmap_2d/test/unit/denoise_layer_test.cpp @@ -102,9 +102,9 @@ class DenoiseLayerTester : public ::testing::Test nav2_costmap_2d::DenoiseLayer denoise_; }; -} +} // namespace nav2_costmap_2d -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT TEST_F(DenoiseLayerTester, removeSinglePixels4way) { const auto in = imageFromString( diff --git a/nav2_costmap_2d/test/unit/image_processing_test.cpp b/nav2_costmap_2d/test/unit/image_processing_test.cpp index cc177d699a..f56ca58149 100644 --- a/nav2_costmap_2d/test/unit/image_processing_test.cpp +++ b/nav2_costmap_2d/test/unit/image_processing_test.cpp @@ -18,8 +18,8 @@ #include "nav2_costmap_2d/denoise/image_processing.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; -using namespace imgproc_impl; +using namespace nav2_costmap_2d; // NOLINT +using namespace imgproc_impl; // NOLINT struct ImageProcTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_test.cpp b/nav2_costmap_2d/test/unit/image_test.cpp index 83912d0622..7ab796e155 100644 --- a/nav2_costmap_2d/test/unit/image_test.cpp +++ b/nav2_costmap_2d/test/unit/image_test.cpp @@ -17,7 +17,7 @@ #include "nav2_costmap_2d/denoise/image.hpp" #include "image_tests_helper.hpp" -using namespace nav2_costmap_2d; +using namespace nav2_costmap_2d; // NOLINT struct ImageTester : public ::testing::Test { diff --git a/nav2_costmap_2d/test/unit/image_tests_helper.hpp b/nav2_costmap_2d/test/unit/image_tests_helper.hpp index 3eee735f7e..2ffe8196ba 100644 --- a/nav2_costmap_2d/test/unit/image_tests_helper.hpp +++ b/nav2_costmap_2d/test/unit/image_tests_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ -#define NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#ifndef UNIT__IMAGE_TESTS_HELPER_HPP_ +#define UNIT__IMAGE_TESTS_HELPER_HPP_ #include "nav2_costmap_2d/denoise/image.hpp" #include @@ -121,4 +121,4 @@ std::ostream & operator<<(std::ostream & out, const Image & image) } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__IMAGE_TESTS_HELPER_HPP_ +#endif // UNIT__IMAGE_TESTS_HELPER_HPP_ diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index a8ab817df3..e54264ebea 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -1,3 +1,5 @@ +// Copyright (c) 2020 Samsung Research +// // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at @@ -8,7 +10,7 @@ // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and -// limitations under the License. +// limitations under the License. Reserved. #include diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index 1ae7e808b6..2ddc9cb00f 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -34,14 +34,6 @@ class PyCostmap2D: """ def __init__(self, occupancy_map): - """ - Initialize costmap2D. - - Args: - ---- - occupancy_map (OccupancyGrid): 2D OccupancyGrid Map - - """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height self.resolution = occupancy_map.info.resolution diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 17d2f130e8..5f9e355a2f 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -51,6 +51,7 @@ class TaskResult(Enum): class BasicNavigator(Node): + def __init__(self, node_name='basic_navigator', namespace=''): super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 070a7bc6e4..8ffc4b653b 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -23,6 +23,7 @@ class TestFootprintCollisionChecker(unittest.TestCase): + def test_no_costmap(self): # Test if a type error raised when costmap is not specified yet fcc_ = FootprintCollisionChecker() diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py index 1360c183e7..8bfb91588b 100644 --- a/nav2_simple_commander/test/test_line_iterator.py +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -19,6 +19,7 @@ class TestLineIterator(unittest.TestCase): + def test_type_error(self): # Test if a type error raised when passing invalid arguements types self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index da8ab82a20..3b3f723095 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -47,6 +47,7 @@ class TestType(Enum): class FilterMask: + def __init__(self, filter_mask: OccupancyGrid): self.filter_mask = filter_mask diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 5e765981ab..b9fb4725ef 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -30,6 +30,7 @@ class GpsWaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_gps_waypoint_tester', namespace='') self.waypoints = None diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index 5e587ad8fa..2bcea31931 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index cbb2ac20e6..8fd1bc6034 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,6 +38,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 765535c6e5..9cea2f6118 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -37,6 +37,7 @@ class NavTester(Node): + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 7eb4645706..2a3ef3df53 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -31,6 +31,7 @@ class WaypointFollowerTest(Node): + def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None From 370dbfd5c8b26ebdb45d99bb3613166b0c5e79b8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 11 Jan 2024 00:59:35 +0100 Subject: [PATCH 10/13] Bring back pep257 docstring (#4045) * test Signed-off-by: Tony Najjar * . Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar --- .../nav2_simple_commander/costmap_2d.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py index 2ddc9cb00f..ad4014522a 100644 --- a/nav2_simple_commander/nav2_simple_commander/costmap_2d.py +++ b/nav2_simple_commander/nav2_simple_commander/costmap_2d.py @@ -34,6 +34,18 @@ class PyCostmap2D: """ def __init__(self, occupancy_map): + """ + Initialize costmap2D. + + Args + ---- + occupancy_map (OccupancyGrid): 2D OccupancyGrid Map + + Returns + ------- + None + + """ self.size_x = occupancy_map.info.width self.size_y = occupancy_map.info.height self.resolution = occupancy_map.info.resolution From 569f1672609c22bc52c4c4c486818fde3bd99646 Mon Sep 17 00:00:00 2001 From: Sebastian Solarte <89881453+Sunart24@users.noreply.github.com> Date: Thu, 11 Jan 2024 14:08:57 -0500 Subject: [PATCH 11/13] handle dynamically changes in parameters. (#4046) Signed-off-by: Sebastian Solarte --- nav2_mppi_controller/src/optimizer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3b053f4577..3c5d203289 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -123,6 +123,8 @@ void Optimizer::reset() control_history_[2] = {0.0, 0.0, 0.0}; control_history_[3] = {0.0, 0.0, 0.0}; + settings_.constraints = settings_.base_constraints; + costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); From 29321d661f6acb505192d4b24aa983fe10a733c5 Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Fri, 12 Jan 2024 13:02:39 +0900 Subject: [PATCH 12/13] Add inflation_layer_name param (#4047) Signed-off-by: Renan Salles --- nav2_mppi_controller/README.md | 1 + .../nav2_mppi_controller/critics/obstacles_critic.hpp | 4 +++- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 6 +++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 4f5a9398d1..fd7ad80214 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -107,6 +107,7 @@ This process is then repeated a number of times and returns a converged solution | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | #### Path Align Critic | Parameter | Type | Definition | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index fd6eeab012..358994c238 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -16,9 +16,10 @@ #define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ #include +#include + #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" - #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" @@ -96,6 +97,7 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; + std::string inflation_layer_name_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 274315a06b..ecb22b295b 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -29,6 +29,7 @@ void ObstaclesCritic::initialize() getParam(collision_cost_, "collision_cost", 100000.0); getParam(collision_margin_distance_, "collision_margin_distance", 0.10); getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); @@ -69,7 +70,10 @@ float ObstaclesCritic::findCircumscribedCost( ++layer) { auto inflation_layer = std::dynamic_pointer_cast(*layer); - if (!inflation_layer) { + if (!inflation_layer || + (!inflation_layer_name_.empty() && + inflation_layer->getName() != inflation_layer_name_)) + { continue; } From a8a0c3a7feae45a3c44e0d22d69c0c4d13ba0c05 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Sun, 14 Jan 2024 16:01:37 +0100 Subject: [PATCH 13/13] missing urdf dep (#4050) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_rviz_plugins/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 3de0d3c9f5..b4d0a8ed01 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -25,6 +25,7 @@ rviz_rendering std_msgs tf2_geometry_msgs + urdf visualization_msgs yaml_cpp_vendor