src
map_saver.cpp
Go to the documentation of this file.
1
/*
2
* map_saver
3
* Copyright (c) 2019, Samsung Research America
4
*
5
* THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6
* COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7
* COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8
* AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9
*
10
* BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11
* BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12
* CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13
* CONDITIONS.
14
*
15
*/
16
17
/* Author: Steven Macenski */
18
19
#include "
slam_toolbox/map_saver.hpp
"
20
21
namespace
map_saver
22
{
23
24
/*****************************************************************************/
25
MapSaver::MapSaver
(
ros::NodeHandle
& nh,
const
std::string& map_name)
26
: nh_(nh), map_name_(map_name), received_map_(false)
27
/*****************************************************************************/
28
{
29
server_
=
nh_
.
advertiseService
(
"save_map"
, &
MapSaver::saveMapCallback
,
this
);
30
sub_
=
nh_
.
subscribe
(
map_name_
, 1, &
MapSaver::mapCallback
,
this
);
31
}
32
33
/*****************************************************************************/
34
void
MapSaver::mapCallback
(
const
nav_msgs::OccupancyGrid& msg)
35
/*****************************************************************************/
36
{
37
received_map_
=
true
;
38
}
39
40
/*****************************************************************************/
41
bool
MapSaver::saveMapCallback
(
42
slam_toolbox_msgs::SaveMap::Request& req,
43
slam_toolbox_msgs::SaveMap::Response& resp)
44
/*****************************************************************************/
45
{
46
if
(!
received_map_
)
47
{
48
ROS_WARN
(
"Map Saver: Cannot save map, no map yet received on topic %s."
,
49
map_name_
.c_str());
50
return
false
;
51
}
52
53
const
std::string name = req.name.data;
54
if
(name !=
""
)
55
{
56
ROS_INFO
(
"SlamToolbox: Saving map as %s."
, name.c_str());
57
int
rc = system((
"rosrun map_server map_saver -f "
+ name).c_str());
58
}
59
else
60
{
61
ROS_INFO
(
"SlamToolbox: Saving map in current directory."
);
62
int
rc = system(
"rosrun map_server map_saver"
);
63
}
64
ros::Duration
(1.0).
sleep
();
65
return
true
;
66
}
67
68
}
// end namespace
map_saver::MapSaver::saveMapCallback
bool saveMapCallback(slam_toolbox_msgs::SaveMap::Request &req, slam_toolbox_msgs::SaveMap::Response &resp)
Definition:
map_saver.cpp:41
map_saver
Definition:
map_saver.hpp:26
map_saver::MapSaver::sub_
ros::Subscriber sub_
Definition:
map_saver.hpp:43
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
map_saver::MapSaver::nh_
ros::NodeHandle nh_
Definition:
map_saver.hpp:41
map_saver::MapSaver::MapSaver
MapSaver(ros::NodeHandle &nh, const std::string &service_name)
Definition:
map_saver.cpp:25
map_saver::MapSaver::map_name_
std::string map_name_
Definition:
map_saver.hpp:44
ROS_WARN
#define ROS_WARN(...)
map_saver::MapSaver::mapCallback
void mapCallback(const nav_msgs::OccupancyGrid &msg)
Definition:
map_saver.cpp:34
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
map_saver::MapSaver::server_
ros::ServiceServer server_
Definition:
map_saver.hpp:42
ros::Duration::sleep
bool sleep() const
ROS_INFO
#define ROS_INFO(...)
map_saver::MapSaver::received_map_
bool received_map_
Definition:
map_saver.hpp:45
ros::Duration
ros::NodeHandle
map_saver.hpp
slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55