From 2a9ad80a41fe19f278a38c0a82d14d1120a2f07e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Aug 2020 13:30:46 -0700 Subject: [PATCH 1/4] see if spin some in cancel will allow result callback (#1914) --- nav2_waypoint_follower/src/waypoint_follower.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 7f941e490e1..a0f879d1c0c 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -134,6 +134,8 @@ WaypointFollower::followWaypoints() if (action_server_->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); rclcpp::spin_until_future_complete(client_node_, cancel_future); + // for result callback processing + spin_some(client_node_); action_server_->terminate_all(); return; } From 635d6bd4d642ac4b857f302936514ad5cd6ba6b5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Aug 2020 16:44:07 -0700 Subject: [PATCH 2/4] Adding near-complete voxel grid test coverage and more to controller server (#1915) * remove erraneous handling done by prior * adding a bunch of voxel unit tests * retrigger --- .../nav2_controller/nav2_controller.hpp | 6 +++ nav2_controller/src/nav2_controller.cpp | 9 ++--- nav2_planner/src/planner_server.cpp | 9 ----- nav2_voxel_grid/test/voxel_grid_tests.cpp | 40 +++++++++++++++++++ 4 files changed, 49 insertions(+), 15 deletions(-) diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 3180b2c1440..49ff57dd1ff 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -189,6 +189,12 @@ class ControllerServer : public nav2_util::LifecycleNode return twist_thresh; } + void pluginFailed(const std::string & name, const pluginlib::PluginlibException & ex) + { + RCLCPP_FATAL(get_logger(), "Failed to create %s. Exception: %s", name.c_str(), ex.what()); + exit(-1); + } + // The controller needs a costmap node std::shared_ptr costmap_ros_; std::unique_ptr costmap_thread_; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 7d91e0128fd..0283e224acd 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -113,8 +113,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) progress_checker_id_.c_str(), progress_checker_type_.c_str()); progress_checker_->initialize(node, progress_checker_id_); } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); - exit(-1); + pluginFailed("progress_checker", ex); } try { goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); @@ -124,8 +123,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) goal_checker_id_.c_str(), goal_checker_type_.c_str()); goal_checker_->initialize(node, goal_checker_id_); } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); - exit(-1); + pluginFailed("goal_checker", ex); } for (size_t i = 0; i != controller_ids_.size(); i++) { @@ -141,8 +139,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->getTfBuffer(), costmap_ros_); controllers_.insert({controller_ids_[i], controller}); } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); - exit(-1); + pluginFailed("controller", ex); } } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 24f104f6f7e..e6919c913fd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -299,15 +299,6 @@ PlannerServer::computePlan() // for example: couldn't get costmap update action_server_->terminate_current(); return; - } catch (...) { - RCLCPP_WARN( - get_logger(), "Plan calculation failed, " - "An unexpected error has occurred. The planner server" - " may not be able to continue operating correctly."); - // TODO(orduno): provide information about fail error to parent task, - // for example: couldn't get costmap update - action_server_->terminate_current(); - return; } } diff --git a/nav2_voxel_grid/test/voxel_grid_tests.cpp b/nav2_voxel_grid/test/voxel_grid_tests.cpp index 6ff58812ff8..cfc596f6ae2 100644 --- a/nav2_voxel_grid/test/voxel_grid_tests.cpp +++ b/nav2_voxel_grid/test/voxel_grid_tests.cpp @@ -147,6 +147,46 @@ TEST(voxel_grid, InvalidSize) { EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN); } +TEST(voxel_grid, MarkAndClear) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(5, 5, 5, 0); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED); + vg.clearVoxelColumn(55); + EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE); +} + +TEST(voxel_grid, clearVoxelLineInMap) { + int size_x = 10, size_y = 10, size_z = 10; + nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z); + vg.markVoxelInMap(0, 0, 5, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED); + + unsigned char * map_2d = new unsigned char[100]; + map_2d[0] = 254; + + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0); + + EXPECT_EQ(map_2d[0], 0); + + vg.markVoxelInMap(0, 0, 5, 0); + vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE); + delete[] map_2d; +} + +TEST(voxel_grid, GetVoxelData) { + uint32_t * data = new uint32_t[9]; + data[4] = 255; + data[0] = 0; + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN); + + EXPECT_EQ( + nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE); + delete[] data; +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 2496218836301f53616af06c9a79c34d8dc7dace Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Aug 2020 18:13:43 -0700 Subject: [PATCH 3/4] adding waypoint follower failure test, voxel layer integration tests, etc (#1916) * adding waypoint follower failure test * adding voxel, more logging * reset -> clear --- nav2_bringup/bringup/params/nav2_params.yaml | 38 +++---------------- nav2_controller/src/nav2_controller.cpp | 10 ++++- .../test/integration/inflation_tests.cpp | 2 +- .../test/integration/obstacle_tests.cpp | 2 +- .../test_costmap_topic_collision_checker.cpp | 2 +- .../testing_helper.hpp | 0 nav2_planner/src/planner_server.cpp | 11 +++++- .../src/waypoint_follower/tester.py | 7 ++++ 8 files changed, 34 insertions(+), 38 deletions(-) rename nav2_costmap_2d/{include/nav2_costmap_2d => test}/testing_helper.hpp (100%) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index c92390f46d2..784eea19b38 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -164,20 +164,10 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] + plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -187,13 +177,13 @@ local_costmap: z_voxels: 16 max_obstacle_height: 2.0 mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points + observation_sources: scan + scan: + topic: /scan max_obstacle_height: 2.0 clearing: True marking: True - data_type: "PointCloud2" + data_type: "LaserScan" static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -214,7 +204,7 @@ global_costmap: use_sim_time: True robot_radius: 0.22 resolution: 0.05 - plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -225,22 +215,6 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: pointcloud - pointcloud: - topic: /intel_realsense_r200_depth/points - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 0283e224acd..76c7c695319 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -63,6 +63,8 @@ ControllerServer::ControllerServer() ControllerServer::~ControllerServer() { RCLCPP_INFO(get_logger(), "Destroying"); + controllers_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -147,6 +149,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) controller_ids_concat_ += controller_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); + odom_sub_ = std::make_unique(node); vel_publisher_ = create_publisher("cmd_vel", 1); @@ -309,7 +315,7 @@ void ControllerServer::computeControl() return; } - RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result"); + RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result"); publishZeroVelocity(); @@ -327,7 +333,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) } controllers_[current_controller_]->setPlan(path); - auto end_pose = *(path.poses.end() - 1); + auto end_pose = path.poses.back(); goal_checker_->reset(); RCLCPP_DEBUG( diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 40262ce9833..2e3aa19c53c 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -44,7 +44,7 @@ #include "nav2_costmap_2d/obstacle_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/node_utils.hpp" using geometry_msgs::msg::Point; diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 7e1910f5f76..a635cd8e6a8 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -41,7 +41,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" using std::begin; using std::end; diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 56baa03a4b4..ad1fb15d3a0 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -24,7 +24,7 @@ #include "nav2_costmap_2d/static_layer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" +#include "../testing_helper.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp similarity index 100% rename from nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp rename to nav2_costmap_2d/test/testing_helper.hpp diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e6919c913fd..c1ab640d7c4 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -67,6 +67,7 @@ PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); planners_.clear(); + costmap_thread_.reset(); } nav2_util::CallbackReturn @@ -110,6 +111,10 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) planner_ids_concat_ += planner_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Planner Server has %s planners available.", planner_ids_concat_.c_str()); + double expected_planner_frequency; get_parameter("expected_planner_frequency", expected_planner_frequency); if (expected_planner_frequency > 0) { @@ -190,6 +195,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) it->second->cleanup(); } planners_.clear(); + costmap_ = nullptr; return nav2_util::CallbackReturn::SUCCESS; } @@ -225,6 +231,7 @@ PlannerServer::computePlan() geometry_msgs::msg::PoseStamped start; if (!costmap_ros_->getRobotPose(start)) { RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); + action_server_->terminate_current(); return; } @@ -281,10 +288,12 @@ PlannerServer::computePlan() result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { + auto planner_period = 1 / max_planner_duration_; + auto cycle_period = 1 / cycle_duration.seconds(); RCLCPP_WARN( get_logger(), "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - 1 / max_planner_duration_, 1 / cycle_duration.seconds()); + planner_period, cycle_period); } action_server_->succeeded_current(result); diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 97ad511675f..6cdd557ad60 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -188,6 +188,7 @@ def main(argv=sys.argv[1:]): rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback result = test.run(True) + assert result # preempt with new point test.setWaypoints([starting_pose]) @@ -200,6 +201,12 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() + # a failure case + test.setWaypoints([[100.0, 100.0]]) + result = test.run(True) + assert not result + result = not result + test.shutdown() test.info_msg('Done Shutting Down.') From 4e2f39e202b43c13598ebea6bddb3e7276b9277f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 5 Aug 2020 19:15:29 -0700 Subject: [PATCH 4/4] minor changes to lower redundent warnings (#1918) --- nav2_controller/src/nav2_controller.cpp | 1 - nav2_planner/src/planner_server.cpp | 4 +--- nav2_system_tests/src/waypoint_follower/tester.py | 1 + 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 76c7c695319..dc9fb88d95b 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -424,7 +424,6 @@ bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) { geometry_msgs::msg::PoseStamped current_pose; if (!costmap_ros_->getRobotPose(current_pose)) { - RCLCPP_ERROR(this->get_logger(), "Could not get robot pose"); return false; } pose = current_pose; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c1ab640d7c4..d6d529f5d02 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -288,12 +288,10 @@ PlannerServer::computePlan() result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { - auto planner_period = 1 / max_planner_duration_; - auto cycle_period = 1 / cycle_duration.seconds(); RCLCPP_WARN( get_logger(), "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", - planner_period, cycle_period); + 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } action_server_->succeeded_current(result); diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 6cdd557ad60..7f5db902fba 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -202,6 +202,7 @@ def main(argv=sys.argv[1:]): test.cancel_goal() # a failure case + time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True) assert not result