HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
Localizations/Libraries/Ros/map_server/test/consumer.py Normal file
View File

@@ -0,0 +1,71 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of the Willow Garage nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
from __future__ import print_function
PKG = 'static_map_server'
NAME = 'consumer'
import sys
import unittest
import time
import rospy
import rostest
from nav_msgs.srv import GetMap
class TestConsumer(unittest.TestCase):
def __init__(self, *args):
super(TestConsumer, self).__init__(*args)
self.success = False
def callback(self, data):
print(rospy.get_caller_id(), "I heard %s" % data.data)
self.success = data.data and data.data.startswith('hello world')
rospy.signal_shutdown('test done')
def test_consumer(self):
rospy.wait_for_service('static_map')
mapsrv = rospy.ServiceProxy('static_map', GetMap)
resp = mapsrv()
self.success = True
print(resp)
while not rospy.is_shutdown() and not self.success: # and time.time() < timeout_t: <== timeout_t doesn't exists??
time.sleep(0.1)
self.assert_(self.success)
rospy.signal_shutdown('test done')
if __name__ == '__main__':
rostest.rosrun(PKG, NAME, TestConsumer, sys.argv)

198
Localizations/Libraries/Ros/map_server/test/rtest.cpp Normal file
View File

@@ -0,0 +1,198 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/* Author: Brian Gerkey */
#include <gtest/gtest.h>
#include <ros/service.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/LoadMap.h>
#include "test_constants.h"
int g_argc;
char** g_argv;
class MapClientTest : public testing::Test
{
public:
// A node is needed to make a service call
ros::NodeHandle* n_;
MapClientTest()
{
ros::init(g_argc, g_argv, "map_client_test");
n_ = new ros::NodeHandle();
got_map_ = false;
got_map_metadata_ = false;
}
~MapClientTest()
{
delete n_;
}
bool got_map_;
boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
{
map_ = map;
got_map_ = true;
}
bool got_map_metadata_;
boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
{
map_metadata_ = map_metadata;
got_map_metadata_ = true;
}
};
/* Try to retrieve the map via service, and compare to ground truth */
TEST_F(MapClientTest, call_service)
{
nav_msgs::GetMap::Request req;
nav_msgs::GetMap::Response resp;
ASSERT_TRUE(ros::service::waitForService("static_map", 5000));
ASSERT_TRUE(ros::service::call("static_map", req, resp));
ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
ASSERT_EQ(resp.map.info.width, g_valid_image_width);
ASSERT_EQ(resp.map.info.height, g_valid_image_height);
ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map");
for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
}
/* Try to retrieve the map via topic, and compare to ground truth */
TEST_F(MapClientTest, subscribe_topic)
{
ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, [this](auto& map){ mapCallback(map); });
// Try a few times, because the server may not be up yet.
int i=20;
while(!got_map_ && i > 0)
{
ros::spinOnce();
ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
i--;
}
ASSERT_TRUE(got_map_);
ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res);
ASSERT_EQ(map_->info.width, g_valid_image_width);
ASSERT_EQ(map_->info.height, g_valid_image_height);
ASSERT_STREQ(map_->header.frame_id.c_str(), "map");
for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
ASSERT_EQ(g_valid_image_content[i], map_->data[i]);
}
/* Try to retrieve the metadata via topic, and compare to ground truth */
TEST_F(MapClientTest, subscribe_topic_metadata)
{
ros::Subscriber sub = n_->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, [this](auto& map_metadata){ mapMetaDataCallback(map_metadata); });
// Try a few times, because the server may not be up yet.
int i=20;
while(!got_map_metadata_ && i > 0)
{
ros::spinOnce();
ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
i--;
}
ASSERT_TRUE(got_map_metadata_);
ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res);
ASSERT_EQ(map_metadata_->width, g_valid_image_width);
ASSERT_EQ(map_metadata_->height, g_valid_image_height);
}
/* Change the map, retrieve the map via topic, and compare to ground truth */
TEST_F(MapClientTest, change_map)
{
ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, [this](auto& map){ mapCallback(map); });
// Try a few times, because the server may not be up yet.
for (int i = 20; i > 0 && !got_map_; i--)
{
ros::spinOnce();
ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
}
ASSERT_TRUE(got_map_);
// Now change the map
got_map_ = false;
nav_msgs::LoadMap::Request req;
nav_msgs::LoadMap::Response resp;
req.map_url = ros::package::getPath("map_server") + "/test/testmap2.yaml";
ASSERT_TRUE(ros::service::waitForService("change_map", 5000));
ASSERT_TRUE(ros::service::call("change_map", req, resp));
ASSERT_EQ(0u, resp.result);
for (int i = 20; i > 0 && !got_map_; i--)
{
ros::spinOnce();
ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
}
ASSERT_FLOAT_EQ(tmap2::g_valid_image_res, map_->info.resolution);
ASSERT_EQ(tmap2::g_valid_image_width, map_->info.width);
ASSERT_EQ(tmap2::g_valid_image_height, map_->info.height);
ASSERT_STREQ("map", map_->header.frame_id.c_str());
for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
ASSERT_EQ(tmap2::g_valid_image_content[i], map_->data[i]) << "idx:" << i;
//Put the old map back so the next test isn't broken
got_map_ = false;
req.map_url = ros::package::getPath("map_server") + "/test/testmap.yaml";
ASSERT_TRUE(ros::service::call("change_map", req, resp));
ASSERT_EQ(0u, resp.result);
for (int i = 20; i > 0 && !got_map_; i--)
{
ros::spinOnce();
ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
}
ASSERT_TRUE(got_map_);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

