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

133
Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp Normal file
View File

@@ -0,0 +1,133 @@
/*
* 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.
*/
/*
* This file contains helper functions for loading images as maps.
*
* Author: Brian Gerkey
*/
#include <cstring>
#include <stdexcept>
#include <stdlib.h>
#include <stdio.h>
// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>
// Use Bullet's Quaternion object to create one from Euler angles
#include <LinearMath/btQuaternion.h>
#include "map_server/image_loader.h"
#include <ros/ros.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
namespace map_server
{
void loadMapFromFile16Bit(nav_msgs::GetMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th, double* origin,
MapMode mode)
{
SDL_Surface* img;
unsigned char* pixels;
unsigned char* p;
int rowstride, n_channels, avg_channels;
unsigned int i,j;
int k;
int alpha;
uint16_t color_avg;
// Load the image using SDL. If we get NULL back, the image load failed.
if(!(img = IMG_Load(fname)))
{
std::string errmsg = std::string("failed to open image file \"") +
std::string(fname) + std::string("\": ") + IMG_GetError();
throw std::runtime_error(errmsg);
}
// Copy the image data into the map structure
resp->map.info.width = img->w*2;
resp->map.info.height = img->h;
resp->map.info.resolution = res;
resp->map.info.origin.position.x = *(origin);
resp->map.info.origin.position.y = *(origin+1);
resp->map.info.origin.position.z = 0.0;
btQuaternion q;
// setEulerZYX(yaw, pitch, roll)
q.setEulerZYX(*(origin+2), 0, 0);
resp->map.info.origin.orientation.x = q.x();
resp->map.info.origin.orientation.y = q.y();
resp->map.info.origin.orientation.z = q.z();
resp->map.info.origin.orientation.w = q.w();
// Allocate space to hold the data
resp->map.data.resize(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
n_channels = img->format->BytesPerPixel;
// NOTE: Trinary mode still overrides here to preserve existing behavior.
// Alpha will be averaged in with color channels when using trinary mode.
if (mode==TRINARY || !img->format->Amask)
avg_channels = n_channels;
else
avg_channels = n_channels - 1;
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
for(j = 0; j < resp->map.info.height; j++)
{
for (i = 0; i < resp->map.info.width; i+=2)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i/2*n_channels;
color_avg = (*(p) << 16) + (*(p+1) << 8) + (*(p+2));
if (n_channels == 1)
alpha = 1;
else
alpha = *(p+n_channels-1);
if(negate)
color_avg = 65535 - color_avg;
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = (unsigned char)(color_avg & 0x00FF);
resp->map.data[MAP_IDX(resp->map.info.width,i+1,resp->map.info.height - j - 1)] = (unsigned char)((color_avg & 0xFF00) >> 8);
}
}
SDL_FreeSurface(img);
}
}

134
Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp Normal file
View File

@@ -0,0 +1,134 @@
/*
* 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.
*/
/*
* This file contains helper functions for loading images as maps.
*
* Author: Brian Gerkey
*/
#include <cstring>
#include <stdexcept>
#include <stdlib.h>
#include <stdio.h>
// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>
// Use Bullet's Quaternion object to create one from Euler angles
#include <LinearMath/btQuaternion.h>
#include "map_server/image_loader.h"
#include <ros/ros.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
namespace map_server
{
void loadMapFromFile32Bit(nav_msgs::GetMap::Response *resp,
const char *fname, double res, bool negate,
double occ_th, double free_th, double *origin,
MapMode mode)
{
SDL_Surface *img;
unsigned char *pixels;
unsigned char *p;
int rowstride, n_channels, avg_channels;
unsigned int i, j;
int k;
int alpha;
uint32_t color_avg;
// Load the image using SDL. If we get NULL back, the image load failed.
if (!(img = IMG_Load(fname)))
{
std::string errmsg = std::string("failed to open image file \"") +
std::string(fname) + std::string("\": ") + IMG_GetError();
throw std::runtime_error(errmsg);
}
// Copy the image data into the map structure
resp->map.info.width = img->w * 2;
resp->map.info.height = img->h * 2;
resp->map.info.resolution = res;
resp->map.info.origin.position.x = *(origin);
resp->map.info.origin.position.y = *(origin + 1);
resp->map.info.origin.position.z = 0.0;
btQuaternion q;
// setEulerZYX(yaw, pitch, roll)
q.setEulerZYX(*(origin + 2), 0, 0);
resp->map.info.origin.orientation.x = q.x();
resp->map.info.origin.orientation.y = q.y();
resp->map.info.origin.orientation.z = q.z();
resp->map.info.origin.orientation.w = q.w();
// Allocate space to hold the data
resp->map.data.resize(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
n_channels = img->format->BytesPerPixel;
// NOTE: Trinary mode still overrides here to preserve existing behavior.
// Alpha will be averaged in with color channels when using trinary mode.
if (mode == TRINARY || !img->format->Amask)
avg_channels = n_channels;
else
avg_channels = n_channels - 1;
// Copy pixel data into the map structure
pixels = (unsigned char *)(img->pixels);
for (j = 0; j < resp->map.info.height; j += 2)
{
for (i = 0; i < resp->map.info.width; i += 2)
{
// Compute mean of RGB for this pixel
p = pixels + j / 2 * rowstride + i / 2 * n_channels;
color_avg = (*(p) << 16) + (*(p + 1) << 8) + (*(p + 2));
if (n_channels == 1)
alpha = 1;
else
alpha = *(p + n_channels - 1);
if (negate)
color_avg = 16777215 - color_avg;
resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 1)] = (color_avg >> 16u) & 0xFF;
resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 1)] = (color_avg >> 24u) & 0xFF;
resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 2)] = (color_avg >> 0u) & 0xFF;
resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 2)] = (color_avg >> 8u) & 0xFF;
}
}
SDL_FreeSurface(img);
}
}

