Skip to content
Snippets Groups Projects
Unverified Commit 635d6bd4 authored by Steven Macenski's avatar Steven Macenski Committed by GitHub
Browse files

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
parent 2a9ad80a
No related branches found
No related tags found
No related merge requests found
......@@ -189,6 +189,12 @@ protected:
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_;
......
......@@ -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);
}
}
......
......@@ -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;
}
}
......
......@@ -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);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment