RCL - RoboCLuedo  v1.0
Francesco Ganci - S4143910 - Experimental Robotics Lab - Assignment 1
armor_cluedo.cpp
Go to the documentation of this file.
1 
2 
14 
15 
16 
17 // default constructor
18 ArmorCluedo::ArmorCluedo( bool debugmode ): ArmorTools( debugmode )
19 {
20  // ...
21 }
22 
23 
24 
25 // destructor
27 {
28  // ...
29 }
30 
31 
32 
33 // init the interface
34 bool ArmorCluedo::Init( std::string ontologyPath )
35 {
36  // try to connect to the service
37  this->ConnectAndLoad( ontologyPath );
38 
39  // test for errors in the connection
40  if( ( this->GetLastErrorCode( ) != 0 ) || !this->TestInterface( ) )
41  {
42  ARMOR_ERR( "in connecting and loading the ontology. " );
43  return false;
44  }
45 
46  // connection successful
47  return true;
48 }
49 
50 
51 
52 // add an individual
53 bool ArmorCluedo::AddIndiv( std::string indivname, std::string classname, bool makeDisjoint )
54 {
55  // return false if the individual is a discarded hypothesis
56  if( classname == "HYPOTHESIS" )
57  {
58  if( this->GetPositionOf( indivname, DiscardHypotheses ) != DiscardHypotheses.end( ) )
59  return false;
60  }
61 
62  // track the individual
63  if( !this->TrackIndiv( indivname ) ) return false;
64 
65  // send the ADD command
66  if( !this->SendCommand( "ADD", "IND", "CLASS", indivname, classname ) )
67  return false;
68 
69  // disjoint this individual from the other
70  if( makeDisjoint ) DisjointAllIndiv( indivname );
71  // this->SendCommand( "REASON" );
72  // this->SendCommand( "DISJOINT", "IND", "CLASS" );
73  // this->SendCommand( "REASON" );
74 
75  return true;
76 }
77 
78 
79 
80 // check the class of an individual
81 std::vector<std::string> ArmorCluedo::GetClassOfIndiv( std::string indivname, bool deep = false )
82 {
83  // ask for the class of a given individual
84  if( !this->SendCommand( "QUERY", "CLASS", "IND", indivname, SS( deep? "true" : "false" ) ) )
85  {
86  ARMOR_ERR( "unable to find the classes of the individual " << indivname );
87  return std::vector<std::string>();
88  }
89 
90  // filter the response and return it
91  return FilterVector( this->GetLastRes( ).queried_objects );
92 }
93 
94 
95 
96 // find the individuals belonging to a class
97 std::vector<std::string> ArmorCluedo::GetIndivOfClass( std::string classname )
98 {
99  if( !this->SendCommand( "QUERY", "IND", "CLASS", classname ) )
100  {
101  ARMOR_ERR( "unable to find the individuals inside the class " << classname );
102  return std::vector<std::string>();
103  }
104 
105  // filter the response and return it
106  return FilterVector( this->GetLastRes( ).queried_objects );
107 }
108 
109 
110 
111 // check if an individual exists
112 bool ArmorCluedo::ExistsIndiv( std::string indivname )
113 {
114  this->SendCommand( "QUERY", "IND", "", indivname );
115 
116  if( this->GetLastRes( ).success )
117  {
118  // exclude discarded hypotheses
119  if( this->GetPositionOf( indivname, DiscardHypotheses ) != DiscardHypotheses.end( ) )
120  return false;
121  else
122  return true;
123  }
124  else
125  return false;
126 }
127 
128 
129 
130 // set a property true
131 bool ArmorCluedo::SetObjectProperty( std::string prop, std::string Aelem, std::string Belem )
132 {
133  // these elements must be added before calling this function
134  if( !ExistsItem( Aelem, individuals ) || !ExistsItem( Belem, individuals ) )
135  return false;
136 
137  // set the protery true by a service call
138  if( !this->SendCommand( "ADD", "OBJECTPROP", "IND", prop, Aelem, Belem ) )
139  {
140  ARMOR_ERR( "unable to set the property " << "(" << Aelem << ", " << Belem << "):" << prop );
141  return false;
142  }
143 
144  return true;
145 }
146 
147 
148 
149 // get the values of a property related to a gven individual
150 std::vector<std::string> ArmorCluedo::GetValuedOfIndiv( std::string prop, std::string indivname )
151 {
152  if( !this->SendCommand( "QUERY", "OBJECTPROP", "IND", prop, indivname ) )
153  {
154  ARMOR_ERR( "unable to find the values of the property " << prop << " related to the individual " << indivname );
155  return std::vector<std::string>();
156  }
157 
158  // filter the response and return it
159  return FilterVector( this->GetLastRes( ).queried_objects );
160 }
161 
162 
163 
164 // find all the complete hypotheses
165 std::vector<std::string> ArmorCluedo::FindCompleteHypotheses( )
166 {
167  std::vector<std::string> temp = GetIndivOfClass( "COMPLETED" );
168 
169  // WORKAROUND for issue "REMOVE IND CLASS"
170  // remove the discarded hypotheses
171  for( auto it = DiscardHypotheses.begin() ; it != DiscardHypotheses.end(); ++it )
172  {
173  auto pos = this->GetPositionOf( *it, temp );
174  if( pos != temp.end( ) )
175  temp.erase( pos );
176  }
177 
178  return temp;
179 }
180 
181 
182 
183 // find all the inconsistent hypotheses
184 std::vector<std::string> ArmorCluedo::FindInconsistentHypotheses( )
185 {
186  return GetIndivOfClass( "INCONSISTENT" );
187 }
188 
189 
190 
191 // remove one hyothesis
192 bool ArmorCluedo::RemoveHypothesis( std::string hypTag )
193 {
194  // check if it exists
195  if( !ExistsIndiv( hypTag ) ) return false;
196 
197  /* sarebbe molto bello ... ma purtroppo REMOVE IND CLASS รจ buggato.
198 
199  // delete the individual from all the classes
200  std::vector<std::string> classnames = this->GetClassOfIndiv( hypTag );
201  for( std::string cname : classnames )
202  if( !this->SendCommand( "REMOVE", "IND", "CLASS", hypTag, cname ) )
203  return false;
204 
205  */
206 
207  // add the hypothesis to the list of the discarded hypotheses
208  DiscardHypotheses.push_back( hypTag );
209 
210  // untrack the individual
211  auto pos = this->GetPositionOf( hypTag, individuals );
212  if( pos != individuals.end( ) )
213  {
214  individuals.erase( pos );
215  }
216 
217  return true;
218 }
219 
220 
221 
222 // rewrite a string like '<uri#value>' into 'value'
223 std::string ArmorCluedo::FilterValue( std::string raw )
224 {
225  std::size_t pos;
226  if( ( pos = raw.find( "#" ) ) == std::string::npos )
227  return raw;
228  else
229  return raw.substr( pos+1, raw.length()-pos-2 );
230 }
231 
232 
233 
234 // filter all the strings inside the array
235 std::vector<std::string> ArmorCluedo::FilterVector( std::vector<std::string>& itemlist )
236 {
237  std::vector<std::string> returnlist;
238 
239  // filter each element
240  for( std::string item : itemlist )
241  returnlist.push_back( this->FilterValue( item ) );
242 
243  return returnlist;
244 }
245 
246 
247 
248 // check if a string exists in one array
249 bool ArmorCluedo::ExistsItem( std::string item, const std::vector<std::string>& container )
250 {
251  for( std::string s : container )
252  if( s == item ) return true;
253 
254  return false;
255 }
256 
257 
258 
259 // track individual
260 bool ArmorCluedo::TrackIndiv( std::string indivname )
261 {
262  // check if the individual was already defined in the array
263  if( ExistsItem( indivname, this->individuals ) )
264  {
265  // ARMOR_ERR( "the individual " << indivname << "is already tracked. " );
266  return false;
267  }
268 
269  // add the item to the list of items
270  this->individuals.push_back( indivname );
271 
272  return true;
273 }
274 
275 
276 
277 // disjoint all the individuals
278 void ArmorCluedo::DisjointAllIndiv( std::string from = "" )
279 {
280  if( from == "" )
281  {
282  // iterative brute force disjoint
283  for( std::string s : this->individuals )
284  this->DisjointAllIndiv( s );
285  }
286  else
287  {
288  // disjoint wrt one element
289  for( std::string s : this->individuals )
290  if( s != from )
291  this->SendCommand( "DISJOINT", "IND", "", s, from );
292  }
293 }
294 
295 
296 
297 // search for a specific iterator on a vector
298 // it return vector::end() if the element is not contained
299 std::vector<std::string>::iterator ArmorCluedo::GetPositionOf( std::string tag, std::vector<std::string>& list )
300 {
301  // ARMOR_INFO( "[ArmorCluedo::GetPositionOf] listsize=" << list.size() );
302  for( auto it = list.begin(); it != list.end( ); ++it )
303  {
304  if( *it == tag ) return it;
305  }
306 
307  // not found
308  return list.end( );
309 }
ArmorTools::ConnectAndLoad
bool ConnectAndLoad(std::string path, std::string uri=ARMOR_DEFAULT_URI, bool manipulationFlag=true, std::string reasoner=ARMOR_DEFAULT_REASONER, bool buffered_reasoner=true)
connect to the server and load the ontology from file.
Definition: armor_tools.cpp:213
SS
#define SS(this_string)
Definition: armor_tools.h:46
ArmorCluedo::AddIndiv
bool AddIndiv(std::string indivname, std::string classname, bool makeDisjoint=true)
add an individual to the ontology
Definition: armor_cluedo.cpp:53
ARMOR_ERR
#define ARMOR_ERR(msg)
Definition: armor_tools.h:41
armor_cluedo.h
additional aRMOR C++ interface
ArmorCluedo::SetObjectProperty
bool SetObjectProperty(std::string prop, std::string Aelem, std::string Belem)
set a property true
Definition: armor_cluedo.cpp:131
ArmorCluedo::Init
bool Init(std::string ontologyPath)
initizalize the interface
Definition: armor_cluedo.cpp:34
ArmorCluedo::FilterValue
std::string FilterValue(std::string raw)
rewrite a string like '<uri#value>' into 'value'
Definition: armor_cluedo.cpp:223
ArmorTools::GetLastRes
armor_msgs::ArmorDirectiveRes & GetLastRes()
get a reference to the last response
Definition: armor_tools.cpp:385
ArmorCluedo::GetValuedOfIndiv
std::vector< std::string > GetValuedOfIndiv(std::string prop, std::string indivname)
get the values of a property related to a gven individual
Definition: armor_cluedo.cpp:150
ArmorCluedo::GetIndivOfClass
std::vector< std::string > GetIndivOfClass(std::string classname)
find the individuals belonging to a class
Definition: armor_cluedo.cpp:97
ArmorCluedo::~ArmorCluedo
~ArmorCluedo()
class destructor
Definition: armor_cluedo.cpp:26
ArmorTools::TestInterface
bool TestInterface()
check the status of the interface
Definition: armor_tools.cpp:351
ArmorCluedo::FilterVector
std::vector< std::string > FilterVector(std::vector< std::string > &itemlist)
filter all the strings inside the array
Definition: armor_cluedo.cpp:235
ArmorCluedo::FindInconsistentHypotheses
std::vector< std::string > FindInconsistentHypotheses()
find all the inconsistent hypotheses
Definition: armor_cluedo.cpp:184
ArmorTools::SendCommand
bool SendCommand(std::string command, std::string first_spec="", std::string second_spec="", std::string arg1="", std::string arg2="", std::string arg3="", std::string arg4="", std::string arg5="", bool printRequest=false)
fill in a command and send it to aRMOR
Definition: armor_tools.cpp:360
ArmorTools
A minimal C++ client for aRMOR.
Definition: armor_tools.h:90
ArmorCluedo::ArmorCluedo
ArmorCluedo(bool debugmode=ARMOR_DEFAULT_DEBUGMODE)
class constructor of ArmorCluedo
Definition: armor_cluedo.cpp:18
ArmorCluedo::GetClassOfIndiv
std::vector< std::string > GetClassOfIndiv(std::string indivname, bool deep)
get the class of a given individual
Definition: armor_cluedo.cpp:81
ArmorCluedo::ExistsIndiv
bool ExistsIndiv(std::string indivname)
check if an individual exists
Definition: armor_cluedo.cpp:112
ArmorTools::GetLastErrorCode
int GetLastErrorCode()
err code referred to the last call
Definition: armor_tools.cpp:319
ArmorCluedo::RemoveHypothesis
bool RemoveHypothesis(std::string hypTag)
discard one hypothesis
Definition: armor_cluedo.cpp:192
ArmorCluedo::FindCompleteHypotheses
std::vector< std::string > FindCompleteHypotheses()
find all the complete hypotheses
Definition: armor_cluedo.cpp:165