1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2% 3% FILE : Env/env_er1.pl 4% 5% AUTHOR : Sebastian Sardina (2003) 6% EMAIL : ssardina@cs.toronto.edu 7% WWW : www.cs.toronto.edu/~ssardina www.cs.toronto.edu/cogrobo 8% TYPE : system independent code 9% TESTED : SWI Prolog 5.0.10 http://www.swi-prolog.org 10% ECLIPSE 5.4 http://www.icparc.ic.ac.uk/eclipse/ 11% 12% This files provides the device for working with the Evolution ER1 robot. 13% 14% An event-after event is used to implement some kind of "tracking" of 15% objects. After an object is seen, it is recorded in the database and 16% it is checked after every some time to check that he object was not lost. 17% This is done with an after_event/2 and event handlers 18% 19% This environment is self-contained (automatically it loads the required 20% libraries). It should be called as follows: 21% 22% eclipse host=<HOST> port=<PORT> -b env_rcx.pl -e start 23% 24% where HOST/PORT is the address of the environment manager socket. 25% 26% 27% Written for ECLiPSe Prolog (http://www.icparc.ic.ac.uk/eclipse/) 28% running under Linux 29%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 30% 31% November 15, 2002 32% 33% This software was developed by the Cognitive Robotics Group under the 34% direction of Hector Levesque and Ray Reiter. 35% 36% Do not distribute without permission. 37% Include this notice in any copy made. 38% 39% 40% Copyright (c) 2000 by The University of Toronto, 41% Toronto, Ontario, Canada. 42% 43% All Rights Reserved 44% 45% Permission to use, copy, and modify, this software and its 46% documentation for non-commercial research purpose is hereby granted 47% without fee, provided that the above copyright notice appears in all 48% copies and that both the copyright notice and this permission notice 49% appear in supporting documentation, and that the name of The University 50% of Toronto not be used in advertising or publicity pertaining to 51% distribution of the software without specific, written prior 52% permission. The University of Toronto makes no representations about 53% the suitability of this software for any purpose. It is provided "as 54% is" without express or implied warranty. 55% 56% THE UNIVERSITY OF TORONTO DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS 57% SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND 58% FITNESS, IN NO EVENT SHALL THE UNIVERSITY OF TORONTO BE LIABLE FOR ANY 59% SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER 60% RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF 61% CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN 62% CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 63% 64%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 65% 66% This file assumes that the following is defined in env_gen.pl: 67% 68% -- start/0 : initialization of the environment (called when loaded) 69% -- finalize/0 : finalization of the environment (called when exiting) 70% -- main_dir/1 : obtain the root IndiGolog directory 71% -- report_exog_event(A, M): 72% report exogenous event A with message M to the 73% environment manager 74% -- All compatibility libraries depending on the architecture such us: 75% -- compat_swi/compat_ecl compatibility libraries providing: 76% 77% -- The following two dynamic predicates should be available: 78% -- listen_to(Type, Name, Channel) 79% listen to Channel of Type (stream/socket) with Name 80% -- terminate/0 81% order the termination of the application 82% 83% -- The following should be implemented here: 84% 85% -- name_dev/1 : mandatory * 86% -- initializeInterfaces(L) : mandatory * 87% -- finalizeInterfaces(L) : mandatory * 88% -- execute/4 : mandatory * 89% -- handle_steam/1 : as needed 90% -- listen_to/3 : as needed 91% 92%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 93:- include(env_gen). % INCLUDE THE CORE OF THE DEVICE MANAGER 94 95 96%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 97% CONSTANTS TO BE USED 98% 99% name_dev/1 : state the name of the device manager (e.g., simulator, rcx) 100%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101% This predicate is used to state that an action should be executed 102% whenever possible. 103:- dynamic alreadySeen/2. 104 105% Name of the environment: <RCX> 106% Set name of the environment here. 107% THIS CONSTANT IS MANDATORY, DO NOT DELETE! 108name_dev(er1). 109 110 111% Port where user-events are sent from ER1 to the ER1 device driver 112port(events_er1, 9001). 113 114% Set verbose debug level 115:- set_debug_level(3). 116 117% ( for this constants read OBJECT RECOGNITION PART below) 118objectLostTime(3). % The number of seconds and object has not been seen 119 % since to assume it was lost from vision 120checkLostEvery(5). % How many seconds to wait until checking for lost objects 121 122%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 123% A - INITIALIZATION AND FINALIZATION OF INTERFACES 124% initializeInterfaces/1 and finalizeInterfaces/1 125% 126% HERE YOU SHOULD INITIALIZE AND FINALIZE EACH OF THE INTERFACES TO BE USED 127%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 128initializeInterfaces(L) :- 129 report_message(system(3),'Establishing connection to ER1 API port'), 130 % 1 - Obtain IP and Port from L 131 member([iper1,Host], L), 132 member([porter1, SPort], L), % Get Host and Port of ER1 from L 133 string_to_number(SPort, Port), 134 % 2 - Start ER1 main communication and events communication 135 initializeER1(Host, Port), 136% initializeER1_Events, 137 report_message(system(2), 138 'Connection to ER1 API port established successfully'), 139 % 3 - Set handler for the recognizing lost objects 140 checkLostEvery(CheckEverySeconds), 141 set_event_handler(eobjects_lost, hobjects_lost/0), 142 event_after_every(eobjects_lost, CheckEverySeconds), 143 report_message(system(3),'After event for lost object started successfully'), 144 % 4 - Start listening for events on ER1 145 listen_to_er1, 146 report_message(system(3),'Exogenous events report system started successfuly in ER1'). 147 148finalizeInterfaces(_) :- 149 finalizeER1, 150% finalizeER1_Events, 151 report_message(system(3),'Disconnection from ER1 successful'). 152 153% Set main connection to API socket (port 9000 of ER1) 154initializeER1(Host, Port) :- 155 printKbInstructions, 156 socket(internet, stream, comm_er1), 157 connect(comm_er1, Host/Port), 158 assert(listen_to(socket, comm_er1, comm_er1)). 159 160% Set the extra user-event communication to ER1 (port 9001 of ER1) 161initializeER1_Events :- 162 socket(internet, datagram, events_er1), 163 port(events_er1, PortEvents), 164 bind(events_er1, 'localhost'/PortEvents), 165 assert(listen_to(socket, events_er1, events_er1)). 166 167% Finalize main connection to API socket (port 9000 of ER1) 168finalizeER1 :- 169 retract(listen_to(socket, comm_er1, comm_er1)), 170 close(comm_er1). 171 172% Finalize the extra user-event communication to ER1 (port 9001 of ER1) 173finalizeER1_Events :- 174 retract(listen_to(socket, events_er1, events_er1)), 175 close(events_er1). 176 177 178% printKbInstructions: Print instructions on how to enter keyboard input 179printKbInstructions :- 180 writeln('*********************************************************'), 181 writeln('* NOTE: This is the ER1 device manager'), 182 writeln('* This window will show the communication'), 183 writeln('* with the ER1 Evolution Robot'), 184 writeln('*********************************************************'), 185 nl. 186 187 188%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 189% B - HANDLERS FOR EACH STREAM/SOCKET THAT IS BEING HEARD: handle_stream/1 190% 191% HERE YOU SHOULD WRITE HOW TO HANDLE DATA COMMING FROM EACH OF THE 192% INTERFACES/CHANNELS USED 193%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 194 195% Handle data comming from ER1: 'play done', 'move done', etc. (ER1 port 9000) 196handle_stream(comm_er1) :- 197 read_response_from_er1(Data), 198 string_to_atom(Data, A), 199 (A=end_of_file -> 200 true % Socket closed! 201 ; 202 (isAnObject(A, O) -> 203 (alreadySeen(O, _) -> 204 updateObject(O) 205 ; 206 updateObject(O), 207 report_exog_event(A, _) 208 ) 209 ; 210 report_exog_event(A, _) 211 ) 212 ). 213 214% Handle data comming from ER1 User-Events socket (ER1 port 9001) 215handle_stream(events_er1) :- 216 read_userevents_er1(Data), % Read a term from socket events_er1 217 A=Data, 218 (A=end_of_file -> 219 true % Socket closed! 220 ; 221 report_exog_event(A, _) 222 ). 223 224 225 226 227%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 228% C - EXECUTION MODULE: execute/4 229% 230% This part implements the execution capabilities of the environment 231% 232% execute(Action, Type, Sensing) : execute Action of Type and return Sensing 233%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 234execute(Action, T, N, Sensing) :- 235 send_command_to_er1('', _), 236 executeER1(Action, T, N, Sensing),!, 237 listen_to_er1. % Set ER1 to the default state for reporting exog events 238 239 240% Actual execution of Action in ER1 241executeER1(Action, T, N, Sensing) :- member(T, [sensing, simsensing]), !, 242 report_message(action, ['Executing sensing action: *',(Action,N),'*']), 243 send_command_to_er1(Action, Response), !, % SEND ACTION TO ER1! 244 extract_sensing(Response, Sensing), 245 report_message(action, 246 ['RESPONSE for action: *',(Action,N),' : ', Sensing]). 247 248executeER1(Action, _, N, Response) :- 249 report_message(action, 250 ['Executing non-sensing action: *',(Action,N),'*']), 251 send_command_to_er1(Action, Response). % SEND ACTION TO ER1! 252 253 254 255 256 257%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 258%%%%%%%%%%%%%%%%%%%%%% COMMUNICATION WITH ER1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 259%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 260 261% Send Command to ER1 and wait for Response from ER1 262send_command_to_er1(Command, Response) :- 263 write(comm_er1, Command), 264 nl(comm_er1), 265 flush(comm_er1), !, 266 read_response_from_er1(Response). % Read acknowledgment from ER1 267% string_to_atom(OK, 'OK'), 268% substring(Response, OK, 1). 269send_command_to_er1(_, failed). 270 271 272% Read a line from ER1 273read_response_from_er1(Command) :- 274 read_string(comm_er1, end_of_line,_, Command). 275 276% Sets ER1 to send all events 277listen_to_er1 :- 278 send_command_to_er1(events, _). 279 280% Read Data from the User-Events socket 281read_userevents_er1(Data) :- 282 read(events_er1, Data). 283 284%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 285%%%%%%%%%%%%%%%%%%%%%% OBJECT RECOGNITION PART %%%%%%%%%%%%%%%%%%%%%%%%%%%% 286% 287% ER1 reports an event "object <name object> ...." every time an object is 288% seen such that the rate between features matched and total features of the 289% object is greater than the confidence threshold. This may cause way too many 290% object spot events to be reported to the environment manager. 291% To solve this problem we take the following approach: 292% 293% 1) An exogenous event "object <name> ..." is reported whenever an object 294% is seen for the first time 295% 2) If an already seen object is seen again, nothing is reported 296% 3) If an object has not been seen for a while, then a "lostObject <name>" 297% event is reported to the environment manager. 298% 299% In this way we use two exogenous event: one for reporting the visualization 300% of an object for the first time and another exogenous event to report that 301% an already seen object has not been seen for a while. At that point, such 302% object can be seen again. 303% 304% IMPLEMENTATION: 305% 306% 1) whenever an object is seen, the clause alreadySeen(Object, Time) is 307% updated where Time is the current time when the Object has just been seen 308% 2) there is an event_every_after/2 that checks every X seconds what 309% objects has not been seen in the last Y seconds. In case an object has not 310% been seen for Y seconds, a lostObject event is reported for hte object and 311% its clause alreadySeen/2 is removed for such object. 312% 313% X seconds is set with clause: checkLostEvery(Seconds) 314% Y seconds is set with clause: objectLostTime(Seconds) 315%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 316% Event is a "spot object" event 317isAnObject(Event, Object) :- 318 get_object_event(Event, Object, _, _, _, _). 319 320% Update alreadySeen/2 for Object 321updateObject(Object) :- 322 retract_all(alreadySeen(Object, _)), 323 get_flag(unix_time, Now), 324 assert(alreadySeen(Object, Now)). 325 326% Event handler for the lost object event "eobject_lost" 327hobjects_lost :- 328 get_flag(unix_time, Now), 329 alreadySeen(Object, Time), 330 Dif is Now-Time, 331 objectLostTime(Seconds), 332 Dif > Seconds, % More than Seconds without seeing the object 333 retract(alreadySeen(Object, Time)), 334 concat_atom(['lostObject ',Object], LostEvent), 335 report_exog_event(LostEvent,_), 336 fail. 337hobjects_lost. 338 339 340% An object stop event has the following form: 341% "object <name> <no features matched> <total features> <x> <x> <distance> 342% e.g., object "warning sign" 5 30 89 89 67.6 343get_object_event(AString, Object, Rate, X, Y, Distance) :- 344 string_to_atom(String, AString), 345 string_to_atom(Quote,'\"'), 346 string_to_atom(Space,' '), 347 split_string(String, Space, Quote, List), 348 List=[Type|RList], 349 string_to_atom(Type, object), % Check it's an object spot event 350 reverse(RList, [SDistance, SY, SX, SFT, SFM|LObject]), 351 string_to_number(SX, X), % Get coordinate X 352 string_to_number(SY, Y), % Get coordinate Y 353 string_to_number(SFT, FT), % Get total features 354 string_to_number(SFM, FM), % Get features matched 355 Rate is (100*FM)/FT, % Calculate rate 356 string_to_number(SDistance, Distance), % Get distance to object 357 reverse(LObject, LObject2), 358 join_string(LObject2, Space, SObject), % Build the object name 359 string_to_atom(SObject, Object). 360 361%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 362%%%%%%%%%%%%%%%%%%%%%%%%%%%% OTHER CODE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 363%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 364:- set_event_handler(170, my_system_error_handler/2). 365 366my_system_error_handler(E, Goal) :- 367 ( 368 errno_id(`Interrupted system call`), 369% errno_id(170, M), errno_id(M), % M is "Unknown error 170" ?? 370 restartable_builtin(Goal) 371 -> 372 call(Goal) 373 ; 374 errno_id(M), 375 report_message(error, M), 376 read(_), 377 error(default(E), Goal) 378 ). 379 380% Builtins that can raise EINTR and can be restarted after that 381restartable_builtin(accept(_,_,_)). 382restartable_builtin(cd(_)). 383restartable_builtin(close(_)). 384restartable_builtin(connect(_,_)). 385restartable_builtin(select(_,_,_)). 386restartable_builtin(wait(_,_)). 387 388 389%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 390% EOF: Env/env_er1.pl 391%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%