Skip to content

Commit

Permalink
Adding near-complete voxel grid test coverage and more to controller …
Browse files Browse the repository at this point in the history
…server (#1915)

* remove erraneous handling done by prior

* adding a bunch of voxel unit tests

* retrigger
  • Loading branch information
SteveMacenski committed Aug 11, 2020
1 parent a88b5f1 commit f4d55e1
Show file tree
Hide file tree
Showing 4 changed files with 49 additions and 15 deletions.
6 changes: 6 additions & 0 deletions nav2_controller/include/nav2_controller/nav2_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
Expand Down
9 changes: 3 additions & 6 deletions nav2_controller/src/nav2_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand All @@ -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++) {
Expand All @@ -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);
}
}

Expand Down
9 changes: 0 additions & 9 deletions nav2_planner/src/planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
40 changes: 40 additions & 0 deletions nav2_voxel_grid/test/voxel_grid_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit f4d55e1

Please sign in to comment.