167
Localizations/Libraries/Ros/map_server/src/image_loader.cpp Normal file
View File

@@ -0,0 +1,167 @@
/*
* 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.
*/
/*
* This file contains helper functions for loading images as maps.
*
* Author: Brian Gerkey
*/
#include <cstring>
#include <stdexcept>
#include <stdlib.h>
#include <stdio.h>
// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>
// Use Bullet's Quaternion object to create one from Euler angles
#include <LinearMath/btQuaternion.h>
#include "map_server/image_loader.h"
#include <ros/ros.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
namespace map_server
{
void
loadMapFromFile(nav_msgs::GetMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th, double* origin,
MapMode mode)
{
SDL_Surface* img;
unsigned char* pixels;
unsigned char* p;
unsigned char value;
int rowstride, n_channels, avg_channels;
unsigned int i,j;
int k;
double occ;
int alpha;
int color_sum;
double color_avg;
// Load the image using SDL. If we get NULL back, the image load failed.
if(!(img = IMG_Load(fname)))
{
std::string errmsg = std::string("failed to open image file \"") +
std::string(fname) + std::string("\": ") + IMG_GetError();
throw std::runtime_error(errmsg);
}
// Copy the image data into the map structure
resp->map.info.width = img->w;
resp->map.info.height = img->h;
resp->map.info.resolution = res;
resp->map.info.origin.position.x = *(origin);
resp->map.info.origin.position.y = *(origin+1);
resp->map.info.origin.position.z = 0.0;
btQuaternion q;
// setEulerZYX(yaw, pitch, roll)
q.setEulerZYX(*(origin+2), 0, 0);
resp->map.info.origin.orientation.x = q.x();
resp->map.info.origin.orientation.y = q.y();
resp->map.info.origin.orientation.z = q.z();
resp->map.info.origin.orientation.w = q.w();
// Allocate space to hold the data
resp->map.data.resize(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
n_channels = img->format->BytesPerPixel;
// NOTE: Trinary mode still overrides here to preserve existing behavior.
// Alpha will be averaged in with color channels when using trinary mode.
if (mode==TRINARY || !img->format->Amask)
avg_channels = n_channels;
else
avg_channels = n_channels - 1;
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
for(j = 0; j < resp->map.info.height; j++)
{
for (i = 0; i < resp->map.info.width; i++)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
color_sum = 0;
int color = 0;
for(k=0;k<avg_channels;k++)
{
color_sum += *(p + (k));
}
color_avg = color_sum / (double)avg_channels;
if (n_channels == 1)
alpha = 1;
else
alpha = *(p+n_channels-1);
if(negate)
color_avg = 255 - color_avg;
if(mode==RAW){
value = color_avg;
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
continue;
}
// If negate is true, we consider blacker pixels free, and whiter
// pixels occupied. Otherwise, it's vice versa.
occ = (255 - color_avg) / 255.0;
// Apply thresholds to RGB means to determine occupancy values for
// map. Note that we invert the graphics-ordering of the pixels to
// produce a map with cell (0,0) in the lower-left corner.
if(occ > occ_th)
value = +100;
else if(occ < free_th)
value = 0;
else if(mode==TRINARY || alpha < 1.0)
value = -1;
else {
double ratio = (occ - free_th) / (occ_th - free_th);
value = 1 + 98 * ratio;
}
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
}
}
SDL_FreeSurface(img);
}
}

