Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#include "PoiManager.h"
#include <sstream>
#include "homer_mapnav_msgs/PointsOfInterest.h"
#include <ros/ros.h>
using namespace std;
PoiManager::PoiManager(ros::NodeHandle *nh)
{
m_POIsPublisher = nh->advertise<homer_mapnav_msgs::PointsOfInterest>("/map_manager/poi_list", 1);
}
PoiManager::PoiManager ( std::vector<homer_mapnav_msgs::PointOfInterest> pois )
{
//copy POIs
m_Pois = pois;
}
std::vector<homer_mapnav_msgs::PointOfInterest> PoiManager::getList()
{
return m_Pois;
}
bool PoiManager::addPointOfInterest (const homer_mapnav_msgs::PointOfInterest::ConstPtr &poi )
{
//make sure there's no POI with the same name
if ( poiExists ( poi->name ) )
{
ostringstream stream;
stream << "Poi with name " << poi->name << " already exists! Doing nothing.";
ROS_WARN_STREAM ( stream.str() );
return false;
}
//copy poi & assigning new id
homer_mapnav_msgs::PointOfInterest new_poi= *poi;
ROS_INFO_STREAM ("Adding POI '" << new_poi.name << "'.");
//insert into list
m_Pois.push_back ( new_poi );
broadcastPoiList();
return true;
}
bool PoiManager::modifyPointOfInterest (const homer_mapnav_msgs::ModifyPOI::ConstPtr &poi )
{
std::string name = poi->old_name;
std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
*it=poi->poi;
broadcastPoiList();
return true;
}
}
ROS_ERROR_STREAM ( "Cannot modify: POI does not exist!" );
return false;
}
void PoiManager::replacePOIList(std::vector<homer_mapnav_msgs::PointOfInterest> poilist)
{
m_Pois = poilist;
broadcastPoiList();
}
bool PoiManager::poiExists ( std::string name )
{
std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
return true;
}
}
return false;
}
bool PoiManager::deletePointOfInterest (std::string name )
{
std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
for ( it=m_Pois.begin() ; it != m_Pois.end(); it++ )
{
if ( it->name == name )
{
ROS_INFO_STREAM ("Erasing POI " << name << ".");
it = m_Pois.erase ( it );
broadcastPoiList();
return true;
}
}
ROS_ERROR_STREAM ("POI " << name << " does not exist.");
return false;
}
void PoiManager::broadcastPoiList() {
ostringstream stream;
//print the current list
std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
stream << "Contents of POI list:\n";
homer_mapnav_msgs::PointsOfInterest poiMsg;
for ( it = m_Pois.begin(); it != m_Pois.end(); it++ ) {
stream << " POI " << it->name << "', " << it->type
<< ", (" << it->pose.position.x << "," << it->pose.position.y << "), '" << it->remarks << "'\n";
}
poiMsg.pois = m_Pois;
ros::Rate poll_rate(10);
while(m_POIsPublisher.getNumSubscribers() == 0)
poll_rate.sleep();
m_POIsPublisher.publish(poiMsg);
ROS_DEBUG_STREAM( stream.str() );
}