32 #define USAGE "\nUSAGE: map_server <map.yaml>\n" \ 33 " map.yaml: map description file\n" \ 34 "DEPRECATED USAGE: map_server <map> <resolution>\n" \ 35 " map: image file to load\n"\ 36 " resolution: map resolution [meters/pixel]" 46 #include "std_msgs/String.h" 47 #include "nav_msgs/MapMetaData.h" 48 #include "yaml-cpp/yaml.h" 50 #ifdef HAVE_NEW_YAMLCPP 54 void operator >> (
const YAML::Node& node, T& i)
64 MapServer(
const std::string& fname,
double res)
66 std::string mapfname =
"";
69 double occ_th, free_th;
72 private_nh.
param(
"frame_id", frame_id, std::string(
"map"));
77 std::ifstream fin(fname.c_str());
79 ROS_ERROR(
"Map_server could not open %s.", fname.c_str());
82 #ifdef HAVE_NEW_YAMLCPP 84 YAML::Node doc = YAML::Load(fin);
88 parser.GetNextDocument(doc);
91 doc[
"resolution"] >> res;
92 }
catch (YAML::InvalidScalar) {
93 ROS_ERROR(
"The map does not contain a resolution tag or it is invalid.");
97 doc[
"negate"] >> negate;
98 }
catch (YAML::InvalidScalar) {
99 ROS_ERROR(
"The map does not contain a negate tag or it is invalid.");
103 doc[
"occupied_thresh"] >> occ_th;
104 }
catch (YAML::InvalidScalar) {
105 ROS_ERROR(
"The map does not contain an occupied_thresh tag or it is invalid.");
109 doc[
"free_thresh"] >> free_th;
110 }
catch (YAML::InvalidScalar) {
111 ROS_ERROR(
"The map does not contain a free_thresh tag or it is invalid.");
115 doc[
"map_id"] >> map_id;
116 }
catch (YAML::InvalidScalar) {
117 ROS_WARN(
"The map does not contain a map_id tag or it is invalid.");
121 doc[
"origin"][0] >> origin[0];
122 doc[
"origin"][1] >> origin[1];
123 doc[
"origin"][2] >> origin[2];
124 }
catch (YAML::InvalidScalar) {
125 ROS_ERROR(
"The map does not contain an origin tag or it is invalid.");
129 doc[
"image"] >> mapfname;
131 if(mapfname.size() == 0)
133 ROS_ERROR(
"The image tag cannot be an empty string.");
136 if(mapfname[0] !=
'/')
139 char* fname_copy = strdup(fname.c_str());
140 mapfname = std::string(dirname(fname_copy)) +
'/' + mapfname;
143 }
catch (YAML::InvalidScalar) {
144 ROS_ERROR(
"The map does not contain an image tag or it is invalid.");
148 private_nh.
param(
"negate", negate, 0);
149 private_nh.
param(
"occupied_thresh", occ_th, 0.65);
150 private_nh.
param(
"free_thresh", free_th, 0.196);
152 origin[0] = origin[1] = origin[2] = 0.0;
155 ROS_INFO(
"Loading map from image \"%s\"", mapfname.c_str());
158 map_resp_.map.header.frame_id = frame_id;
160 ROS_INFO(
"Read a %d X %d map @ %.3lf m/cell",
172 void SwitchPublisher(
bool up) {
184 nav_msgs::GetMap::Response &res )
217 std::map< std::string, MapServer >
msv;
221 nav_msgs::GetMap::Response &res ) {
229 if(
msv.find(msg->data) !=
msv.end()) {
232 ROS_INFO(
"map[%s] is available.", msg->data.c_str());
236 int main(
int argc,
char **argv)
248 for(
int i=1; i < argc; i++) {
250 std::string
id = ms.map_id;
251 msv.insert(make_pair(
id, ms));
254 msv.find(
id)->second.SwitchPublisher(
true);
260 catch(std::runtime_error& e)
262 ROS_ERROR(
"map_server exception: %s", e.what());
ros::Publisher metadata_pub
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main(int argc, char **argv)
std::map< std::string, MapServer > msv
nav_msgs::MapMetaData meta_data_message_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer service
ROSCPP_DECL void spin(Spinner &spinner)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
MapServer(const std::string &fname, double res)
nav_msgs::GetMap::Response map_resp_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void loadMapFromFile(nav_msgs::GetMap::Response *resp, const char *fname, double res, bool negate, double occ_th, double free_th, double *origin, MapMode mode=TRINARY)
void MapSelect(const std_msgs::StringConstPtr &msg)
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)