40
Localizations/Libraries/Ros/map_server/src/main.cpp Normal file
View File

@@ -0,0 +1,40 @@
#include "ros/ros.h"
#include "ros/console.h"
#include "map_server/map_server.h"
#define USAGE "\nUSAGE: map_server <map.yaml>\n" \
" map.yaml: map description file\n" \
"DEPRECATED USAGE: map_server <map> <resolution>\n" \
" map: image file to load\n" \
" resolution: map resolution [meters/pixel]"
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_server", ros::init_options::AnonymousName);
// ros::init(argc, argv, "map_server");
ros::NodeHandle nh("~");
if (argc != 3 && argc != 2)
{
ROS_ERROR("%s", USAGE);
exit(-1);
}
if (argc != 2)
{
ROS_WARN("Using deprecated map server interface. Please switch to new interface.");
}
std::string fname(argv[1]);
double res = (argc == 2) ? 0.0 : atof(argv[2]);
try
{
map_server::MapServer* map_sever_ptr = new map_server::MapServer(fname, res);
ros::spin();
}
catch (std::runtime_error& e)
{
ROS_ERROR("map_server exception: %s", e.what());
return -1;
}
return 0;
}

104
Localizations/Libraries/Ros/map_server/src/map_generator.cpp Normal file
View File

@@ -0,0 +1,104 @@
#include "map_server/map_generator.h"
map_server::MapGenerator::MapGenerator(ros::NodeHandle nh, const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free)
: mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
{
ROS_INFO("Waiting for the map by topic %s", map_topic.c_str());
map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this);
}
map_server::MapGenerator::MapGenerator(const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free)
: mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
{
ros::NodeHandle nh;
ROS_INFO("Waiting for the map");
map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this);
}
void map_server::MapGenerator::mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
{
ROS_INFO("[MapGenerator] Received a %d X %d map @ %.3f m/pix",
map->info.width,
map->info.height,
map->info.resolution);
std::string mapdatafile = mapname_ + ".pgm";
ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
FILE* out = fopen(mapdatafile.c_str(), "w");
if (!out)
{
ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
return;
}
fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
map->info.resolution, map->info.width, map->info.height);
for (unsigned int y = 0; y < map->info.height; y++) {
for (unsigned int x = 0; x < map->info.width; x++) {
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
fputc(254, out);
}
else if (map->data[i] >= threshold_occupied_) { // (occ,255]
fputc(000, out);
}
else { //occ [0.25,0.65]
fputc(205, out);
}
}
}
fclose(out);
std::string mapmetadatafile = mapname_ + ".yaml";
ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
/*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
*/
geometry_msgs::Quaternion orientation = map->info.origin.orientation;
tf2::Matrix3x3 mat(tf2::Quaternion(
orientation.x,
orientation.y,
orientation.z,
orientation.w
));
double yaw, pitch, roll;
mat.getEulerYPR(yaw, pitch, roll);
fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);
fclose(yaml);
ROS_INFO("Done\n");
saved_map_ = true;
}
bool map_server::MapGenerator::waitingGenerator(double tolerance)
{
bool result = true;
ros::Time start = ros::Time::now();
while(!this->isMapSaved() && ros::ok())
{
if((ros::Time::now() - start) > ros::Duration(tolerance))
{
result = false;
break;
}
ros::spinOnce();
}
if(this->isMapSaved()) result = true;
return result;
}

119
Localizations/Libraries/Ros/map_server/src/map_saver.cpp Normal file
View File

