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