17 #include <gtest/gtest.h>
20 #include <nav_msgs/OccupancyGrid.h>
21 #include <ros/console.h>
28 msg.info.height = 500;
29 msg.info.resolution = 0.1;
30 msg.info.origin.position.x = rand() % 10 - 5;
31 msg.info.origin.position.y = rand() % 10 - 5;
32 msg.info.origin.position.z = 0;
34 msg.info.origin.orientation.x = 0;
35 msg.info.origin.orientation.y = 0;
36 msg.info.origin.orientation.z = 0;
37 msg.info.origin.orientation.w = 1;
39 msg.data.resize(msg.info.width * msg.info.height, -1);
44 nav_msgs::OccupancyGrid srcRos;
49 srcRos.info.origin.orientation.x = 0;
55 for (uint32_t h = 0; h < srcRos.info.width; h++)
57 for (uint32_t w = 0; w < srcRos.info.width; w++)
60 desMrpt.
getPos(w, h), 0.5);
65 TEST(Map, check_ros2mrpt_and_back)
67 nav_msgs::OccupancyGrid srcRos;
69 nav_msgs::OccupancyGrid desRos;
77 for (uint32_t h = 0; h < srcRos.info.width; h++)
78 for (uint32_t w = 0; w < srcRos.info.width; w++)
79 EXPECT_EQ(desRos.data[h * srcRos.info.width + h], -1);
82 for (
int i = 0; i <= 100; i++)
85 srcRos.data[i] = (std::abs(i - 50) <= 2) ? -1 : i;
91 for (
int i = 0; i <= 100; i++)
99 EXPECT_NEAR(srcRos.data[i], desRos.data[i], 1) <<
"ros to mprt to ros";