Skip to content

Commit c0a910e

Browse files
fmricofmrico
committed
Tests and final functionality
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
1 parent 5fb342e commit c0a910e

5 files changed

Lines changed: 143 additions & 46 deletions

File tree

‎easynav_gridmap_maps_manager/src/easynav_gridmap_maps_manager/GridmapMapsBuilderNode.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ GridmapMapsBuilderNode::GridmapMapsBuilderNode(const rclcpp::NodeOptions & optio
5757
}
5858

5959
pub_ = this->create_publisher<grid_map_msgs::msg::GridMap>(
60-
"map_builder_gridmap/gridmap", rclcpp::QoS(1).transient_local().reliable());
60+
"map_builder_gridmap/gridmap", 100);
6161

6262
register_handler(std::make_shared<PointPerceptionHandler>());
6363
}

‎easynav_gridmap_maps_manager/src/easynav_gridmap_maps_manager/GridmapMapsManager.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ GridmapMapsManager::on_initialize()
6767
node->get_parameter(plugin_name + ".package", package_name);
6868
node->get_parameter(plugin_name + ".map_path_file", map_path_file);
6969

70+
map_path_ = "/tmp/gridmap_map.yaml";
71+
7072
if (package_name != "" && map_path_file != "") {
7173
std::string pkgpath;
7274
try {
@@ -87,7 +89,7 @@ GridmapMapsManager::on_initialize()
8789

8890
incoming_map_sub_ = node->create_subscription<grid_map_msgs::msg::GridMap>(
8991
node->get_name() + std::string("/") + plugin_name + "/incoming_map",
90-
rclcpp::QoS(1).transient_local().reliable(),
92+
100,
9193
[this](grid_map_msgs::msg::GridMap::UniquePtr msg) {
9294
grid_map::GridMap incoming;
9395
if (grid_map::GridMapRosConverter::fromMessage(*msg, incoming, {"elevation"})) {
@@ -130,7 +132,7 @@ GridmapMapsManager::update(NavState & nav_state)
130132
EASYNAV_TRACE_EVENT;
131133

132134
if (map_need_update_) {
133-
nav_state.set("map", map_);
135+
nav_state.set("map.elevation", map_);
134136
map_need_update_ = false;
135137
}
136138
}

‎easynav_gridmap_maps_manager/tests/CMakeLists.txt‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,15 @@ target_link_libraries(gridmap_maps_builder_tests
77
${PROJECT_NAME} ${dependencies}
88
)
99

10+
ament_add_gtest(gridmap_maps_manager_tests gridmap_maps_manager_tests.cpp)
11+
target_include_directories(gridmap_maps_manager_tests PUBLIC
12+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
13+
$<INSTALL_INTERFACE:include>
14+
)
15+
target_link_libraries(gridmap_maps_manager_tests
16+
${PROJECT_NAME} ${dependencies}
17+
)
18+
1019
ament_add_gtest(gridmap_utils_tests gridmap_utils_tests.cpp)
1120
target_include_directories(gridmap_utils_tests PUBLIC
1221
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

‎easynav_gridmap_maps_manager/tests/gridmap_maps_builder_tests.cpp‎

Lines changed: 38 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,8 +50,7 @@ class GridmapMapsBuilderTest : public ::testing::Test
5050
using namespace std::chrono_literals;
5151

5252
TEST_F(GridmapMapsBuilderTest, test_configure_success)
53-
{
54-
rclcpp::NodeOptions options;
53+
{ rclcpp::NodeOptions options;
5554
options.append_parameter_override("use_sim_time", true);
5655
options.append_parameter_override("sensors", std::vector<std::string>{"map"});
5756
options.append_parameter_override("downsample_resolution", 1.0);
@@ -64,6 +63,15 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
6463
auto test_node = std::make_shared<rclcpp::Node>("test_node");
6564
auto pub = test_node->create_publisher<sensor_msgs::msg::PointCloud2>("map", 10);
6665

66+
grid_map_msgs::msg::GridMap::SharedPtr received_gridmap;
67+
auto sub = test_node->create_subscription<grid_map_msgs::msg::GridMap>(
68+
"/map_builder_gridmap/gridmap", 100,
69+
[&] (const grid_map_msgs::msg::GridMap::SharedPtr msg) {
70+
received_gridmap = msg;
71+
}
72+
);
73+
74+
6775
builder_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
6876
ASSERT_EQ(builder_node->get_current_state().id(),
6977
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -122,6 +130,7 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
122130

123131
rclcpp::executors::SingleThreadedExecutor executor;
124132
executor.add_node(builder_node->get_node_base_interface());
133+
executor.add_node(test_node);
125134

126135
auto start = test_node->now();
127136
while ((test_node->now() - start).seconds() < 1.0) {
@@ -158,4 +167,30 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
158167
}
159168
}
160169
}
161-
}
170+
171+
ASSERT_NE(received_gridmap, nullptr);
172+
grid_map::GridMap incoming;
173+
ASSERT_TRUE(grid_map::GridMapRosConverter::fromMessage(*received_gridmap, incoming, {"elevation"}));
174+
175+
ASSERT_EQ(map.getLength().x(), incoming.getLength().x());
176+
ASSERT_EQ(map.getLength().y(), incoming.getLength().y());
177+
ASSERT_EQ(map.getPosition().x(), incoming.getPosition().x());
178+
ASSERT_EQ(map.getPosition().y(), incoming.getPosition().y());
179+
ASSERT_EQ(map.getResolution(), incoming.getResolution());
180+
181+
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
182+
const grid_map::Index index(*iterator);
183+
grid_map::Position pos;
184+
map.getPosition(index, pos);
185+
186+
double map_z = map.at("elevation", index);
187+
188+
grid_map::Index index2;
189+
ASSERT_TRUE(incoming.getIndex(pos, index2));
190+
191+
double incoming_z = incoming.at("elevation", index2);
192+
193+
ASSERT_NEAR(map_z, incoming_z, 0.001);
194+
}
195+
196+
}

‎easynav_gridmap_maps_manager/tests/gridmap_maps_manager_tests.cpp‎

Lines changed: 91 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,10 @@
2929
#include "lifecycle_msgs/msg/state.hpp"
3030

3131
#include "easynav_gridmap_maps_manager/GridmapMapsBuilderNode.hpp"
32+
#include "easynav_gridmap_maps_manager/GridmapMapsManager.hpp"
3233
#include "easynav_gridmap_maps_manager/utils.hpp"
3334

34-
class GridmapMapsBuilderTest : public ::testing::Test
35+
class GridmapMapsMangerTest : public ::testing::Test
3536
{
3637
protected:
3738
static void SetUpTestSuite()
@@ -49,8 +50,9 @@ class GridmapMapsBuilderTest : public ::testing::Test
4950

5051
using namespace std::chrono_literals;
5152

52-
TEST_F(GridmapMapsBuilderTest, test_configure_success)
53+
TEST_F(GridmapMapsMangerTest, test_save)
5354
{
55+
5456
rclcpp::NodeOptions options;
5557
options.append_parameter_override("use_sim_time", true);
5658
options.append_parameter_override("sensors", std::vector<std::string>{"map"});
@@ -59,18 +61,50 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
5961
options.append_parameter_override("map.topic", "map");
6062
options.append_parameter_override("map.type", "sensor_msgs/msg/PointCloud2");
6163
options.append_parameter_override("map.group", "points");
64+
std::vector<std::string> remappings = {
65+
"/map_builder_gridmap/gridmap:=/test_node/test2/incoming_map"
66+
};
67+
options.arguments(remappings);
6268

6369
auto builder_node = std::make_shared<easynav::GridmapMapsBuilderNode>(options);
64-
auto test_node = std::make_shared<rclcpp::Node>("test_node");
70+
auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_node");
71+
auto manager = std::make_shared<easynav::GridmapMapsManager>();
72+
manager->initialize(test_node, "test2");
73+
6574
auto pub = test_node->create_publisher<sensor_msgs::msg::PointCloud2>("map", 10);
6675

76+
grid_map_msgs::msg::GridMap::SharedPtr received_gridmap;
77+
auto sub = test_node->create_subscription<grid_map_msgs::msg::GridMap>(
78+
"/test_node/test2/incoming_map", 100,
79+
[&] (const grid_map_msgs::msg::GridMap::SharedPtr msg) {
80+
received_gridmap = msg;
81+
}
82+
);
83+
84+
grid_map_msgs::msg::GridMap::SharedPtr emitted_gridmap;
85+
auto sub2 = test_node->create_subscription<grid_map_msgs::msg::GridMap>(
86+
"/test_node/test2/map", rclcpp::QoS(1).transient_local().reliable(),
87+
[&] (const grid_map_msgs::msg::GridMap::SharedPtr msg) {
88+
emitted_gridmap = msg;
89+
}
90+
);
91+
92+
easynav::NavState navstate;
93+
6794
builder_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
6895
ASSERT_EQ(builder_node->get_current_state().id(),
6996
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
7097
builder_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
7198
ASSERT_EQ(builder_node->get_current_state().id(),
7299
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
73100

101+
test_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
102+
ASSERT_EQ(test_node->get_current_state().id(),
103+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
104+
test_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
105+
ASSERT_EQ(test_node->get_current_state().id(),
106+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
107+
74108
sensor_msgs::msg::PointCloud2 cloud;
75109
cloud.header.frame_id = "map";
76110
cloud.header.stamp = test_node->now();
@@ -122,12 +156,14 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
122156

123157
rclcpp::executors::SingleThreadedExecutor executor;
124158
executor.add_node(builder_node->get_node_base_interface());
159+
executor.add_node(test_node->get_node_base_interface());
125160

126161
auto start = test_node->now();
127162
while ((test_node->now() - start).seconds() < 1.0) {
128163
pub->publish(cloud);
129164
executor.spin_some();
130165
builder_node->cycle();
166+
manager->update(navstate);
131167
rclcpp::sleep_for(std::chrono::milliseconds(100));
132168
}
133169

@@ -159,55 +195,70 @@ TEST_F(GridmapMapsBuilderTest, test_configure_success)
159195
}
160196
}
161197

162-
auto client = test_node->create_client<std_srvs::srv::Trigger>(
163-
"/gridmap_maps_builder_node/gridmap_maps_builder_node/savemap");
164-
ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(2)));
198+
ASSERT_NE(received_gridmap, nullptr);
199+
grid_map::GridMap incoming;
200+
ASSERT_TRUE(grid_map::GridMapRosConverter::fromMessage(*received_gridmap, incoming, {"elevation"}));
165201

202+
ASSERT_EQ(map.getLength().x(), incoming.getLength().x());
203+
ASSERT_EQ(map.getLength().y(), incoming.getLength().y());
204+
ASSERT_EQ(map.getPosition().x(), incoming.getPosition().x());
205+
ASSERT_EQ(map.getPosition().y(), incoming.getPosition().y());
206+
ASSERT_EQ(map.getResolution(), incoming.getResolution());
207+
208+
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
209+
const grid_map::Index index(*iterator);
210+
grid_map::Position pos;
211+
map.getPosition(index, pos);
212+
213+
double map_z = map.at("elevation", index);
214+
215+
grid_map::Index index2;
216+
ASSERT_TRUE(incoming.getIndex(pos, index2));
217+
218+
double incoming_z = incoming.at("elevation", index2);
219+
220+
ASSERT_NEAR(map_z, incoming_z, 0.001);
221+
}
222+
223+
ASSERT_EQ(received_gridmap->info, emitted_gridmap->info);
224+
ASSERT_EQ(received_gridmap->layers, emitted_gridmap->layers);
225+
ASSERT_EQ(received_gridmap->basic_layers, emitted_gridmap->basic_layers);
226+
ASSERT_EQ(received_gridmap->data, emitted_gridmap->data);
227+
228+
auto test_node_srv = rclcpp::Node::make_shared("test_node_srv");
229+
auto client = test_node_srv->create_client<std_srvs::srv::Trigger>("/test_node/test2/savemap");
166230
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
231+
auto result_future = client->async_send_request(request);
167232

168-
auto future = client->async_send_request(request);
169233

170-
if (rclcpp::spin_until_future_complete(test_node, future, 1s) !=
171-
rclcpp::FutureReturnCode::SUCCESS)
172-
{
173-
std::cerr << "Spinnning" << std::endl;
234+
start = test_node->now();
235+
while ((test_node->now() - start).seconds() < 1.0) {
236+
pub->publish(cloud);
174237
executor.spin_some();
238+
rclcpp::spin_until_future_complete(test_node_srv, result_future);
239+
builder_node->cycle();
240+
manager->update(navstate);
241+
rclcpp::sleep_for(std::chrono::milliseconds(100));
175242
}
243+
auto result = result_future.get();
176244

177-
grid_map::GridMap map2;
178-
ASSERT_TRUE(easynav::load_gridmap("gridmap.yaml", map2));
245+
grid_map::GridMap map_file;
246+
ASSERT_TRUE(easynav::load_gridmap("/tmp/gridmap_map.yaml", map_file));
179247

180-
ASSERT_EQ(map2.getLayers(), std::vector<std::string>({"elevation"}));
181248

249+
for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
250+
const grid_map::Index index(*iterator);
251+
grid_map::Position pos;
252+
map.getPosition(index, pos);
182253

183-
{
184-
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
185-
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
186-
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
254+
double map_z = map.at("elevation", index);
187255

188-
ASSERT_EQ(cloud.width, 10);
189-
for (size_t i = 0; i < cloud.width; ++i, ++iter_x, ++iter_y, ++iter_z) {
190-
grid_map::Position pos(*iter_x, *iter_y);
191-
grid_map::Index index;
192-
if (map2.getIndex(pos, index)) {
193-
auto z = map2.at("elevation", index);
256+
grid_map::Index index2;
257+
ASSERT_TRUE(map_file.getIndex(pos, index2));
194258

195-
if (*iter_x < -5.0) {
196-
EXPECT_NEAR(z, -2.0, 0.1);
197-
} else if (*iter_x < 0) {
198-
EXPECT_NEAR(z, -1.0, 0.1);
199-
} else if (*iter_x < 5) {
200-
EXPECT_NEAR(z, 1.0, 0.1);
201-
} else {
202-
EXPECT_NEAR(z, 2.0, 0.1);
203-
}
204-
}
205-
}
259+
double incoming_z = map_file.at("elevation", index2);
260+
261+
ASSERT_NEAR(map_z, incoming_z, 0.05);
206262
}
207263

208-
ASSERT_EQ(map.getLength().x(), map2.getLength().x());
209-
ASSERT_EQ(map.getLength().y(), map2.getLength().y());
210-
ASSERT_EQ(map.getPosition().x(), map2.getPosition().x());
211-
ASSERT_EQ(map.getPosition().y(), map2.getPosition().y());
212-
ASSERT_EQ(map.getResolution(), map2.getResolution());
213264
}

0 commit comments

Comments
 (0)