22
Localizations/Libraries/Ros/map_server/test/rtest.xml Normal file
View File

@@ -0,0 +1,22 @@
<launch>
<arg name="map_file" default="$(find map_server)/maps/maze.yaml" doc="Path to a map .yaml file (required)." />
<arg name="virtual_walls_map_file" default="$(find map_server)/maps/maze_virtual_walls.yaml" doc="Path to a virtual walls map .yaml file (optional)." />
<arg name="with_virtual_walls" default="false" />
<node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/lane_mask" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="16"/>
</node>
<!-- <node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
<param name="frame_id" type="string" value="map"/>
</node> -->
<!-- <arg name="direction_zones_map_file" default="$(find mir_gazebo)/maps/warehouse/direction_zones.yaml" doc="Path to a direction zones .yaml file (optional)." />
<arg name="with_direction_zones" default="true" />
<node if="$(eval direction_zones_map_file != '')" name="direction_zones_map_server" pkg="map_server" type="map_server" args="$(arg direction_zones_map_file)" ns="/direction_zones" output="screen">
<param name="frame_id" type="string" value="map"/>
<param name="type" type="int" value="16"/>
</node> -->
</launch>

BIN
Localizations/Libraries/Ros/map_server/test/spectrum.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 134 B

84
Localizations/Libraries/Ros/map_server/test/test_constants.cpp Normal file
View File

@@ -0,0 +1,84 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/* Author: Brian Gerkey */
/* This file contains global constants shared among tests */
/* Note that these must be changed if the test image changes */
#include "test_constants.h"
const unsigned int g_valid_image_width = 10;
const unsigned int g_valid_image_height = 10;
// Note that the image content is given in row-major order, with the
// lower-left pixel first. This is different from a graphics coordinate
// system, which starts with the upper-left pixel. The loadMapFromFile
// call converts from the latter to the former when it loads the image, and
// we want to compare against the result of that conversion.
const char g_valid_image_content[] = {
0,0,0,0,0,0,0,0,0,0,
100,100,100,100,0,0,100,100,100,0,
100,100,100,100,0,0,100,100,100,0,
100,0,0,0,0,0,0,0,0,0,
100,0,0,0,0,0,0,0,0,0,
100,0,0,0,0,0,100,100,0,0,
100,0,0,0,0,0,100,100,0,0,
100,0,0,0,0,0,100,100,0,0,
100,0,0,0,0,0,100,100,0,0,
100,0,0,0,0,0,0,0,0,0,
};
const char* g_valid_png_file = "test/testmap.png";
const char* g_valid_bmp_file = "test/testmap.bmp";
const char* g_spectrum_png_file = "test/spectrum.png";
const float g_valid_image_res = 0.1;
// Constants for testmap2
namespace tmap2
{
const char g_valid_image_content[] = {
100,100,100,100,100,100,100,100,100,100,100,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,100,100,100,100,100,100,100,100,0,100,
100,0,100,0,0,0,0,0,0,100,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,0,100,0,0,0,0,0,0,100,0,100,
100,0,0,0,0,0,0,0,0,0,0,100,
100,100,100,100,100,100,100,100,100,100,100,100,
};
const float g_valid_image_res = 0.1;
const unsigned int g_valid_image_width = 12;
const unsigned int g_valid_image_height = 12;
}

51
Localizations/Libraries/Ros/map_server/test/test_constants.h Normal file
View File

