Skip to content

Commit

Permalink
Fix cppcheck errors (#1718)
Browse files Browse the repository at this point in the history 
* Fix cppcheck errors

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix test name generator

* Fix returning after it is deallocated / released error

* Increase sleep to 10 seconds to allow map server node to start up

* Fix infinite wait for service in nav2_map_server tests
  • Loading branch information
@naiveHobo
naiveHobo authored May 11, 2020
1 parent 7addcb7 commit 536ed1f
Show file tree
Hide file tree
Showing 8 changed files with 145 additions and 89 deletions.
26 changes: 13 additions & 13 deletions nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,28 +77,28 @@ unsigned int countValues(
return count;
}

nav2_costmap_2d::StaticLayer * addStaticLayer(
void addStaticLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
{
nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer();
slayer = std::make_shared<nav2_costmap_2d::StaticLayer>();
layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/);
return slayer;
}

nav2_costmap_2d::ObstacleLayer * addObstacleLayer(
void addObstacleLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer)
{
nav2_costmap_2d::ObstacleLayer * olayer = new nav2_costmap_2d::ObstacleLayer();
olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>();
olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/);
layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(olayer));
return olayer;
}

void addObservation(
nav2_costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0,
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z)
{
sensor_msgs::msg::PointCloud2 cloud;
Expand All @@ -122,15 +122,15 @@ void addObservation(
olayer->addStaticObservation(obs, true, true);
}

nav2_costmap_2d::InflationLayer * addInflationLayer(
void addInflationLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer)
{
nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer();
ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>();
ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/);
std::shared_ptr<nav2_costmap_2d::Layer> ipointer(ilayer);
layers.addPlugin(ipointer);
return ilayer;
}


Expand Down
71 changes: 51 additions & 20 deletions nav2_costmap_2d/test/integration/inflation_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,12 @@ class TestNode : public ::testing::Test
void validatePointInflation(
unsigned int mx, unsigned int my,
nav2_costmap_2d::Costmap2D * costmap,
nav2_costmap_2d::InflationLayer * ilayer,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius);

void initNode(double inflation_radius);

void waitForMap(nav2_costmap_2d::StaticLayer * slayer);
void waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer);

protected:
nav2_util::LifecycleNode::SharedPtr node_;
Expand Down Expand Up @@ -106,7 +106,7 @@ std::vector<Point> TestNode::setRadii(
return polygon;
}

void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer)
void TestNode::waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
{
while (!slayer->isCurrent()) {
rclcpp::spin_some(node_->get_node_base_interface());
Expand All @@ -117,7 +117,7 @@ void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer)
void TestNode::validatePointInflation(
unsigned int mx, unsigned int my,
nav2_costmap_2d::Costmap2D * costmap,
nav2_costmap_2d::InflationLayer * ilayer,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius)
{
bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
Expand Down Expand Up @@ -206,8 +206,12 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove)
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

addObservation(olayer, 0, 0, MAX_Z);
Expand All @@ -234,8 +238,12 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns)
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

addObservation(olayer, 0, 0, MAX_Z);
Expand All @@ -260,8 +268,12 @@ TEST_F(TestNode, testCostFunctionCorrectness)
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

addObservation(olayer, 50, 50, MAX_Z);
Expand Down Expand Up @@ -331,8 +343,12 @@ TEST_F(TestNode, testInflationOrderCorrectness)
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

// Add two diagonal cells, they would induce problems under the
Expand All @@ -359,9 +375,14 @@ TEST_F(TestNode, testInflation)
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1);

auto slayer = addStaticLayer(layers, tf, node_);
nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);

std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);

nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
Expand Down Expand Up @@ -432,9 +453,15 @@ TEST_F(TestNode, testInflation2)
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1);

auto slayer = addStaticLayer(layers, tf, node_);
nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);

std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

waitForMap(slayer);
Expand Down Expand Up @@ -464,8 +491,12 @@ TEST_F(TestNode, testInflation3)
// 1 2 3
std::vector<Point> polygon = setRadii(layers, 1, 1.75);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
addInflationLayer(layers, tf, node_);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);

std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);

layers.setFootprint(polygon);