@@ -0,0 +1,119 @@
/*
* map_saver
* 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 <ORGANIZATION> 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.
*/
#include <ros/ros.h>
#include "map_server/map_generator.h"
#define USAGE "Usage: \n" \
" map_saver -h\n"\
" map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_saver");
std::string mapname = "map";
int threshold_occupied = 65;
int threshold_free = 25;
for(int i=1; i<argc; i++)
{
if(!strcmp(argv[i], "-h"))
{
puts(USAGE);
return 0;
}
else if(!strcmp(argv[i], "-f"))
{
if(++i < argc)
mapname = argv[i];
else
{
puts(USAGE);
return 1;
}
}
else if (!strcmp(argv[i], "--occ"))
{
if (++i < argc)
{
threshold_occupied = std::atoi(argv[i]);
if (threshold_occupied < 1 || threshold_occupied > 100)
{
ROS_ERROR("threshold_occupied must be between 1 and 100");
return 1;
}
}
else
{
puts(USAGE);
return 1;
}
}
else if (!strcmp(argv[i], "--free"))
{
if (++i < argc)
{
threshold_free = std::atoi(argv[i]);
if (threshold_free < 0 || threshold_free > 100)
{
ROS_ERROR("threshold_free must be between 0 and 100");
return 1;
}
}
else
{
puts(USAGE);
return 1;
}
}
else
{
puts(USAGE);
return 1;
}
}
if (threshold_occupied <= threshold_free)
{
ROS_ERROR("threshold_free must be smaller than threshold_occupied");
return 1;
}
map_server::MapGenerator mg(mapname, "map", threshold_occupied, threshold_free);
while(!mg.isMapSaved() && ros::ok())
ros::spinOnce();
return 0;
}

308
Localizations/Libraries/Ros/map_server/src/map_server.cpp Normal file
View File