@@ -0,0 +1,51 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef MAP_SERVER_TEST_CONSTANTS_H
#define MAP_SERVER_TEST_CONSTANTS_H
/* Author: Brian Gerkey */
/* This file externs global constants shared among tests */
extern const unsigned int g_valid_image_width;
extern const unsigned int g_valid_image_height;
extern const char g_valid_image_content[];
extern const char* g_valid_png_file;
extern const char* g_valid_bmp_file;
extern const float g_valid_image_res;
extern const char* g_spectrum_png_file;
namespace tmap2 {
extern const char g_valid_image_content[];
extern const float g_valid_image_res;
extern const unsigned int g_valid_image_width;
extern const unsigned int g_valid_image_height;
}
#endif

BIN
Localizations/Libraries/Ros/map_server/test/testmap.bmp Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 374 B

BIN
Localizations/Libraries/Ros/map_server/test/testmap.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 188 B

6
Localizations/Libraries/Ros/map_server/test/testmap.yaml Normal file
View File

@@ -0,0 +1,6 @@
image: testmap.png
resolution: 0.1
origin: [2.0, 3.0, 1.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
Localizations/Libraries/Ros/map_server/test/testmap2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 202 B

6
Localizations/Libraries/Ros/map_server/test/testmap2.yaml Normal file
View File

@@ -0,0 +1,6 @@
image: testmap2.png
resolution: 0.1
origin: [-2.0, -3.0, -1.0]
negate: 1
occupied_thresh: 0.5
free_thresh: 0.2

149
Localizations/Libraries/Ros/map_server/test/utest.cpp Normal file
View File

@@ -0,0 +1,149 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/* Author: Brian Gerkey */
#include <stdexcept> // for std::runtime_error
#include <gtest/gtest.h>
#include "map_server/image_loader.h"
#include "test_constants.h"
/* Try to load a valid PNG file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file.
*
* This test can fail on OS X, due to an apparent limitation of the
* underlying SDL_Image library. */
TEST(MapServer, loadValidPNG)
{
try
{
nav_msgs::GetMap::Response map_resp;
double origin[3] = { 0.0, 0.0, 0.0 };
map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1, origin);
EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++)
EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
}
catch(...)
{
ADD_FAILURE() << "Uncaught exception : " << "This is OK on OS X";
}
}
/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file. */
TEST(MapServer, loadValidBMP)
{
try
{
nav_msgs::GetMap::Response map_resp;
double origin[3] = { 0.0, 0.0, 0.0 };
map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false, 0.65, 0.1, origin);
EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++)
EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
}
catch(...)
{
ADD_FAILURE() << "Uncaught exception";
}
}
/* Try to load an invalid file. Succeeds if a std::runtime_error exception
* is thrown. */
TEST(MapServer, loadInvalidFile)
{
try
{
nav_msgs::GetMap::Response map_resp;
double origin[3] = { 0.0, 0.0, 0.0 };
map_server::loadMapFromFile(&map_resp, "foo", 0.1, false, 0.65, 0.1, origin);
}
catch(std::runtime_error &e)
{
SUCCEED();
return;
}
catch(...)
{
FAIL() << "Uncaught exception";
}
ADD_FAILURE() << "Didn't throw exception as expected";
}
std::vector<unsigned int> countValues(const nav_msgs::GetMap::Response& map_resp)
{
std::vector<unsigned int> counts(256, 0);
for (unsigned int i = 0; i < map_resp.map.data.size(); i++)
{
unsigned char value = static_cast<unsigned char>(map_resp.map.data[i]);
counts[value]++;
}
return counts;
}
TEST(MapServer, testMapMode)
{
nav_msgs::GetMap::Response map_resp;
double origin[3] = { 0.0, 0.0, 0.0 };
map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, TRINARY);
std::vector<unsigned int> trinary_counts = countValues(map_resp);
EXPECT_EQ(90u, trinary_counts[100]);
EXPECT_EQ(26u, trinary_counts[0]);
EXPECT_EQ(140u, trinary_counts[255]);
map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, SCALE);
std::vector<unsigned int> scale_counts = countValues(map_resp);
EXPECT_EQ(90u, scale_counts[100]);
EXPECT_EQ(26u, scale_counts[0]);
unsigned int scaled_values = 0;
for (unsigned int i = 1; i < 100; i++)
{
scaled_values += scale_counts[i];
}
EXPECT_EQ(140u, scaled_values);
map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, RAW);
std::vector<unsigned int> raw_counts = countValues(map_resp);
for (unsigned int i = 0; i < raw_counts.size(); i++)
{
EXPECT_EQ(1u, raw_counts[i]) << i;
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}