// There should be no occupied cells
Expand Down
16 changes: 9 additions & 7 deletions nav2_costmap_2d/test/integration/obstacle_tests.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,7 +157,7 @@ TEST_F(TestNode, testRaytracing) {

nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// Add a point at 0, 0, 0
addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
Expand All @@ -179,7 +179,7 @@ TEST_F(TestNode, testRaytracing2) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// If we print map now, it is 10x10 all value 0
// printMap(*(layers.getCostmap()));
Expand Down Expand Up @@ -236,7 +236,7 @@ TEST_F(TestNode, testWaveInterference) {
// Start with an empty map, no rolling window, tracking unknown
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
layers.resizeMap(10, 10, 1, 0, 0);
nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
// printMap(*(layers.getCostmap()));
Expand Down Expand Up @@ -265,7 +265,7 @@ TEST_F(TestNode, testZThreshold) {
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
layers.resizeMap(10, 10, 1, 0, 0);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// A point cloud with 2 points falling in a cell with a non-lethal cost
addObservation(olayer, 0.0, 5.0, 0.4);
Expand All @@ -285,7 +285,7 @@ TEST_F(TestNode, testDynamicObstacles) {
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// Add a point cloud and verify its insertion. There should be only one new one
addObservation(olayer, 0.0, 0.0);
Expand All @@ -310,7 +310,7 @@ TEST_F(TestNode, testMultipleAdditions) {
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);

nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);

// A point cloud with one point that falls within an existing obstacle
addObservation(olayer, 9.5, 0.0);
Expand All @@ -327,7 +327,9 @@ TEST_F(TestNode, testMultipleAdditions) {
TEST_F(TestNode, testRepeatedResets) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);

std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);

// TODO(orduno) Add obstacle layer

Expand Down
7 changes: 5 additions & 2 deletions nav2_costmap_2d/test/integration/test_collision_checker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,12 +128,15 @@ class TestCollisionChecker : public nav2_util::LifecycleNode

layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
// Add Static Layer
auto slayer = addStaticLayer(*layers_, *tf_buffer_, shared_from_this());
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer);

while (!slayer->isCurrent()) {
rclcpp::spin_some(this->get_node_base_interface());
}
// Add Inflation Layer
addInflationLayer(*layers_, *tf_buffer_, shared_from_this());
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer);

return nav2_util::CallbackReturn::SUCCESS;
}
Expand Down
39 changes: 19 additions & 20 deletions nav2_map_server/test/component/test_map_saver_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,25 +39,23 @@ class RclCppFixture

RclCppFixture g_rclcppfixture;

class TestNode : public ::testing::Test
class MapSaverTestFixture : public ::testing::Test
{
public:
TestNode()
static void SetUpTestCase()
{
node_ = rclcpp::Node::make_shared("map_client_test");
map_server_node_name_ = "map_saver";
lifecycle_client_ =
std::make_shared<nav2_util::LifecycleServiceClient>(map_server_node_name_, node_);
std::make_shared<nav2_util::LifecycleServiceClient>("map_saver", node_);
RCLCPP_INFO(node_->get_logger(), "Creating Test Node");


std::this_thread::sleep_for(std::chrono::seconds(1)); // allow node to start up
std::this_thread::sleep_for(std::chrono::seconds(5)); // allow node to start up
const std::chrono::seconds timeout(5);
lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
}

~TestNode()
static void TearDownTestCase()
{
lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
Expand All @@ -73,10 +71,7 @@ class TestNode : public ::testing::Test
auto result = client->async_send_request(request);

// Wait for the result
if (
rclcpp::spin_until_future_complete(node, result) ==
rclcpp::executor::FutureReturnCode::SUCCESS)
{
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
return result.get();
} else {
return nullptr;
Expand All @@ -96,19 +91,23 @@ class TestNode : public ::testing::Test
}
}

std::string map_server_node_name_;
rclcpp::Node::SharedPtr node_;
std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
static rclcpp::Node::SharedPtr node_;
static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
};


rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
nullptr;

// Send map saving service request.
// Load saved map and verify obtained OccupancyGrid.
TEST_F(TestNode, SaveMap)
TEST_F(MapSaverTestFixture, SaveMap)
{
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/" + map_server_node_name_ + "/save_map");
"/map_saver/save_map");

RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
ASSERT_TRUE(client->wait_for_service());
Expand All @@ -132,12 +131,12 @@ TEST_F(TestNode, SaveMap)

// Send map saving service request with default parameters.
// Load saved map and verify obtained OccupancyGrid.
TEST_F(TestNode, SaveMapDefaultParameters)
TEST_F(MapSaverTestFixture, SaveMapDefaultParameters)
{
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/" + map_server_node_name_ + "/save_map");
"/map_saver/save_map");

RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
ASSERT_TRUE(client->wait_for_service());
Expand All @@ -162,12 +161,12 @@ TEST_F(TestNode, SaveMapDefaultParameters)
// Send map saving service requests with different sets of parameters.
// In case of map is expected to be saved correctly, load map from a saved
// file and verify obtained OccupancyGrid.
TEST_F(TestNode, SaveMapInvalidParameters)
TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
{
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/" + map_server_node_name_ + "/save_map");
"/map_saver/save_map");

RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
ASSERT_TRUE(client->wait_for_service());
Expand Down
Loading

0 comments on commit 536ed1f

Please sign in to comment.