@@ -0,0 +1,308 @@
#include "map_server/map_server.h"
map_server::MapServer::MapServer(const std::string& fname, double res)
{
ros::NodeHandle private_nh("~");
this->initialize(private_nh, fname, res);
}
map_server::MapServer::MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res)
{
nh_ = ros::NodeHandle(name_sp);
this->initialize(nh, fname, res);
}
void map_server::MapServer::initialize(ros::NodeHandle nh, const std::string& fname, double res)
{
std::string mapfname = "";
double origin[3];
int negate;
double occ_th, free_th;
MapMode mode = TRINARY;
nh.param("frame_id", frame_id_, std::string("map"));
nh.param("type", type_, 8);
// When called this service returns a copy of the current map
get_map_service_ = nh_.advertiseService("static_map", &map_server::MapServer::mapCallback, this);
// Change the currently published map
change_map_srv_ = nh_.advertiseService("change_map", &map_server::MapServer::changeMapCallback, this);
// Latched publisher for metadata
metadata_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
// Latched publisher for data
map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
if (type_ != 8) map_convert_ = nh_.advertise<nav_msgs::OccupancyGrid>("map_convert", 1, true);
deprecated_ = (res != 0);
if (!deprecated_)
{
if (!loadMapFromYaml(fname))
{
exit(-1);
}
}
else
{
if (!loadMapFromParams(fname, res))
{
exit(-1);
}
}
}
bool map_server::MapServer::mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
{
// request is empty; we ignore it
// = operator is overloaded to make deep copy (tricky!)
res = map_resp_;
ROS_INFO("Sending map");
return true;
}
bool map_server::MapServer::changeMapCallback(nav_msgs::LoadMap::Request& request,
nav_msgs::LoadMap::Response& response)
{
if (loadMapFromYaml(request.map_url))
{
response.result = response.RESULT_SUCCESS;
ROS_INFO("Changed map to %s", request.map_url.c_str());
}
else
{
response.result = response.RESULT_UNDEFINED_FAILURE;
}
return true;
}
bool map_server::MapServer::loadMapFromValues(std::string map_file_name, double resolution,
int negate, double occ_th, double free_th,
double origin[3], MapMode mode)
{
ROS_INFO("Loading map from images \"%s\"", map_file_name.c_str());
try
{
if (type_ == 32)
map_server::loadMapFromFile32Bit(&map_resp_, map_file_name.c_str(),
resolution, negate, occ_th, free_th,
origin, mode);
else if (type_ == 16)
map_server::loadMapFromFile16Bit(&map_resp_, map_file_name.c_str(),
resolution, negate, occ_th, free_th,
origin, mode);
else
map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(),
resolution, negate, occ_th, free_th,
origin, mode);
}
catch (std::runtime_error& e)
{
ROS_ERROR("%s", e.what());
return false;
}
// To make sure get a consistent time in simulation
ros::Time::waitForValid();
map_resp_.map.info.map_load_time = ros::Time::now();
map_resp_.map.header.frame_id = frame_id_;
map_resp_.map.header.stamp = ros::Time::now();
ROS_INFO("Read a %d X %d map @ %.3lf m/cell",
map_resp_.map.info.width,
map_resp_.map.info.height,
map_resp_.map.info.resolution);
meta_data_message_ = map_resp_.map.info;
// Publish latched topics
metadata_pub_.publish(meta_data_message_);
map_pub_.publish(map_resp_.map);
if (type_ == 16)
{
double occ_th = 0.65;
double free_th = 0.196;
nav_msgs::OccupancyGrid convert;
convert.header = map_resp_.map.header;
convert.info = map_resp_.map.info;
convert.info.width = map_resp_.map.info.width / 2;
convert.data.resize(convert.info.width * convert.info.height);
unsigned int color_sum, color;
double color_avg;
unsigned char value;
int size_y = map_resp_.map.info.height;
int size_x = map_resp_.map.info.width;
for (int j = 0; j < map_resp_.map.info.height; j++)
{
for (int i = 0; i < map_resp_.map.info.width; i += 2)
{
unsigned char low = map_resp_.map.data[MAP_IDX(size_x, i, size_y - j - 1)];
unsigned char high = map_resp_.map.data[MAP_IDX(size_x, i + 1, size_y - j - 1)];
color = (int)((low & 0x00FF) | (high << 8));
unsigned char red = (unsigned char)(color & 0x00FF0000) >> 16;
unsigned char green = (unsigned char)(color & 0x0000FF00) >> 8;
unsigned char blue = (unsigned char)(color & 0x000000FF);
color_sum = red + green + blue;
color_avg = color_sum / 2.0;
double occ = (255 - color_avg) / 255.0;
if (occ > occ_th)
value = +100;
else if (occ < free_th)
value = 0;
else
{
double ratio = (occ - free_th) / (occ_th - free_th);
value = 1 + 98 * ratio;
}
convert.data[MAP_IDX(size_x / 2, i / 2, size_y - j - 1)] = value;
}
}
map_convert_.publish(convert);
}
return true;
}
bool map_server::MapServer::loadMapFromParams(std::string map_file_name, double resolution)
{
ros::NodeHandle private_nh("~");
int negate;
double occ_th;
double free_th;
double origin[3];
private_nh.param("negate", negate, 0);
private_nh.param("occupied_thresh", occ_th, 0.65);
private_nh.param("free_thresh", free_th, 0.196);
origin[0] = origin[1] = origin[2] = 0.0;
return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY);
}
bool map_server::MapServer::loadMapFromYaml(std::string path_to_yaml)
{
std::string mapfname;
MapMode mode;
double res;
int negate;
double occ_th;
double free_th;
double origin[3];
std::ifstream fin(path_to_yaml.c_str());
if (fin.fail())
{
ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str());
return false;
}
#ifdef HAVE_YAMLCPP_GT_0_5_0
// The document loading process changed in yaml-cpp 0.5.
YAML::Node doc = YAML::Load(fin);
#else
YAML::Parser parser(fin);
YAML::Node doc;
parser.GetNextDocument(doc);
#endif
try
{
doc["resolution"] >> res;
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
return false;
}
try
{
doc["negate"] >> negate;
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain a negate tag or it is invalid.");
return false;
}
try
{
doc["occupied_thresh"] >> occ_th;
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
return false;
}
try
{
doc["free_thresh"] >> free_th;
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
return false;
}
try
{
std::string modeS = "";
doc["mode"] >> modeS;
if (modeS == "trinary")
mode = TRINARY;
else if (modeS == "scale")
mode = SCALE;
else if (modeS == "raw")
mode = RAW;
else
{
ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str());
return false;
}
}
catch (YAML::Exception&)
{
ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary");
mode = TRINARY;
}
try
{
doc["origin"][0] >> origin[0];
doc["origin"][1] >> origin[1];
doc["origin"][2] >> origin[2];
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain an origin tag or it is invalid.");
return false;
}
try
{
doc["image"] >> mapfname;
// TODO: make this path-handling more robust
if (mapfname.size() == 0)
{
ROS_ERROR("The image tag cannot be an empty string.");
return false;
}
boost::filesystem::path mapfpath(mapfname);
if (!mapfpath.is_absolute())
{
boost::filesystem::path dir(path_to_yaml);
dir = dir.parent_path();
mapfpath = dir / mapfpath;
mapfname = mapfpath.string();
}
}
catch (YAML::InvalidScalar&)
{
ROS_ERROR("The map does not contain an image tag or it is invalid.");
return false;
}
return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode);
}

9
Localizations/Libraries/Ros/map_server/src/map_server.dox Normal file
View File

@@ -0,0 +1,9 @@
/**
@mainpage
@htmlinclude manifest.html
Command-line usage is in the wiki.
*/