Home

Software Manual - Research Group on the Foundations of Artificial

1. sfDISPLAY 0 10 2 Controls display update rate State is the number of 100 ms cycles between updates Value 10 is once per second for example Value of 0 turns the display off TRUE FALSE Controls local global viewpoint of display window sfWAKE TRUE FALSE Controls drawing of breadcrumb wake behind robot EP sfSTI TRUE FALSE Controls single step mode when connected to the Pioneer simulator sfOCCGRID TRUE FALSE Controls display of occupancy grid results If enabled enables global viewpoint Default state values are in bold typeface void sfMessage char str void sfSMessage char str sfMessage writes the null terminated string st r into the message section of the information area in the Saphira main window followed by a carriage return Use sfSMessage to format the string similar to the standard printf C function which accepts optional arguments that are to be inserted into the string A problem in the Colbert evaluator prevents floating point numbers from being printed using sfSMessage As a workaround convert them to integers before calling sfSMessage void sfKeyProcFn int fn int myKeyFn int ch The sfKeyProcFn registers an optional user key process callback with the prototype of myKeyFn It is called by Saphira whenever the user presses a key when the main Saphira window is active The ch argument is the character representing the key that was pressed and is operat
2. 0 1320 350 X control Front Forward and rotational velocities Figure 8 1 Saphira s LPS coordinate system The side pointing sonars are somewhat useful for obstacle avoidance because they signal when it isn t useful to turn to one side or the other But their main purpose is to delineate features for the recognition algorithms They are good for this purpose because the robot is often moving parallel to wall surfaces and by accumulating the side sonar readings it s possible to pick out a nice straight feature The buffers differ slightly in how they accumulate sonar readings based on the difference in their utility They are all circular buffers that is a new reading replaces the oldest one The front buffer sraw_buf accumulates one reading each time a sonar is fired regardless of whether it sees anything If nothing is found the valid flag at that buffer position is set to 0 otherwise it is set to 1 and the xbuf and ybuf slots are set to the position of the sonar reading in the robot s local coordinate system This strategy guarantees that the front buffer can be cleared out after a short amount of time when nothing is in the robot s way For example if the robot is getting 20 front sonar readings a second and the front buffer is 30 elements long it will be completely clear in 1 5 seconds if there is nothing in front of the robot The two side buffers sr_lbuf and s1_buf accumulate sonar readings only when
3. Error Bookmark not defined Map File int sfLoadMapFile char name lt Unix MSW gt LLS int sfLoadMapFile char name int vref lt Mac gt 115 Occupancy int sfOccBox int xy int cx int cy int h int w 105 132 int sfOccBoxRet int xy int cx int cy float x float y int sfOccPlane int xy int source int int sfOccPlaneRet int xy int source int d float x OS and Window Functions int myButtonFn int x int y int b float y int myKeyFn int ch void sfButtonProcFn int fn void sfErrMessage char str void sfErrSMessage char str Error void sfKeyProcFn int fn void sfOnConnectFn void fn void sfOnDisconnectFn void fn void sfOnStartupFn void fn float sfScreenToWorldX int x int y float sfScreenToWorldY int x int y int h int w d int sl void sfSetDisplayState int menu int state void sfSMessage char str void sfStartup HANDLE hInst int cmdShow void sfStartup int async void sfPause in ms int sfIsConnected Packet Functions char sfReadClientString void int sfClientBytes void int sfConnectToRobot int port char name int sfHaveClientPacket void int sfReadClientByte void int sfReadClientSint void int fFReadClientUsint void int sfReadClientWord void int sfWaitClientPacket int ms void sfDisconnectFromRobot void void sfProcessClientPacket void void sfResetRobotVa
4. 2 3 9 Processes Window The Processes window displays the states of all micro tasks in the Saphira client multitasking queue Figure 2 7 Open it from the Functions Processes menu in the main window The Processes window contains a scrolled list where each entry consists of the micro task name and state The display is updated in real time as the micro task state changes 20 Saphira Software Manual Process motor clamp sonar wake draw test matching test where people tracking speech input Figure 2 7 A sample Saphira Processes window You may interrupt a running micro task by selecting it in the window and pressing the Enter key or by double clicking with the mouse This action forces the micro task state to INT interrupt Resume an interrupted micro task with the same action which forces the micro task state to RES resume An interrupted micro task does not automatically suspended processing that depends on how the micro task handles the interrupt state Some micro task ignore the interrupt and continue with their tasks For example the motor micro task does not care what its state is it always performs the same action of sending motor commands to the robot server In general you should only interrupt micro tasks that you have added to the Saphira application and for which there is a defined interrupt behavior 2 3 10 Activities Window Saphira s Activities window shows the state and relationship of all curre
5. 8 10 Maps and Registration Saphira has a set of routines for creating and using global maps of an indoor environment This facility is still under construction this section gives an overview of current capabilities and some of the functions a client program can access A map is a collection of artifacts with global position information Typically a map will consist of corridors doors and walls all artifacts of the offices where the robot is situated Maps may be loaded and deleted using the interface Files menu or by function calls A map can either be created by the robot as it wanders around the environment or you may create one as a file You can also save the map created by the robot to a file for later recall 8 10 1 Map File Format A map file contains optional comments designated with a semi colon prefix and lines specifying artifacts in the map All coordinates for artifacts are global coordinates For example the following is a portion of the map file for SRI s Artificial Intelligence Center 7 Map of a small portion of the SRI Artificial Intelligence Center a a X Y Th Length Width CORRIDOR 20007 3000 OG 3500 800 CORRIDOR 1000 2000 90 6000 1000 DOOR 3 S000 2600 90 1000 DOOR 4 L500 L000 180 1000 JUNCTION 1500 3000 800 WALL 1000 4000 0 1000 WALL 800 3500 90 400 WALL 800 4500 90 400 The CORRIDOR lines define a series of corridor artifacts The number in parentheses
6. approximately 3 pixels per degr KY This constant lets a client convert from image pixel coordinates to angles The area is the approximate size of the blob in pixels If the area is zero no blob was found In BLOB_BB_MODE the bounding box of the blob is also returned with h and w being the height and width of the box in pixels In LINE_MODE the slots x first and num are active The value x is the horizontal center of the line first is the first bottom most row with a line segment and num is the number of consecutive rows with line segments If no line was found num is zero Global variables hold information for each channel as follows extern struct vinfo sfVaInfo sfVbInfo sfVcInfo For example to see if channel A is in BLOB_MODE use sfVaInfo type 9 3 Sample Vision Application The sample Saphira client which enables the FTVS can be found as the source file handler src apps btech c and chroma c The compiled executables are found in the bin directory These files define functions to put the channels into BLOB_BB_MODE to turn the robot looking for a blob on channel A to draw the blob on the graphics window and to approach the blob void setup_vision_system void Sets up parameters of the vision system putting all channels into BLOB_BB_MODE and initializing line parameters int found_blob int channel int delta Returns the X image coordinate of a blob on channel O A 1 B 2 C if the blob s cen
7. Activ MEDIA INC __ REAL WORLD Saphira Software Manual Saphira Version 6 1 ii Saphira Software Manual Copyright 1997ActivMedia Inc All rights reserved Under international copyright laws this manual or any portion may not be copied or on any way duplicated without the expressed written consent of ActivMedia Inc The Saphira libraries and software on disk which are available for network download are solely owned and copyrighted by SRI International formerly Stanford Research Institute Developers and users are authorized by revocable license to develop and operate Saphira based custom software for personal research and educational use only Duplication distribution reverse engineering or commercial application of the software without the expressed written consent of SRI International is explicitly forbidden The various names and logos for products used in this manual are registered trademarks or trademarks of their respective companies Mention of any third party hardware or software constitutes neither an endorsement nor a recommendation Saphira Manual Version 6 1 October 1997 iii Contents 1 SAPHIRA SOFTWARE amp RESOURCES 1 1 Saphira Client Server 1 2 Colbert Robot Programming Language 1 3 Behavior Compiler and Executive 1 4 Robot Simulator 1 5 Required and Optional Components 1 6 Saphira Client Installation 1 7 Saphira Quick Start 1 8 Additional Resources 1 8 1 FTP Software Archive
8. Behaviors and direct motion control will conflict if a client attempts to use both at the same time to control the robot For example in the bin saphira sample client the bump and go procedure uses direct motion control while the obstacle avoidance routines are behaviors The bump and go procedure is inactive until the robot hits something at which point it takes over motion control and backs the robot up To suppress behavior execution during this time the sfBehaviorControl flag is setto 0 When bump and go is finished it resets the flag to 1 and the behaviors resume control int sfBehaviorControl A value of 0 suppress behavior control of motion although all behaviors are still evaluated A value of 1 allows the results of behavior evaluation to control the robot motion 56 Saphira Software Manual 5 6 Fuzzy variables Fuzzy variables are floating point numbers in the range 0 1 Several functions are defined for creating fuzzy variables from single numeric values 5 6 1 Fuzzy variable creation functions float straight_up float x float min float max float down_straight float x float min float max float f_greater float x float c float delta float f_smaller float x float c float delta float f_eq float x float c float delta The functions straight_up and down_straight convert numerical values into a fuzzy value based on its inclusion in a range Both take three arguments the value itself the start of the ran
9. inch 1 4 MB floppy diskette or at the ActivMedia Internet site Each archive is configured and compiled for a particular operating system such as Windows95 NT or Solaris Choose the version that matches your client computer system You may obtain additional Saphira archives for other platforms and updates from the ActivMedia Internet site see Additional Resources below in this chapter for details 1 We do not recommend using Macintosh for Saphira development at this time because the native operating system System 8 does not fully support multitasking which is essential for Saphira operation Saphira Software Manual The Windows95 NT versions are PKZIP d and UNIX versions come GZIP d and TAR d To decompress the software into useable files you will need the appropriate decompression archive software pkunzip gunzip or compatible program consult the respective program s user manual or help files For Linux and other UNIX users we recommend that you create a saphira directory in usr local or some other publicly accessible directory and set the appropriate permissions for access and use by your robotics groups Copy the Saphira archive to that directory then uncompress and untar the Saphira archive For example with Linux tar zxvf saphira linux 61 tgz For Windows95 NT or Macintosh similarly uncompress the ZIP or SIT archive respectively but the location of the files is up to you The recommended directory is c saph
10. 8 5 3 Micro task Manipulation 8 5 4 Invoking Behaviors 8 5 5 Activity Schema Instantiation 8 6 Local Perceptual Space 8 6 1 Sonar buffers 8 6 2 Occupancy functions 8 7 Artifacts 8 7 1 Points and Lines 8 7 2 Other Artifact Creation Functions 8 7 3 Geometry Functions 8 8 Sensor Interpretation 8 9 Drawing and Color Functions 8 10 Maps and Registration 8 10 1 Map File Format 8 10 2 Map Registration 8 10 3 Map Element Creation 8 11 File Loading Functions 8 12 Colbert Evaluator Functions 8 13 Packet Communication Functions 9 SAPHIRA VISION 9 1 Channel modes 9 2 Vision Packets 9 3 Sample Vision Application 10 PARAMETER FILES 10 1 Parameter File Types 10 2 Sample Parameter File 11 SAMPLE WORLD DESCRIPTION FILE Saphira Software Manual 90 90 90 91 91 92 93 93 95 96 97 98 98 100 100 101 102 102 103 103 103 105 107 108 109 110 112 112 113 113 115 116 116 116 117 121 121 121 122 125 125 125 129 Vii 12 SAPHIRA API REFERENCE 131 viii List of Tables Table 2 1 Keyboard enabled Saphira drive and behaviors Table 3 1 Example drive error tolerance values for a parameters file Table 5 2 Avoid collision behavior parameters Table 5 3 Stop collision behavior parameters Table 5 4 Keep off behavior parameters Table 5 5 Go to position behavior parameters Table 5 6 Attend at position behavior parameters Table 5 7 Follow lane be
11. CFLAGS g D CONFIG CC gcc INCLUDE IS INCD I X11D include Hat aH HH HE EE HE HH EH EE EEE EE E E E EE EE EE EE EE EE EE EE EE HH all S BIND btech BIND saphira BIND async BIND packet BIND nowin touch all OBJD saphira o SRCD saphira c CC CFLAGS c SRCD saphira c INCLUDE o OBJD saphira o BIND saphira OBJD saphira o ECE S OBJD saphira o o BIND saphira LS SAPHIRA handler obj lsf LS MOTIFD 1lib LLIBS le lm S OBJD testload o SRCD testload c INCD saphira h CC S CFLAGS c SRCD testload c S INCLUDE o OBUD testload o testload so OBJD testload o LD SHARED OBJD testload o o testload so The first part of the makefile defines variables that are useful in compilation and linking Note that the SAPHIRA environment variable must be defined as the top level of the Saphira distribution with no final slash The handler include directory contains header files and handler obj has the libraries Next the file handler include os h is read in This file determines the operating system type and sets some system library variables appropriately for X windows and Motif It also sets the CONFIG variable to the particular OS of the machine which is important for handling some of the system routines correctly For most OS s the Motif MOTIFD X11 X11D and system libraries LLIBS are set correctly but there may be cases i
12. an instance of the structure being made accessible here a pointer to it is passed to the function num is the number of members in the structure The rest of the arguments are triplets each defining one structure member The format of the triplets is char slotname amp s slot int type where slotname is the Colbert name for the member s slot is the instance structure member and type is the C index of the member type The available types are int sfINT i 1LO a lte sfFLOAT void sfVOID string sfSTRING ETE SPRACHEN ILI behavior sfBEHAVIOR vondi sfPTR In addition pointers to types can be defined with the function sfTypeRef int type For example to define a pointer to an integer use sfTypeRef sfINT The function sfTypeDeref performs the inverse operation giving the type of the reference of a pointer but this is less useful in defining argument types For example the sfRobot structure is defined as int sfSrobot rbot rs sfSrobot sfAddEvalStruct robot sizeof robot char amp r 22 x amp r x S FLOAT local coords y amp r y SfFLOAT th er th sfFLOAT ax amp r ax SfFLOAT global coords ay amp r ay SfFLOAT ath amp r ath sfFLOAT Control amp r control sfFLOAT heading control Status amp xr status sfINT status int here i sfAddEvalVar sfRobot sfSrobot fvalue amp sfRobot The structure name in Co
13. int found_blob int channel int delta sfRobotComStr VISION_COM line _bottom_row 0 sfRobotComStr VISION_COM pioneer_X_mode N void search_and_go_blob void void setup_vision_system void 95 95 96 96 112 112 105 105 105 98 98 98 98 97 98 99 99 122 122 122 121 121 123 122 Index activities defining 35 intend_beh 105 invoking behaviors 105 Activities window 21 activity 1 loading 12 activity files demo act 12 Activity language See Colbert ActivMedia Inc 1 API artifacts 109 Drawing and Color 114 See drawing and color Fuzzy variables 57 See fuzzy variables General See API maps 115 See maps Motor stall 99 OS functions 95 window mode See OS functions argument types 88 Artifacts 15 109 points and lines 110 See points and lines async sample client 81 Asynchronous routines 9 66 Attend at position parameters 61 autoconfiguration 91 AUTOEXEC BAT 22 Avoid collision parameters 59 bat 16 battery 16 Behavior executive 56 Behavior grammar 56 behavior beh 59 Behaviors sfInitBehavior 54 Behaviors Attend At Position 61 Avoid Collision 59 behavior beh 59 Constant Velocity 59 Description 11 Follow Corridor 62 Follow Door 62 Follow Lane 61 Go To Position 61 grammar 56 Saphira Software Manual implementing 58 init function 58 input parameters 58 keep off 19 60 Predefined 59 rules 58 schema 58 Stop Collision 60 Turn To 63 updat
14. lt Explicit type casting is not permitted although implicit casting is performed The for and switch statements are not defined Variables may not be initialized when defined No embedded assignments are allowed e g if x a gt 2 VVV VV V New functions are not defined in Colbert but may be imported from native C object files Only a few standard C library functions are initially available although others can be made available by telling the evaluator about them with sfAddEvalFn see below This and the other sfAddEval XXX functions are only available in C code so you must compile and load a shared object file to link in C library functions 41 4 42 Using Colbert Some of these limitations may be removed in future releases As Colbert provides for dynamic linking of C object files these restrictions aren t absolute native C functions can be loaded For example to reference an array you can define a C function that takes an offset and array as its argument and returns the array element 4 9 2 Comments Standard C comment syntax is used a comment 4 9 3 Keywords In addition to many of the ANSI C keywords Colbert defines several new keywords that cannot be used as variables labels or other names Here is a list of these names act behavior fail halt help iname interrupt load move noblock priority remove resume rotate speed string succeed suspend timeout trace turn tu
15. negative values are clockwise Blocking speed int mms Move the robot at a speed of mms millimeters per second forward positive or backwards negative Nonblocking rotate int degs Move the robot at a rotational speed of degs degrees per second counter clockwise positive or clockwise negative Nonblocking 45 4 46 Using Colbert halt Halts all direct motion commands 4 9 14 Activity and Behavior Invocation and Signaling Activities and behaviors are started with the start statement start lt aname gt c_exp noblock iname lt symbol gt priority lt int gt suspend timeout lt int gt The activity should be invoked with as many arguments as in its definition If there are no arguments then the argument parentheses may be omitted All of the optional keywords can occur in any order noblock causes execution to continue with the next statement after a single break without waiting for the activity to complete The activity can be given an instance name so that other activities can refer to it by default this name is its schema name If there is another activity or behavior with this instance name an error is signaled The priority keyword is only for behaviors which compete for control on the basis of their priorities A timeout specifies the maximum number of 100 ms cycles the activity or behavior will be allowed to execute If present suspend invokes the acitivity or behavior bu
16. point from the list of course sfFindArtifact returns the artifact on pointlist with identifier id if it exists else it returns NULL Finally sfDeleteArtifact removes an artifact from the list given its id point sfGlobalOrigin point sfRobotOrigin These are SYSTEM points representing the global origin 0 0 0 and the robot s current position 8 7 2 Other Artifact Creation Functions Walls corridors doors junctions and lanes can all be created with the following help functions These artifacts are important in defining maps for the robot point sfCreateLocalArtifact int type int id float x float y float th float width float length point sfCreateGlobalArtifact int type int id float x float y float th float width float length These two functions create and return artifacts of the specified type using either local or global coordinates These are the allowed types Table 8 4 Artifact creation types type return value s fCORRIDOR GC OnatakCl ames sfLANI lane sfDOO door sfJUNCI ACE LOMm sfWALL wall sfPOIN joOlme Although these functions are declared as returning type point in fact they return a pointer to the appropriate structure and the result should be cast as such All these structures are similar in their first several arguments i e local and global coordinates so they can all be used in the geometry manipulation functions Unlike the sfCreateXPoint functions th
17. sfOccBox 107 sfOccBoxRet 107 sfOccPlaneRet 108 occupancy 108 Open Agent Architecture OAA 6 10 12 OS functions sflsConnected 95 sfPause 95 OS functions display states 96 myButtonFn 97 myKeyFn 96 sfButtonProcFn 97 sfErrMessage 96 sfErrS Message 96 sfKeyProcFn 96 sfMessage 96 sfOnConnectFn 95 sfOnDisconnectFn 95 sfOnStartupFn 95 sfScreenToWorldX 97 sfScreenToWorldY 97 sfSetDisplayState 96 137 Index sfSMessage 96 sfStartup 95 os h 70 packet client example 79 packet communication 7 9 70 81 99 123 packet functions sfRobotCom2Bytes 121 packet functions port types and names 120 sfClientBytes 122 sfConnectToRobot 120 sfDisconnectFromRobot 121 sfHaveClientPacket 121 sfProcessClientPacket 121 sfReadClientByte 122 sfReadClientSint 122 sfReadClientString 122 sfReadClientUsint 122 sfReadClientWord 122 sfResetRobotVars 121 sfRobotCom 121 sfRobotComInt 121 sfRobotComStr 121 sfRobotComStrn 121 sfWaitClientPacket 121 packets checksum 86 data types 86 errors 87 protocols 85 Parameter File 127 parameter files 26 PCOMCLOSE 88 pioneer users See Customer resources Pkzip See Installation points and lines sfAdd2Angle 112 sfAddAngle 112 sfAddPoint 111 sfAddPointCheck 111 sfChangeVP 113 sfCreateGlobalPoint 110 sfCreateLocalPoint 110 sfFindArtifact 111 sfGlobalOrigin 111 sfMoveRobot 113 sfNorm2Angle 112 sfNorm3Angle 112 sfNormAngle 112 sfPointBaric
18. sfStartup initializes the Saphira OS If the client has been linked with the window libraries a user interface window is opened and Saphira information is displayed graphically If async is 0 Saphira has principal control of the client and thereafter calls other functions only from the Saphira multi tasking OS see below If async is 1 control returns immediately to the calling program and the Saphira interface runs as a separate thread The sfStartup function may be called at any time by your program but should be called only once Also include with the Windows version of this function the application instance handle hInst and the window visibility parameter cmdShow If the client program is running asynchronously in parallel with the Saphira OS then it may be useful to insert timing breaks in the client code The appropriate method is with sfPause which waits a specified number of milliseconds before continuing sfPause allows the Saphira OS to keep running during the break These functions are not available in Colbert void sfOnStartupFn void fn void sfOnConnectFn void fn void sfOnDisconnectFn void fn int sfIsConnected int sfIsExited These functions register callbacks for Saphira events when the Saphira OS first starts up when it connects to a robot and when it disconnects The functions are only used in standalone client code that calls sfStartup The variable sfIsConnected is also useful in Colbert a
19. use connect serial lt port gt start DoPackets The standalone client is similar but uses a micro task instead of the activity As in every standalone client the startup function is registered and then the sfStartup function is invoked to initiate the Saphira OS In the startup function the display state is changed to show global movement of the robot and the task myTask is instantiated Then the two default Saphira micro tasks that handle packets and motor control are removed so that the user task can perform these functions Finally the sfConnect ToRobot function is called to connect the client to the robot server The myTask micro task waits until the robot is connected then opens the motor controller and tells it to move forward at 300 millimeters per second Execution now proceeds as in the packet act activity the only difference is that the micro task must explicitly sequence its operations by changing state After the packets are received the task stops the robot and disconnects from the server include saphira h void myStartupFn void A orwand rers a void myTask void WOW Wein linc ere Chew ESN iliac aL Qs sfOnStartupFn myStartupFn register 2 StartUp iebiaeicakoin A Seo uae U ONONE start up the Saphira window wait void myStartupFn void fSetDisplayState sfGLOBAL TRUE use the global view fFInitProcess myTask myPackets fFRemoveTask packets Moer ea
20. 0 if successful 1 if the file cannot be found Any map file errors are reported in the message window but note that only the last one will be displayed long enough to be read If the argument to the map file functions is a relative directory path e g maps mymap then Saphira will use the map directory sfMapDir as a prefix for this path By default sfMapDir is set to the directory maps in the top level of the Saphira distribution Loaded artifacts are added to any map artifacts already in the system To delete all map artifacts use the sfDeleteMapArtifacts function Individual artifacts can be deleted using their id number see Section 8 7 The current client map can be saved to a file using sf SaveMapFile The saved file is in map file format so it can be read in with sfLoadMapFile When using the simulator with Saphira clients that have maps it is useful to have the simulated world correspond to the map Unfortunately the format of simulator world files is different from map files and currently there is no utility to convert map files into simulator world files they must be created by hand A simulator world file can be loaded into the simulator either by the menu commands in the simulator or by the sfLoadWor1ldFile command issued from a client connected to the simulator 8 10 2 Map Registration As the robot moves its dead reckoned position will accumulate errors To eliminate these errors a registration routine attempts to mat
21. 1 8 2 Saphira Newsgroup 1 8 3 SRI Saphira Web Pages 1 8 4 Support 1 8 5 Acknowledgments 2 SAPHIRA SYSTEM OVERVIEW 2 1 System Architecture 2 1 1 Micro tasking OS 2 1 2 User Routines 2 1 3 Packet Communications 2 1 4 State Reflector 2 2 Saphira Control Architecture 2 2 1 Representation of Space 2 2 2 Direct Motion Control 2 2 3 Behaviors and Fuzzy Control 2 2 4 Activities and Colbert 2 2 5 Sensor Interpretation Routines 2 2 6 Registration and Maps 2 2 7 Graphics Display 2 3 Running the Sample Client 2 3 1 Loading an Activity File 2 3 2 Connecting to a Robot 2 3 3 LPS Display 2 3 4 Information Area 2 3 5 Text Interaction Area 2 3 6 Menus 2 3 7 Keyboard Actions 2 3 8 Behaviors Window 2 3 9 Processes Window 2 3 10 Activities Window 2 3 11 System Environment Variables 3 THE SIMULATOR 3 1 Starting the Simulator ADDNUNUHNRNNNE Se Saphira Software Manual 3 1 1 Listening on Other Ports 25 3 2 Parameter File 26 3 3 World Description File 27 3 4 Simulator Menus 27 3 4 1 Load Files Menu 27 3 4 2 Connect Menu 27 3 4 3 Display Menu Grow Shrink and Wake 27 3 4 4 Recenter Menu 28 3 4 5 Exit Menu 28 3 4 6 Information Area 28 3 5 Mouse Actions 28 4 USING COLBERT 29 4 1 A Colbert Example 29 4 2 Evaluator Interaction Area 31 4 3 Evaluator Help 31 4 4 Syntax Errors 31 4 5 Evaluator File Loading 32 4 5 1 Loading Shared Object Files 32 4 5 2 Load Directory 33 4 5 3
22. 180 to 180 sfPointDist returns the distance from the point to the robot sfPointNormalDist returns the distance from the robot to the line represented by the artifact point The second three functions compute properties of points sfPointDistPoint returns the distance between its arguments sfPointNormalDistPoint returns the distance from point q to the line represented by artifact point p sfPointBaricenter sets point p3 to be the point midway between point p1 and p2 void sfChangeVP point pl point p2 point p3 void sfUnchangeVP point pl point p2 point p3 float sfPointXo point p float sfPointYo point p float sfPointXoPoint point p point q float sfPointYoPoint point p point q void sfPointMove point pl1 float dx float dy point p2 void sfMoveRobot float dx float dy float dth These functions transform between coordinate systems Since each point artifact represents a coordinate system it is often convenient to know the coordinates of one point in another s system All functions that transform points operate on the local coordinates if you want to update the global coordinates as well use sfSetGlobalCoords sfChangeVP takes a point p2 defined in the LPS and sets the local coordinates of p3 to be p2 s position in the coordinate system of p1 sfUnchangeVP does the inverse that is takes a point p2 defined in the coordinate system of p1 and sets the local coordinates of p3 to be p2 s position in
23. 2 2 Saphira Client Command Support Saphira fully supports client commands with useful library functions Prototypes can be found in handler include saphira h and saphira pro See Chapters 5 and 6 for details 86 Saphira Software Manual Table 7 4 PSOS 4 2 supported client commands Name Number Value s sfsynco f ol none Start connection server echoes these synchronization commands back to client ae S reg secomputse of none Communication pulse za none Set server origin l 7 EL 11 signed int Forward or reverse velocity mm sec s fCOMHEAD 12 unsigned int Turn to absolute heading 0 360 degrees degrees sfCOMDHEAD 13 signedint Turn heading 255 degrees degrees EL 21 signed int Set rotational velocity 255 degrees sec degrees sec s COMVEL2 32 2 bytes Set wheel velocities independently 4 mm sec 4mm sec sfCOMDIGOUT integer Set digital output bits bits 0 7 4 sfCOMTIMER integer Initiate user input timer triggering an event pin 0 7 with specified pin MGRIPP I sfCO ER integer Sets gripper state 0 1 4 5 sfCOMPTUPOS ka integer Set pulse width for position servo control 1 2000 ms sfCOMSTEP 64 none Single step mode simulator only 30 31 33 1 64 7 3 Server Information Packets The Saphira aware server automatically sends a packet of information over the communication port back to the client every 100 milliseconds The s
24. 2 3 4 5 Miscellaneous Bat CPU Scrn The battery Bat voltage level on the server indicates when the robot needs to be recharged The CPU utilization is the percentage of total processing time used by the client On UNIX machines this does not include CPU time used by the X server which can be an appreciable fraction of total CPU time The last value is the LPS update rate 2 3 5 Text Interaction Area The interaction area is at the bottom of the window Here Saphira prints information about the system and the user can type commands to the Colbert evaluator A scrollbar allows the user to look at previous information The small square on the far upper right of the window is a dragging handle for resizing the interaction area In the interaction area you can do the following tasks e Load activity files and change the working directory e Connect and disconnect from a robot server e Define start and stop activities e Trace and untrace activities e Get help on API and evaluator functions e Examine and set internal Saphira variables The evaluator lets users write and debug programs from the running Saphira application Typically the user code will be in a text file that is read into the system with the Load command as we did for this example colbert demo act The code file contains a mixture of activity schema definitions and calls to library functions The user can invoke the activities from the interaction area with the start comm
25. 33 set server 33 speed 34 turn 34 turnto 34 COMDHEAD 88 COMDIGOUT 88 Communication packets 85 See packets communications rate 89 COMOPEN 88 COMORIGIN 88 compiling clients 66 136 MS Visual C 69 system requirements 65 Unix clients 68 COMPOLLING 88 Components Optional 2 COMPTUPOS 88 COMPULSE 88 COMSETO 88 COMSTEP 88 COMTIMER 88 COMVEL 88 config h 70 configuration 92 connect 17 33 connect menu connect 17 disconnect 17 27 Connecting 12 CPU 16 Customer resources FTP 5 Newsgroups 5 Support 6 data types 86 delete map 17 demo act 12 direct client example 77 direct motion 29 34 Direct motion control 11 56 100 disconnect 17 27 33 display states 96 display menu 17 local 17 occ grid 17 single step 17 wake 17 display states 96 down_straight 57 draw_blobs 124 drawing and color set_vector_buffer 114 sfDrawCenteredRect 114 sfDrawRect 114 sfSetLineColor 114 sfSetLineType 114 sfSetLineWidth 114 sfSetPatchColor 114 environment variable LD_LIBRARY_PATH 4 SAPHIRA_LOAD 12 environment variables 22 SAPHIRA 22 SAPHIRA _COMPIPE 22 SAPHIRA_COMSERIAL 22 SAPHIRA _COMSERVER 22 SAPHIRA_LOAD 22 33 SAPHIRA_SERIALBAUD 22 setting 22 errors 87 exit 33 exit menu 28 f_and 58 f_eq 57 f_greater 57 f_not 58 f_or 58 f_smaller 57 Fast Track Vision System 123 files menu delete map 17 save map 17 find_blob 124 Follow corridor parameters
26. 62 Follow door parameters 62 Follow lane parameters 61 found_blob 124 FTP See Customer resources functions menu 17 Fuzzy variables 57 combination functions 57 down_straight 57 f_eq 57 f_greater 57 f_or 58 f_smaller 57 straight_up 57 Go to position parameters 61 grow 17 27 Gzip See Installation halt 34 help facility 31 information area 28 information packet 89 init act 12 Installation 2 intend_beh 105 interaction area 16 31 Keep off behavior 19 Keep off parameters 60 keyboard 18 76 keyboard actions 76 Konolige Dr Kurt 1 LD_LIBRARY_PATH environment variable 4 load directory 12 32 33 load menu load param file 27 load world file 27 load param file 27 load world file 27 Loading files 32 Saphira Software Manual Colbert files 29 load directory 33 object files 33 local 17 Local Perceptual Space 105 106 LPS 105 See Local Perceptual Space main window 13 majordomo See Customer resources manual drive 18 maps file format 115 registration and creation 118 sfLoadMapFile 117 Menus See also Client Saphira client 16 Simulator 27 micro tasking OS 7 8 66 67 micro tasks 7 8 9 10 12 20 21 66 67 70 73 75 81 98 102 See processes motion setpoint 11 100 Motor stall 99 sfStalledMotor 100 mouse 76 mouse actions 76 move 34 MPac 16 MS Visual C 69 myButtonFn 97 myKeyFn 96 Newton Labs Inc 123 nowin example client 83 occ grid 17 occupancy
27. Corridor artifacts display with double dotted lines doors display with double solid lines walls display as single solid lines junctions as pairs of solid lines Numbers are the artifact ID s For illustration the defining vector for each artifact is shown Note that a map represents artificial structures in the Saphira client in the same way that latitude and longitude lines are artifacts in global maps and are not found on the earth s surface The robot or simulator will not pay attention to these lines since they are internal to the client This can be a useful feature For example a corridor is conceptually a straight path through an office environment even where it has door openings or junctions with other corridors you can imagine the corridor walls as extended through these areas The robot can still go through the artifact corridor sides at these points The registration micro 114 Saphira Software Manual tasks described below use the map artifacts as registration markers matching sensor data from the sonars against this internal model to keep the robot registered on the map Obstacles within corridors such as water coolers or boxes can be represented using wall structures such as the one in corridor 2 int sfLoadMapFile char name int sfSaveMapFile char name char sfMapDir int sfDeleteMapArtifacts void int sfLoadWorldFile char name The sfLoadMapFile function loads a map file name into Saphira Returns
28. Once started an act may be signaled by other acts by itself or by the user through the interaction area or the activities window Sending an act a signal causes it to go into a special state For example a suspending act or behavior can be restarted by sending ita resume signal We can illustrate the utility of signals with a monitoring example Suppose we want to program the robot to patrol until it sees some object in front then it should stop patrolling and approach the object To accomplish this task we ll set up two activities the patrol activity of the previous example and a supervisory activity that checks if there is something in front of the robot and if so approaches it Figure 4 4 act approach AE 3 8 start patrol 1 timeout 300 noblock checking if sfGetTaskState patrol sfTIMEOUT sfiIsStalled eurie x sfObjInFront if x gt 2000 goto checking suspend patrol move x 200 succeed Figure 4 4 An activity that monitors another This activity starts off by invoking patrol with a negative argument so it continues indefinitely However instead of causing the approach to wait for its completion the patrol activity is invoked with two special parameters The first timeout 300 causes patrol to quit after 30 seconds 300 cycles have elapsed The second noblock allows the execution of approach to continue in parallel with patrol The former now goes into a monitoring loop in w
29. SEN y SENER A sfAddEvalConst SfFollowCorridor sfBEHAVIOR sfFollowCorridor sfAddEvalConst SfLEFT SENT 0 sfAddEvalVar SsfCurrentEnvironment sfPTR fvalue amp sfCurrentEnvironment ind_mystruct sfAddEvalStruct mystruct sizeof struct mysce Chan tam 3 Smaa SEENI Uaw Wig Gmo SHE IOI WE aie Sele y EvalVar m ind_mystruct fvalue amp m 47 4 48 Using Colbert The function sfLoadInit if present is invoked when the object file is loaded into Saphira In this case it prints a message then makes a structure a variable a function a behavior a constant and another variable visible to the Colbert Details on how to make C functions and variables available in Colbert are contained in the next few sections 4 10 2 Making Native C Functions Accessible Native C functions including Saphira library functions are made accessible in Colbert with the sfAddEvalFn function sfAddEvalFn is not callable from Colbert since Colbert has no way to access the underlying C environment It must always be compiled and loaded from a shared object file usually as a call in the sfLoadInit function see the example in the previous section The format of sfAddEvalFn is sfAddEvalFn char name void fn int rtype int nargs name is the name of the function as seen by Colbert fn is a function pointer to the C function being mad
30. SYNCO 91 SYNC1 88 SYNCI 91 SYNC2 88 SYNC2 91 syntax errors 31 turn 34 turnto 34 63 Unix clients 68 user process 75 sample 75 ver53 3 See also Installation Vision 123 Saphira Software Manual channel modes 123 See Vision chroma h 123 draw_blobs 124 find_blob 124 found_blob 124 packets 123 sample application 124 search_and_go_blob 125 setup_vision_system 124 sfRobotComStr 123 Vision packets 123 VPac 16 wake 17 27 World Description File 131 World files 27 Zip See Installation 141 Saphira Software Manual Warranty amp Liabilities The developers and marketers of Saphira software shall bear no liabilities for operation and use with any robot or any accompanying software except that covered by the warranty and period The developers and marketers shall not be held responsible for any injury to persons or property involving the Saphira software in any way They shall bear no responsibilities or liabilities for any operation or application of the software or for support of any of those activities And under no circum stances will the developers marketers or manufacturers of Saphira take responsibility for or support any special or custom modification to the software Saphira Software Manual Version 5 3 January 1997 143
31. Saphira s Function Activities menu see previous chapter All of the behaviors used in this function are available as part of the Saphira library 73 6 Creating Clients and Load Files AOL ts 1S lee Oita aos NOTA O Ss Gi seCh switch process_state Case JNILICg sfPreferredTurnDir sfLEFTTURN sfStartBehavior sfConstantVelocity 0 3 0 300 f sfStartBehavior sfStop 0 4 0 SeStateBehavwelore is hAviOnG Colslsnsus ni PORO 3 0 Eront sensitivity 30 side sensitivity SES HAREAMIY yy E pstakia lt gt LOOO 7 Sitamcloire sfStartBehavior sfKeepOff 0 1 0 100 0 caution speed 0225 sensitivity process_state SUSPEND break case RESUME sfMessage Resumed break 6 3 3 The testload so Loadable Object File Example Native C code can be loaded into Colbert and executed by compiling the code and linking it to create a shared object file in UNIX or a dynamic load library in MS Windows The sample file test load c contains Saphira library and user function calls in C source As in the standalone client examples the header file saphira h must be included at the beginning of the source file The rest of the file contains C function variable and structure definitions The difference loadable objects and a standalone client is that there is no main function instead the sfLoadInit function is called after loading the file and it typically makes the objects in
32. Saphira function tries to open a communications channel to the robot server on port type port with name name Returns 1 if successful 0 if not This function is also available as the connect command in Colbert Table 8 2 Port types and names for server connections sfLOCALPORT connect to simulator on the host machine ara sfTTYPORT connect to Pioneer on a tty port 117 8 Saphira API Guide sfCOMLOCAL local pipe or mailslot name sfCOM1 tty port 1 dev ttya or dev cua0 for UNIX COM1 for MSW modem for Mac sf COM2 tty port 2 dev ttyb or dev cual for UNIX COM2 for MSW printer for Mac This function also sets the global variables sfRobotName sfRobotClass and sfRobot Subclass according to the information returned from the robot see the table below Assuming the environment variable SAPHIRA is set correctly it will autoload the correct parameter file from the params directory using first the subclass if it exists and then the class Table 8 3 Robot names and classes char sfRobotName See robot descriptions for information on how to set the name The simulator returns the name of the machine it is running on char sfRobotClass Robot classes are B14 B21 and Pioneer char sfRobotSubclass Subclasses are subtypes e g in Pioneer class robots the subclass is the OS type currently PSOS41 void sfDisconnectFromRobot void Sends the server a close command then shuts down the communications channel to the ser
33. a corridor e sfCurrentEnvironment sfCreateLocalPoint 10000 0 0 point ahead of robot sfAddPoint p start sfFollowCorridor e p priority 2 iname follow noblock He tollon Corridor to jxosinie y waitfor sfCurrentEnvironment e sfTaskFinished follow remove follow remove this behavior goto NOCORRIDOR vesume checking for corridor oninterrupt remove follow suspend start FindAndFollow suspend The BumpAndGo schema uses direct actions rather than behaviors to rescue the robot from stall situations The update facility of activity schemas is used to calculate a stalled variable on each cycle The act allows behaviors to control the robot until it detects a stall condition then it turns off behavior execution and starts issuing direct action commands In the file the BumpAndGo activity is initiated in the active state his activity detects bump collisions on Pioneer BumpAndGo in stalled 7 static local variables in recovering update f Och xecuted on every cycle stalled sfStalledMotor 0 sfStalledMotor 1 NOCONTACT unt sfBehaviorControl ig 7 gt behaviors Cin waitfor stalled sfBehaviorControl 0 behaviors on c t BumpAndGo fe Siceise de vio The standalone client version demo c has most of the same functions However because Colbert activities are only available in the evaluator th
34. a side sonar actually sees a surface hence their valid flag is always set Thus readings stay in the side buffers for longer periods of time and Saphira has a chance to figure out what the features are As the robot moves all the entries in the circular buffers are updated to reflect the robot s motion i e the sonar readings stay registered with respect to the robot s movements 104 Saphira Software Manual define CBUF_L typedef struct je Cirera loyeiriceuesi 7 mestan fe imeenea OEE DOLEE int end internal buffer pointer iag Isimnlie 7 current butfer size float xbuf CBUF_LE float ybuf CBUF_LE int valid CBUF_LEN fe aan to il ror validi Giainy es DU COURS TAWE DUR ee lobar Gil _lowuep These buffers are not currently available in Colbert void sfSetFrontBuffer int n void sfSetSideBuffer int n float sfFrontMaxRange The first two functions when given an argument greater than zero set the front and side buffer limits to that argument respectively If given an argument of zero they clear their buffers that is set the valid flags to 0 These buffer limits can also be set from the parameter file and are initialized for a particular robot on connection sfFrontMaxRange is the maximum range at which a front sonar reading is considered valid It is initially set to 2500 2 5 meters Setting this range higher will make the obstacle avoidance routines more sensitiv
35. are not declared in the Saphira headers so they must be declared in the application code 5 7 5 Behavior schema A complete behavior schema is a structure combining its rules init and update functions The rules can be included directly in the definition here is the example constant velocity function extern int Accel Decel Turn_left Turn_right behavior constant_velocity Constant Vel cv_setup cv_check_speed 1 2 nS DCCC Um CCV AEO ORS EOW amp Accel amp cv_speedup T Uslov Dorat sev COO fast amp Decel amp cv_slowdown The first argument is the name of the behavior the second is the init function the third is the update function and the fourth argument is the number of parameters The number of rules is the fifth argument 58 Saphira Software Manual and the rules themselves are the sixth Note that all global variables are referenced as pointers in the behavior The maximum number of rules in a behavior is 10 The consequent values Accel and so on must be declared as external integers 5 8 Pre Defined Saphira Behaviors Saphira has a number of pre defined behaviors for obstacle avoidance and goal directed movement Most of the complexity of these behaviors is in the update functions which extract data from the LPS and update a small set of fuzzy variables relevant to the behavior Besides integrating these behaviors with your own routines you can use them as templates to create new behaviors The exa
36. artifact relates to its use by the LPS Currently Saphira supports three categories system for artifacts with an internal function percept for artifacts representing hypothesized objects extracted from sensor input and artifact for user created artifacts such as map information and goal artifacts typedef enum SLoleM PERCEPT ARITEACT cat_type typedef enum INVALID POS WALL CORRIDOR LANE DOOR JUNCTION OFFICE EAK OBJ pt_type The point type consists of a directed point position and direction with an identifier a type a category and other parameters used by the system X Y coordinates are in millimeters and direction is in degrees from 180 to 180 The type POS is used for goal positions in behaviors Other types may add additional fields to the basic point type e g length and width for corridors typedef struct IPILOSNE Sp We elas fe So yo Tela jeXeySalicakojin Ope jeyo slioe iceulencatys ro ieeloyoe lt 7 pt_type type fe tye OE OLME 3 Cat_type Cat category boolean snew whether we just found it boolean viewable whether it s valid alone all f Un quesaumMers tems y lOs Ebx yp ny la elaolbal coords s unsigned int matched last time we matched unsigned int announced last time we announced J jooaliane 5 The orientation of a point is useful when defining various behaviors For example a doorway is represented
37. by a point at its center a width and a direction indicating which way is into the corridor point sfCreateLocalPoint float x float y float th point sfCreateGlobalPoint float x float y float th void sfSetLocalCoords point p void sfSetGlobalCoords point p The first two functions create new ARTIFACT points of type POS based on the supplied coordinates For example sfCreateLocalPoint 1000 0 0 0 0 0 creates a point meter in front of the robot Very useful for behavior goal positions The second two functions reset the local or global coordinates from the other set based on the robots current position These functions are useful after making a change in one set of coordinates In order to keep a point s local coordinates updated within the LPS it must be added to the pointlist after itis created The pointlist is a list of artifacts that Saphira updates when the robot moves The following functions add and delete members of the pointlist 108 Saphira Software Manual void sfAddPoint point p void sfAddPointCheck point p void sfRemPoint point p point sfFindArtifact int id void sfDeleteArtifact int id Normally to add a point to the pointlist use sfAddPointCheck which first checks to make sure point p is not in the list already before adding it It is not a good idea to have two copies of a pointer to a point in the pointlist because its position will get updated twice The sfRemPoint function removes a
38. commands and gathering sensory information from the robot The state reflector is a set of data structures in the client that reflects the sensor and motor state of the robot The client can examine sensor information by looking at the reflector data and can control the robot by setting reflector control values It is the responsibility of the Saphira OS to maintain the state reflector by communicating with the robot server receiving information packets and parsing them into the state reflector and sending command packets to implement the state reflector control values The micro tasks started by sf InitBasicProcs are the relevant ones you must invoke this function for the state reflector to function There are three important data structures in the state reflector 1 The sfRobot structure holds motion and position integration information as well as some sensor readings motor stall sensors digital I O ports 2 The sonar buffers hold information about current and past sonar returns 3 The control structures command robot motions This section describes the robot and sonar information structures the next one the direct motion commands that affect the control structures 96 Saphira Software Manual struct robot sfRobot The sfRobot variable holds basic information reflected from the robot server Table 8 2 below shows the values of the various fields in this structure the definition is in handler include struct h All of the values in t
39. current range readings of all the sonars can be found in the sonar bucket structures see the section on the state reflector above As the robot moves these readings are accumulated in the LPS in three internal buffers These buffers are available to user programs and are also used by the obstacle finding functions in the next subsection The reading values are placed on the centerline of the sonar at the range that the sonar indicates Saphira s display routines draw sonar readings as small open rectangles and if the robot moves about enough they give a good picture of the world There three buffers are the front and two side buffers left and right Each buffer is a cbuf structure defined below Client programs unless they are interested in the temporal sequence of sonar readings can just treat these buffers as linear structures with size limit The buffer size can be changed using the functions defined below The reason for having different buffers is that they satisfy different needs of the robot control software The front sonars pointed in the direction of the robot s travel warn when obstacles are approaching But the spatial definition of these sonars isn t very good and its almost impossible to distinguish the shape of the obstacle for recognition purposes A wall in front of the robot for example will only look a little bit like a straight line see the excellent book by Leonard and Durant Whyte 103 8 Saphira API Guide
40. direct motion routines to move the robot back and forth between two points The patrol routine is a Saphira micro task gt packet act c This client bypasses the state reflector of Saphira providing its own packet communication handler gt async c This client uses the state reflector and direct motion routines but instead of invoking a micro task it calls the motion routines asynchronously gt nowin c Like the previous client this one calls the motion commands asynchronously but ignores the user interface routines and connects to the robot directly 6 3 1 The Basic Saphira Client The basic client bin saphira is used as the typical development environment It starts up basic micro tasks for communication and control It also starts the Colbert evaluator for user interaction which loads the Colbert file init act from the working directory if it exists Like all Saphira C source files this example starts with a header file that reads in all prototype and structure information for the Saphira libraries The headers can be read by C or C programs all library names are C names The file handler include saphira h automatically configures the C compiler for the operating system you re running on UNIX SGI Solaris Linux FreeBSD or MS Windows 95 NT If you need to customize these files for example if you have the Motif libraries in a different place than Saphira assumes then look in handler include os h and the various configuratio
41. immediately in front of the robot is the angular setpoint 2 3 3 6 Artifacts Artifacts are internal representations of external objects or imaginary constructions such as goal positions Figure 2 3 shows a corridor artifact long double lines and a doorway labeled door 2 2 3 4 Information Area The information area is at the left of the main window It contains four sets of data returned from the robot server 2 3 4 1 Status St Shows the robot server status as moving stopped or no servo when the motors are stuck 15 2 Saphira System Overview 2 3 4 2 Velocity Tr Rot The robots translational Tr velocity in millimeters per second and rotational Rot velocity in degrees per second 2 3 4 3 Position X Y Th Absolute robot position in millimeters and degrees Note that this is not the server dead reckoned position which has accumulated errors Instead it is the registered global position of the robot based on Saphira s map registration routines operating in conjunction with position integration returned from the server 2 3 4 4 Communication MPac SPac VPac The communication values in the information area are the number of packets of the given type received in the last second They are useful for checking the communication link with the server Normally a client will receive ten motor packets Mpac and approximately 25 sonar packets SPac per second Vision packets Vpac currently are not supported
42. in the interaction window Colbert files can contain functions to evaluate at the top level of the file On load the file starts off by invoking several sets of pre defined micro tasks for behavior control sfInitControlProcs registration of the robot to a map sfInitRegistrationProcs sensor interpretation and object recognition sfInitInterpretationProcs and an environment tracking procedure sfInitAwareProcs These library functions are all accessible in Colbert and are invoked as the file is read The second set of statements initializes a variable and then starts up four behaviors for obstacle avoidance and movement The movement behaviors are invoked in a suspended state so that they won t cause the robot to move until they re resumed from the Activities menu or with the resume command demo act Demonstration of behaviors and activities using the Colbert evaluator A Sie Limite Come oND Toce s for behavior control sfInitRegistrationProcs register robot using sensed artifacts i sfInitInterpretationProcs j find walls and doors sfInitAwareProcs figure out where we are Start up some behaviors sfPreferredTurnDir sfLEFTTURN Sirsict SiwNVOUCICOLIASLOM S Bp SUESRUNRPIN WOO jormorriy O7 grenit SEKSECO LOO 25y SPW NRN joieioreaey7 ip start sfConstantVelocity 200 priority 2 suspend srar a SAESiECO jori mincy SUSPEN The second part of the file defines two acti
43. in Colbert for referring to the state of an activity rather the library functions sfGetTaskState sfTaskFinished and sfTaskSuspended are used r int sfGetTaskState char iname int sfTaskFinished char iname int sfTaskSuspended char iname sfGetTaskState returns the state of the micro task whose instance name is iname This micro task may be an activity behavior or simple micro task If there is no such micro task the result is sfINACTIVE Note that the instance name is a string since sfGetTaskState is a C function see Figure 4 4 for an example States that are less than or equal to 10 are special states initial suspended finished or interrupted states A micro task that has completed its activity will be in one of the finished states succeeded failed or timed out The function sfTaskFinished returns 1 if the micro task is in one of these states and 2 if it is not present or is in the state s fFREMOVE If the micro task is present and not finished then sfTaskFinished returns 0 A suspended micro task has the state sf SUSPEND if it is suspended indefinitely or a negative integer if it is suspended for a number of cycles The function sfTaskSuspended returns 1 if a micro task is in a suspended state and 0 otherwise 4 8 5 Hierarchical Invocation Like other micro tasks acts can run concurrently accomplishing different goals for the robot In the previous section there was an example of a monito
44. of the behavior is pri A timeout tout must be specified a timeout of 0 is indefinite execution The suspend argument is 0 if the behavior is to be active immediately and 1 if it is to be started in a suspended state to be activated by a resume signal The remainder of the arguments to sfStartBehavior are the arguments to the behavior There must be exactly the same number and types of arguments as are specified by the behavior parameters This function is equivalent to start b iname in timeout tout priority pri suspend where b is the name of the behavior schema 8 5 5 Activity Schema Instantiation An activity schema can only be instantiated from another Colbert activity or the user interaction area with the start command Section 4 8 3 8 6 Local Perceptual Space Local Perceptual Space LPS is a geometric representation of the robot and its immediate environment Unlike the internal coordinate system we described earlier in Chapter 4 which represents the dead reckoned position of the robot server the LPS is an egocentric coordinate space that remains clamped to the robot center Figure 8 1 Units in the LPS are millimeters and degrees For example the position of a point artifact in the LPS is represented by an X and Y coordinate in mm and an angle relative to the X axis in degrees NOTE starting with version 6 1 all internal and user angles are specified in degrees rather than radians 8 6 1 Sonar buffers The
45. on one of the serial ports if the appropriate port name is selected from the menu In this case the simulator and client can run on different machines The Disconnect item causes an immediate disconnect of the simulator from its connected client Normally the simulator will disconnect automatically when the client sends it the sfCLOSE command In situations where the client has a system error and exits abnormally the client may remain connected even though the connection is no longer valid In this case the Disconnect item will force the connection to close so the simulator can go back to a listening state With the Windows95 NT version an Exit option also is in the Connect menu 3 4 3 Display Menu Grow Shrink and Wake The Grow and Shrink menus or items in the simulator s Display menu change the size of the display The Wake item if on causes a the simulator to display a breadcrumb of the last few seconds of simulated robot travel 27 3 28 The Simulator 3 4 4 Recenter Menu Selecting the Recenter menu item centers the display around the current robot position It does not change the robot s position Normally the simulator will keep the robot icon near the center of the display by moving the display window when the robot approaches an edge 3 4 5 Exit Menu The Exit menu or item in Connect menu terminates the simulator A connected simulator should be disconnected first from the client side or it will
46. parameters described in Table 5 8 This behavior sets the sfPreferredTurnDir variable depending on how the robot is misaligned with the corridor 61 5 62 Behavioral Control Table 5 8 Follow corridor behavior parameters Effect sfPTR Corridor This is a corridor artifact the robot is to follow The path of the robot is bounded by a lane set in from the sides of the corridor sfPTR Goal position The robot moves along the corridor in the direction of the goal until it reaches it This should be a pointer to a point artifact behavior sfFollowDoor Tells the robot to go in a doorway as represented by a door artifact The direction is whether to go in or out of the doorway this could be decided automatically by the position of the robot but isn t because the robot may already be on the correct side When active the behavior draws its lane as a set of dotted lines in the LPS This behavior takes two parameters described in Table 5 9 This behavior sets the sfPreferredTurnDir variable depending on how the robot is misaligned with the lane through the doorway Table 5 9 Follow door behavior parameters Effect sfPTR Door This is a door artifact the robot is to go in or out of The path of the robot is bounded by a narrow lane perpendicular to the door sfINT Direction s FIN or s fOUT In means into the room out means out of the room and into the corridor behavior sfTurnTo Turns the robot to
47. process state values sfREMOVE SEREMOVE Requests the scheduler to remove this micro task Process cycle time is 100 milliseconds On every cycle Saphira calls each micro task with process_state set to the current value for that micro task The micro task may change its state by resetting process_state A micro task may suspend itself by setting the state to sf SUSPEND Some other micro task or your program must resume a suspended micro task see below for relevant functions A micro task may also suspend itself for n cycles by setting process_state to n in which case it will resume after the allotted time with state sf RESUME 100 Saphira Software Manual x The sf INTERRUPT state indicates an interrupt request from another micro task or the user Micro tasks should be written to respond to interrupts by saving needed information then suspending until receipt of a resume request Many of Saphira s predefined micro tasks are written in this way The sfSUCCESS and sfFAILURE states are used to indicate the successful or unsuccessful completion of a micro task The micro task may set these as appropriate or signal other micro tasks to set them No further processing takes place unless the micro task is resumed Simple micro tasks do not have timeouts but activities and behaviors do In these cases a state of sfTIMEOUT means that the micro task has timed out of its allott
48. programming for mobile robotics Saphira and Flakey appeared in the October 1994 show Scientific American Frontiers with Alan Alda Saphira and the Pioneer robots placed first in the AAAI robot competition Call a meeting in August 1996 which also appeared in an April 1997 segment of the same program The Saphira system can be thought of as two architectures built one on top of the other The system architecture is an integrated set of routines for communicating with and controlling a robot from a host computer The system architecture is designed to make it easy to define robot applications by linking in client programs Because of this the system architecture is an open architecture Users who wish to write their own robot control systems but don t want to worry about the intricacies of hardware control and communication can take advantage of the micro tasking and state reflection properties of the system architecture to bootstrap their applications For example a user who is interested in developing a novel neural network control system might work at this level On top of the system routines is a robot control architecture that is a design for controlling mobile robots that addresses many of the problems involved in navigation from low level control of motors and sensors to high level issues such as planning and object recognition Saphira s control architecture contains a rich set of representations and routines for processing sens
49. recompile reload re execute cycle Colbert sources and shared object files are also much easier to distribute and share than C source for clients So the interactive method is the one we recommend for most development tasks For mature applications it may be useful to create a new client with all user functions pre loaded It is also possible to use the Saphira system from other languages such as LISP or PROLOG as long as they have a foreign function interface facility In this case the developer writes some routines in C or C that are compiled into object files and then these object files together with the Saphira libraries are loaded into the LISP or PROLOG system C or C programs can be compiled into object files using standard compilers such as gcc or MS Visual C The header files in handler include contain prototypes and definitions of structures and variables in the Saphira library After compiling his or her files the developer links them with the Saphira library to create either a shared object file or an executable client Shared object files are loaded into Colbert and clients are standalone systems for controlling the robot User clients may also invoke the Colbert evaluator for instance the sample client bin saphira calls the evaluator as a micro task The next chapter contains details of the Saphira API which should be used as a reference guide to the Saphira libraries In addition to the Saphira API the best reference ma
50. string PSOS version for Pioneer class robots and their simulator Null string for other robots and simulators The parameter file that is appropriate for a robot can be found in the Saphira params directory The name of the parameter file will be the same as the lowercase version of the subclass string if it exists or the class string 7 4 3 Opening the Servers s COMOPEN Once the communication link is established the client should then send the sfCOMOPEN command which causes the robot or the simulator to perform some housekeeping functions start the sonar and motor controllers among other things start listening for client commands and to begin transmitting server information 7 4 4 Keeping the Beat sf COMPULSE As mentioned earlier a server safety watchdog expects that the robot receives at least one communication packet from the client every two seconds Otherwise it assumes the client server connection is broken and shuts down the robot s motors If your client application will be otherwise distracted for some time periodically issue the sfCOMPULSE client command to let the server know you are indeed alive and well If the robot shuts down due to lack of communications traffic it will revive upon receipt of a client command and automatically accelerate to the last specified speed at the current heading 7 4 5 Closing the Connection s COMCLOSE To close a connection and reset the server to the wait state si
51. than compile time All clients share a copy of the library take up less space and are quicker to compile Saphira applications need to be able to find these libraries e Under UNIX the distribution contains the file handler obj libsf so 6 1 x There are two ways to make the library accessible to an application We recommend leaving it in this directory and putting the directory name onto the load library list using the shell command export LD_LIBRARY_PATH S SAPHIRA handler obj A second method is to copy the library file into the standard library directory usually ausr lib e Under MS Windows the shared library is bin sf d11 You must copy or move this file to the standard MS Windows system directory In Windows 95 this is C Windows System in Windows NT it is C Winnt System32 If an application cannot find the shared libraries it will complain and exit Also there will be problems if the application uses older libraries It is good practice to clean up by deleting older shared libraries after doing an installation 1 7 Saphira Quick Start To start the Saphira client demonstration program navigate to inside the bin directory and execute the program named saphira exe For instance with the mouse double click the saphira exe icon inside the saphira ver61 bin folder on your Windows95 desktop With UNIX you must be running the X Window system to execute the Saphira client software and make sure to export or s
52. the LPS In some behaviors it s useful to know the robot s position in the coordinate system of a point sfPointXo and sfPointYo give the robot s x and y coordinates relative to their argument s coordinate system sfPointXoPoint and sfPointYoPoint do the same for an arbitrary point q sfPointMove sets p2 to the coordinates of p1 moved a distance dx and dy in its own coordinate system sfMoveRobot moves the robot in the global coordinate system by the given amount This is a trickier operation than one might suspect because the local coordinates of all artifacts must be updated to keep them in proper correspondence with the robot Note that the values dx and dy are in the robot s coordinate system e g s MoveRobot 1000 0 0 moves the robot forward 1 meter along the direction it is currently pointing Line artifacts are called walls A wall consists of a straight line segment defined by its directed centerpoint plus length Any linear surface feature may be modeled using the wall structure The only type currently defined is WALL 111 8 Saphira API Guide Like points walls may be added or removed from the pointlist so that Saphira registers them in the LPS with the robot s movements Cast each to type point before manipulating them with the pointlist functions described above Drawing artifacts on the LPS display screen is useful for debugging behaviors and interpretation routines Saphira currently draws most types of artifacts
53. the bin bgram program and the resulting source code can be compiled and loaded into Saphira The Saphira library also has a number of pre compiled behavior available for obstacle avoidance and goal seeking These behaviors are described in Section 5 8 5 2 Invoking Behaviors We introduce behaviors with an example invocation of a pre defined behavior There are two ways to invoke behaviors with the Colbert start command from the interaction area or an activity or with the sfStartBehavior function from C code The behavior sfGoToPos moves the robot to a goal position It takes three arguments the speed at which the robot is to move in mm sec a point artifact representing the goal position and a success radius in mm In the interaction area type point goal goal sfCreateLocalPoint 1000 0 0 sfAddPoint goal start sfGoToPos 200 goal 100 53 5 54 Behavioral Control The first two statements create a point artifact situated 1 meter in front of the robot The sfAddPoint function adds it to the pointlist so that its position is updated as the robot moves Finally the start command invokes the sfGoToPos behavior at 200 mm sec to the goal point with success defined as being at most 100 mm from the goal If the robot is connected it will start to move towards the goal point and stop when it gets near From C code you can invoke behaviors using the sfStartBehavior function The arguments are similar to those of t
54. the file available to the Colbert evaluator through the use of sfAddEvalXxXxX function calls For information on the effect of these calls see Section 4 10 1 Under UNIX the loadable object source is compiled normally and the resultant object file is converted to a loadable object file so extension using the LD command and the SHARED link flags see Section 6 2 2 Under MS Windows the project type is set to dynamic load library rather than application test load file for dynamic loading z include saphira h int nopen 0 TANE myfn int a return atl SEL UCR mye lEuce SLIME EA iloeic Jp 74 Saphira Software Manual woe es SEELE STOE ma Iie alimel_mhypSie were f void sfLoadInit void this should be evaluated on open loat a alysis sqrt a rintf Opened d Sf n nopentt a FSMessage Opened d nopen PACES vE ANAN a A EENE il aE ICINVIE 2 fAddEvalConst sfFollowCorridor sfBEHAVIOR sfFollowCorridor fFAddEvalConst SfLEFT Sree ONE fAddEvalVar sfCurrentEnvironment sfPTR fvalue amp sfCurrentEnvironment ag a S S S S S ind_mystruct sfAddEvalStruct mystruct sizeof struct mystruct Chars soma oi Ne Sisal cdi Moy Sitt loy SIE MILONIC NE iii VEIN P sfAddEvalVar m ind_mystruct fvalue amp m 6 3 4 The Direct Client Using direct motion commands the direct cl
55. to create a shared object file so extension You must include the shared object flags SHARED defined in os h for each particular OS 6 2 3 Compiling and Linking Client Programs under MSVC With Microsoft Windows the sample Saphira clients are MS Visual C 4 x projects There are two mak files for all of the sample clients in the handler src apps directory one for botech and one for all the rest Load these into MSVC and you should be able to compile and link the clients One problem with the included projects is that they use absolute path names for the source files including the library file sf lib At this time there seems to be no way to specify relative path names so if you use a different distribution directory i e not c Saphira ver61 then you will not be able to compile the sample applications until you add in the same files using the add files command To run the clients make sure that the SF DLL file is accessible in the C Windows System directory or in a directory on your PATH variable The easiest way to compile and link your own clients is to use the sample project files and modify them to include your source files instead of the sample clients Here are some things to remember when creating new MSVC projects 1 The Saphira library file handler ob4j sf 1ib must be included in the project files 2 The project must compiled in 32 bit mode not 16 bit mode 3 You must add the directory for the include files
56. 1 is a graphical view of the execution process The main client thread starts up and invokes the Saphira OS with the sfStartup function Once startup the OS wakes up every 100 ms and runs every micro task If the argument to sfStartup is 0 then control never returns to the main thread If it is 1 then control returns immediately and both threads execute concurrently Explanations of some sample Saphira client programs are given later in this section 65 6 66 Creating Clients and Load Files 6 2 2 Compiling and Linking Client Programs under UNIX Once the client programs are written they must be compiled with a C or C compiler We recommend the gcc compiler for UNIX systems all sample programs have been compiled using this compiler Other C compilers provided with UNIX systems should also work however The compiler and linker are typically called using the make facility The file handler src apps make file is used to make all of the sample clients and load files Here is a portion of this makefile FEFE E TE AE FE E TE FE FE HE EE EEE REE HERE HE HEE RE HE HEE EE HE HEE HH EE HEE HE HE HEE E E November 1996 Makefile for Saphira applications FEFE E TE AE AE EEE EE EERE EE EERE EE HEE EE EE HEE EE HE HEE HH EE HE HE HE HEE HE SRC OBJ INC LIB BIN 3 off handler include handler obj HIRA if SAPHIRA SAP find out which OS we have include SAPHIRA handler include os h
57. 3 7 Saphira Servers Table 7 1 Main elements of PSOS communication packet protocol OxFA OxFB Packet header same for client and server 1 N 2 Number of subsequent data bytes plus checksum must be lt 200 total bytes long Byte Count Data N command Client command or server information or SIB block discussed in subsequent sections Packet integrity checksum 7 1 1 Packet Data Types Packetized client commands and server information blocks use several data types as defined in Table 7 2 There is no convention for sign each packet type is interpreted idiosyncratically by the receiver Negative integers are sign extended Table 7 2 Communication packet data types bo low byte by high byte by low byte bs high byte up to 200 bo length of string length prefixed b first byte of string unlimited bo first byte of string null terminated 0 null last byte 7 1 2 Packet Checksum A communication packet checksum is derived by successively adding data byte pairs high byte first to the running checksum initially zero disregarding sign and overflow If there is an odd number of data bytes the last byte is XOR ed in to the low order byte of the checksum NOTE The checksum word is placed at the end of the packet with its bytes in the reverse order of that used for arguments and data that is bo is the high byte and b is the low byte Use the following C code fragment in your client applications to compute a
58. LPS see below in the section on the LPS typedef struct sonar data collection buffer IEIMOENE 1x IE IEC lO robot position when sonar read float afx afy afth absolute position when sonar read LOI Zp WE sonar reading in flakey RW coords int range sonar range reading in mm int snew whether it s a new reading sdata IMPORT extern sdata sbucket holds one sdata per sonar indexed by sonar number sdata sfSonarBucket int num int sfSonarRange int num float sfSonarXCoord int num float sfSonarYCoord int num int sfSonarNew int num The first function returns a pointer to the data structure of the num th sonar or NULL if there is no such sonar The next three functions return the range and x y coordinates of the sonar reading The last function returns if it s a new reading 0 if not it also resets the new flag to 0 so that the same reading won t be returned twice 8 4 Direct Motion Control Direct motion control uses the state reflector capability of the Saphira OS to implement a useful client side motion control system Instead of sending motor commands to the server a client sets motion setpoints in the state reflector The OS takes care of transmitting appropriate motor commands to the robot Direct motion control offers three advantages over sending motor control packets directly 1 It checks that the setpoints are actually sent to the robot server given the unr
59. N behavior sfGoToPos Sends the robot to a given point Takes three parameters described in Table 5 5 Table 5 5 Go to position behavior parameters Effect Speed in mm sec Robot moves at this speed towards goal position sfPTR Goal position Should be a pointer to a point artifact sfFLOAT Success radius in mm Defines how close the robot must be to the goal position before the behavior goal is satisfied behavior sfAttendAtPos Moves the robot near a given goal position and points the robot towards the goal position Takes three parameters described in Table 5 6 Saphira Software Manual Table 5 6 Attend at position behavior parameters Effect sfFLOAT Speed Robot moves at this speed towards goal position Value in mm sec sfPTR Goal position Should be a pointer to a point artifact sfFLOAT Success radius Defines how close the robot must be to the goal position before the behavior goal is satisfied Value in mm behavior sfFollow Tells the robot to follows a lane as represented by a lane artifact The lane structure is a directed point with a width although the width is ignored in this behavior since there are explicit parameters for the latitude the robot is allowed in the lane There is also a goal point representing a position in the lane that the robot is to achieve When active the behavior draws its lane as a set of dotted lines in the LPS This behavior takes seven parameters des
60. SAPHIRA handler include into the Additional include directories slot in the Build Settings menu under the C C tab and Preprocessor category Also make sure the symbol _WINDOWS is defined in the Preprocessor definitions slot here 4 Executables should be linked with the multithreaded libraries in the Code Generation item of the C C tab of Build Settings To make a loadable shared file for Colbert just select the dynamic load library a11 project type The library header file 1ib extension containing linkage information for the load library is not needed by Colbert 6 3 Client Examples In this section we illustrate some of the ways of writing Saphira clients with some examples These files are all in handler src apps For explanations of the functions and data structures see the relevant sections of the Saphira API reference Most of the examples exist as loadable Colbert files and compilable standalone clients 67 6 68 Creating Clients and Load Files gt saphira c This is the source for the basic client bin saphira It invokes very basic micro tasks for communication and display and starts the Colbert evaluator gt demo act c A demonstration client that invokes behaviors activities and perception micro tasks as well as user interface functions on the mouse buttons gt testload c Source for a shared object file to be loaded into Colbert gt direct act c This client uses the state reflector and the
61. Sample Application Files 33 4 6 Communication and Connection Utilities 33 4 7 Direct Motion Commands 34 4 8 Activity Schemas 35 4 8 1 Act Definition 35 4 8 2 Colbert Evaluator and Activity States 36 4 8 3 Invocation and Signaling 37 4 8 4 Accessing Activity States 40 4 8 5 Hierarchical Invocation 40 4 8 6 Activity Window 41 4 8 7 Tracing and Error Recovery 41 4 9 Colbert Language 41 4 9 1 Major Changes from ANSI C 41 4 9 2 Comments 42 4 9 3 Keywords 42 4 9 4 Types 42 4 9 5 Expressions 42 4 9 6 Variables 43 4 9 7 Statement Grouping 43 4 9 8 Conditional Statements 44 4 9 9 Iteration and Branching Statements 44 4 9 10 Assignment Statements 44 4 9 11 Function Statements 45 4 9 12 Activity Schemas 45 4 9 13 Direct actions 45 4 9 14 Activity and Behavior Invocation and Signaling 46 4 10 Loading Native C Code 47 4 10 1 Format of Native C Files 47 4 10 2 Making Native C Functions Accessible 48 4 10 3 Making Native C Variables Accessible 48 4 10 4 Making Native C Structures Accessible 4 10 5 Compiling and Loading C Files 5 BEHAVIORAL CONTROL 5 1 Behaviors and Fuzzy Control 5 2 Invoking Behaviors 5 3 Behavior Grammar 5 4 Behavior Grammar in BNF 5 5 Behavior Executive 5 6 Fuzzy variables 5 6 1 Fuzzy variable creation functions 5 6 2 Fuzzy variable combination functions 5 7 Implementing Behaviors 5 7 1 Input parameters 5 7 2 Update function 5 7 3 Init function 5 7 4 Ru
62. T In Unix they are set from a shell using setenv or export Environment Variable Effect SAPHIRA Top level of the Saphira distribution This variable must be set for Saphira clients and the simulator to run correctly In Unix there should be no final slash in the path e g usr local saphira ver6l SAPHIRA_LOAD Initial load directory for the Colbert evaluator This directory will be searched for the file init act when the Colbert evaluator starts If not set defaults to the directory from which the client was started SAPHIRA_COMSERIAL Serial port for connecting to the robot Defaults to the primary serial port for the system being used e g COM1 22 SAPHIRA _SERIALBAUD SAPHIRA_COMPIPI SAPHIRA_COMS Gl ERVE Saphira Software Manual under MS Windows dev cua0 under Linux etc Baud rate for serial connection Defaults to 9600 Local communication port for connection to the Saphira simulator Can be set so that multiple copies of the simulator can run on the same machine and clients can connect to them This variable affects both the simulator and the client application Default depends on the system Machine name or IP address for TCP IP connection Defaults to NULL 23 Saphira Software Manual 3 The Simulator The simulator is a very useful alternative to a physical robot for developing robotics programs Although there is nothing like real world conditions to humble th
63. acing Activity states are set by sending them signals Section 4 8 3 and the state can be examined using the library functions sfGetTaskState sfTaskFinished and sfTaskSuspended Section 4 8 4 4 8 3 Invocation and Signaling Activities and behaviors are invoked with the start command which has the following form start lt schema gt iname lt symbol gt timeout lt int gt priority lt int gt noblock suspend The lt schema gt argument is required and is the name of the activity or behavior schema All of the other arguments are optional and cause modification of the invoked activity or behavior iname lt symbol gt An instance name to give the executing program All references to the activity are through its instance name for example the activity can be signaled using this name By default the instance 37 4 38 Using Colbert name is the schema name If you start two instances of the same schema you must give them different instance names timeout lt int gt A timeout in 100 ms units After this amount of time if the activity or behavior is still executing it is terminated priority lt int gt Behaviors only specifies the behavior priority noblock Don t wait for completion of this activity or behavior go on to the next statement of the act suspend Invoke the activity or behavior but leave it suspended The act or behavior is added to the list of micro tasks but it does not start executing
64. ack registered with sfKeyProcFn This callback should return 0 if the you want the default key action moving the robot when the user presses one of the movement keys for example Otherwise the function should return 1 to Saphira Software Manual signal that it has handled the keypress If you don t want to perform any special keyboard actions you don t have to register a callback Similarly mouse clicks are sent to the callback registered with sfButtonProcFn Again returning 0 from the callback means the default action is invoked returning means the callback handled the mouse click The mouse callback simply returns 0 invoking the default mouse click action Note that the mouse callback could have been omitted it s simply here to illustrate how a mouse callback is invoked 69 6 Creating Clients and Load Files include saphira h void myConnectFn void prototypes void myStartupFn void int myKeyFn int ch HIME TMV UNECOMAA GLINE 2 Wiis Wp IME IO Sine NA ifdef IS_UNIX 7 UNIX main function WoC metio sine arge Clayeite SSe set up user button and key processing sfButtonProcFn myButtonFn sfKeyProcFn myKeyFn sfOnConnectFn myConnectFn sfOnStartupFn myStartupFn fie erare to Conie decti primen W Sie anen Smo warae UPON endif ifdef MS_WINDOWS int PASCAL WinMain HANDLE hInst HANDLE hPrevInstance LPSTR lpszCmdLine int nCmdShow set up user button
65. ally received even in noisy environments If a significant percentage of packets are lost then Saphira s performance will degrade 2 1 4 State Reflector It is tedious for robot control programs to deal with the issues of packet communication So Saphira incorporates an internal state reflector to mirror the robot s state on the host computer Essentially the state reflector is an abstract view of the actual robot s internal state There is information about the robot s movement and sensors all conveniently packaged into data structures available to any micro task or asynchronous user routine Similarly to control the robot a routine just sets the appropriate control 2 Saphira System Overview 10 variable in the state reflector and the communication routines will send the appropriate command to the robot 2 2 Saphira Control Architecture The Saphira control architecture is built on top of the state reflector Figure 2 2 It consists of a set of micro tasks that implement all of the functions required for mobile robot navigation in an office environment A typical client will use some subset of this functionality TCP IP link to other agents Agent Interface Display routines Colbert Executive Registration routines Fuzzy control Sensor interp Direct motion routines control State Reflector Figure 2 2 Saphira Control Architecture The control architecture is a set of routines that interpret sensor readings
66. and or use the Function Activities window During execution the user can examine the state of Saphira variables and stop and start other activities If there is an error the offending activity is suspended and a message is printed The user can change the Colbert text file reload it and run the changed activities There is no need to exit from the application and recompile Even new C functions can be dynamically linked into the system by loading a shared object file 2 3 6 Menus There are seven pulldown menus in the main client window These let you control the display of information in the LPS and related subwindows manage communication to the server and load and save parameter and map files 4 Not all menus are implemented for all versions 16 Saphira Software Manual 2 3 6 1 Connect Menu The Connect menu lets you make and break a connection to the robot server There are three Connect items the standard serial port a local port for the simulator and Bxx robot servers and a TCP connection Choosing one of these items causes the client to try to connect to the physical robot or to the simulator Parameters such as the baud rate and port names can be changed from the interaction window or via library calls see Section 4 6 The Disconnect item closes an open connection to the robot Exit causes the client program to terminate closing any open connection first 2 3 6 2 Files Menu Load the robot s parameters and ma
67. and key processing sfButtonProcFn myButtonFn sfKeyProcFn myKeyFn sfOnConnectFn myConnectFn sfOnStartupFn myStartupFn sfStartup hInst nCmdShow 0 return 0 endif void myStartupFn void sfSetDisplayState sfDISPLAY 2 fe Sete sli Co 5 la o sfRunEvaluator do the evaluator HIME WMV comia LIME 2 SLIME y IUMIE IO e im aeeiewein Os orderan ken nandien int myKeyFn int ch any user processing of keys here switch ch case SPACEKEY sfSetVelocity 0 fe aro Tae rogor sfMessage Stopped return 1 return 0 fo erur Q ror Cerawli merrel void myConnectFn void start those processes 70 Saphira Software Manual 6 3 2 The Demo Client This is the most complex client example making use of activities and pre defined micro tasks and behaviors to implement a handler for the robot There are behaviors for obstacle avoidance and forward motion at constant velocity as well as processes for interpreting sonars recognizing corridors and registering the robot against previously found objects The demo client comes in two forms a loadable Colbert language file demo act and a compilable native C code file We encourage you to use the Colbert language as it s more understandable and easier to work with and modify The Colbert file shown immediately below is loaded into the evaluator by using the load command
68. anguage is C like in that it has a syntax that is close to that of ANSI C It has many but not all of C s expression and statement constructs and additional constructs that are specific to Saphira such as the direct motion commands and the invocation of activities and behaviors Colbert is not meant to be a replacement for writing code in C You cannot define new C functions in Colbert acts are like functions but are executed differently For any complicated computation we recommend writing a C function compiling it into a shared object and then loading it into the evaluator Section 4 10 Most of the Saphira library functions variables and structures are available in Colbert There aren t many C library functions such as the trigonometric functions but these can easily be added by the user via shared object files 4 9 1 Major Changes from ANSI C The typing system is slightly different The basic types are int float void and string essentially char There is no double or char type Structures are permitted but only by explicitly importing them from a native C shared object file gt There are no arrays or array operators gt The type double is not available instead all floating point numbers are single precision float gt There isno typedef operator and new structures cannot be defined in Colbert they must be imported from native C object files The following operators are not defined op gt gt lt
69. annel X is a b or c small letters On startup the vision system channels are set to BLOB_MODE The processing performed in BLOB_MODE BLOB_BB_MODE and LINE_MODE are explained in the FTVS manual In line mode several FTVS parameters affect the processing line_bottom_row first row for line processing line_num_slices how many row we process line_slice_size how many pixels thick each row is line_min_mass number of pixels needed to determine a line segment These parameters can be set using for example sfRobotComStr VISION_COM line bottom_row 0 9 2 Vision Packets If the FTVS is working properly it will send a vision packet every 100 ms to the Saphira client In the information window the VPac slot should read about 10 indicating that 10 packets second are being delivered If it reads 0 the vision system is not sending information 121 9 Saphira Vision Saphira parses these packets into a vision information structure struct vinfo int type BLOB BLOB _BB or LINE E int x y center of mass int area size int h w height and width of bounding box int first num first and number of lines In BLOB_MODE the x y and area slots are active The x y coordinates are the center of mass of the blob in image coordinates where the center of the image is 0 0 For the lens shipped with the FTVS each pixel subtends approximately 1 3 degree define DEG_TO_PIXELS 3 0
70. ary variables and can also be accessed set and queried by using the variables connect serial lt port gt Connects via lt port gt or the current serial port sfComSerial at the specified baud rate sfSerialBaud set serial lt port gt Sets or returns the serial port sfComSerial If lt port gt is given sets the serial port to this value The first serial port of the machine COM1 dev ttya etc is the default set baud lt rate gt Sets or returns the baud rate of the serial connection sfSerialBaud If the argument lt baud gt is given sets it to this value The default rate is 9600 baud With PSOS 4 3 the Pioneer server now supports 19200 baud 33 4 Using Colbert 34 Other baud rates may be used for specialized applications connect local lt pipe Connects via the local communication port with name lt pipe gt or the default name sfComPipe This is the normal connection for the simulator or for the Bxx robot servers The default name of the connection can be changed by setting sfComPipe to another string set local lt pipe gt Sets or returns the local connection name This command is useful when running multiple simulators on the same machine since each simulator can be assigned a unique local connection name connect server lt netaddr gt Connects to a robot server via TCP IP to a remote machine specified by lt net addr gt or the default address sfComServer The robot may be a Bxx or s
71. ause the language is interpreted by the executive it is much easier to develop and debug activities since errors can be trapped an activity changed in a text editor and then re invoked without leaving the running application 2 2 5 Sensor Interpretation Routines Sensor interpretation routines are processes that extract data from sensors or the LPS and return information to the LPS Saphira activates interpretative processes in response to different tasks Obstacle detection surface reconstruction and object recognition are some of the routines that currently exist all working with reflected data from the sonars and from motion sensing 11 2 12 Saphira System Overview 2 2 6 Registration and Maps In the global map space Saphira maintains a set of internal data structures artifacts that represent the office environment Artifacts include corridors door walls and rooms These maps can be created either by direct input from a map file or by running the robot in the environment and letting Saphira extract the relevant information Registration is the process of keeping the robot s global location in an internal map consistent with sensor readings from the local environment Routines exist for extracting relevant information from the LPS and matching it to map structures in the GMS then updating the robot s position 2 2 7 Graphics Display Displaying internal information of the client is essential for debugging robot c
72. avior sfFollow 61 behavior sfFollowCorridor 61 behavior sfFollowDoor 62 behavior sfGoToPos 60 behavior sfKeepOff 60 behavior sfStop 59 behavior sfStopCollision 59 behavior TurnTo 62 Direct Motion Control int sfDoneHeading 99 int sfDonePosition int dist 99 void sfSetDHeading int dhead 99 void sfSetHeading int head 99 void sfSetMaxVelocity int vel 99 void sfSetPosition int dist 99 void sfSetRVelocity int rvel 99 void sfSetVelocity int vel 99 void sfTargetHead void 99 void sfTargetVel void 99 Drawing and Color void sfDrawCenteredRect float x float y float w float h 112 void sfDrawRect float x float y float dx float dy 112 void sfSetLineColor int color 112 void sfSetLineType int w 112 void sfSetLineWidth int w 112 void sfSetPatchColor int color 112 void sfSetTextColor int color Fuzzy Variables float down_straight float x float min float max 57 float f_and float x float y 57 float f_eq float x float c float delta BY float f_greater float x float c float delta 57 Float f_not float x 57 float f_or float x float y 57 float f_smaller float x float c float delta 57 float straight_up float x float min float max 57 Activities int finished process p Error Bookmark not defined process intend_beh behavior b char name int timeout beh_params params int priority Error Bookmark not defined process sfInitActivity void fn void char name int timeout
73. avior controller by using the direct motion functions then don t initiate this process Function Name Description 95 8 Saphira API Guide xecute_current_behaviors execute _ evaluates behaviors and outputs a motor control void sfInitInterpretationProcs void Starts up processes for interpretation of sonar results Function Description occgrid_proc occupancy grid computes an occupancy grid side_segment_proc side segs forms linear artifacts robot motion test_wall_proc test wall wall recognition test_wall_break_proc test wall break door and junction recognition These processes must be started to have results deposited in sfLeftWallHyp and sfRightWallHyp void sfInitRegistrationProcs void Starts up some position registration processes useful for navigation in an office environment Function Description test_match_proc test matching matching of linear and point artifacts test_environment_proc test where identification of current situation void sfRunEvaluator void This micro task starts up the Colbert evaluator which is the executive for activities The evaluator also accepts input from the interaction window The basic client bin saphira c starts this process If you define a standalone client and want to run Colbert then start this micro task using sf InitProcess in your startup callback 8 3 State Reflection State reflection is a way of isolating client programs from the work involved in send control
74. b2Angle float al float a2 These functions compute angles in the LPS Normally angles in the LPS are represented in degrees using floating point numbers Artifact angles are always normalized to the interval 0 360 sfNormAngle will put its argument into this range The corresponding functions sfAddAngle and sfSubAngle also normalize their results this way It is often convenient to give headings in terms of positive counterclockwise and negative clockwise angles The second normalization function sfNorm2Angle converts its argument to the range 180 180 5 so that the discontinuity in angle is directly behind the robot The corresponding functions sfAdd2Angle and sfSub2Angle also normalize their results this way 110 Saphira Software Manual Finally it is sometimes useful to reflect all angles into the upper half plane 90 90 The function sfNorm3Angl1e will do this to its argument by reflecting any angles in the lower half plane around the X axis e g 100 degrees is reflected to 80 degrees float sfPointPhi point p float sfPointDist point p float sfPointNormalDist point p float sfPointDistPoint point pl point p2 float sfPointNormalDistPoint point p point q void sfPointBaricenter point pl point p2 point p3 The first three functions compute properties of points relative to the robot The sfPointPhi function returns the angle of the vector between the robot and the point p in degrees from
75. basic behavior beh You can refer to these for reference and ideas on how to write behaviors 5 4 Behavior Grammar in BNF Here are the complete rules for the behavior grammar in the form accepted by the YACC or BISON parsers 55 5 Behavioral Control Behavior definition name params rules init update activity EHAVIOR BeginBehavior symbol Params PARAM_STMTS Rules RULE_STMTS TA CESTMTS Update LESS TMS Nene sbyALE sy ACT_STMTS EndBehavior behavior parameters PARAM STMTS MS 26 INGE sy EILYOVACI See I Syaqilevodl PARAMS Rule definition name fuzzy var action mod RULE_STMTS SYMBOL If FUZZY_EXP Then CONTROL RUL fuzzy expression FUZZY_EXP symbol float Wine RUZE AE HUZAZY EEXPIRE RUZAN TEI NSO re VOSA NC 1h DOANE aye S47 Please Hons andi mod Erens maar CONTROL Wien Ikebe MOI abuieia IRavejlate MODA Turn symbol MOD Speed MVAL Very Slowly Slowly Moderately Sharply Very Sharply symbol symol amie miloer activity statements AGT SIMTS lt Taronani VSixeecl Goel eie eicass INU Aa ACT_STMTS 5 5 Behavior Executive Before any behaviors can be invoked and run the behavior executive must be started Normally this is done using the sfInitControlProcs call
76. beginning of the activity remove lt symbol gt sfREMOVE Causes the scheduler to reap the activity trace lt symbol gt Traces the activity untrace lt symbol gt Untraces the activity Saphira Software Manual 4 10 Loading Native C Code Native object files can be dynamically loaded into the execution environment giving access to Saphira internals and new C functions Since Colbert has only a limited implementation of C in many cases you must load C object files to accomplish a task For example the only way to define new structures is to load in an appropriate C object file 4 10 1 Format of Native C Files In general a C source file will contain user defined functions and variables and a special function sfLoadInit that will be called when the file is loaded SfLoadInit will contain calls to the sfAddEvalXXx functions which will make native C functions and variables defined in the file or already loaded in the system available to Colbert The following is an example load file in handler src apps testload c test load file for dynamic loading if include saphira h int nopen 0 IQUE Iyer Ligne Gi return atl SiGiSUIGNE UMV ASIC 1EUICIE AONE BYP fiear o Wiealel wer m e HNO SeU CEs void sfLoadInit void evaluated on load Foara IPO a sqrt a primer Ovenec Sel Sie inl DODEN a y sfSMessage Opened d nopen SACE a aa a A
77. beh_params type is an array of such parameters typedef union a param can be either a fp number f ie ei jo alincere 4 iE Jo uc tE VOLO jd param typedef param beh_params 5 7 2 Update function On each Saphira cycle 100 ms the behavior updates its state variables using information from the LPS and then evaluates its rules Updating is accomplished by an update function which takes the beh_param structure as an argument 5 7 3 Init function When Saphira instantiates a behavior schema its init function is called to set up the initial fuzzy state The input to the init function is a beh_params structure containing the initial parameters of the behavior The init function can set any initial state that is needed by the behavior a clock for example if the behavior has a timeout 5 7 4 Rules Each behavior rule is defined as a structure beh_rule which consists of a name and two indices into the fuzzy state the antecedent value for the rule and the mean value of the output action Each rule can recommend only one action which is the consequent value one of Accel Decel Turn_left or Turn_right typedef struct char name name of the rule int antecedent fe accalwilicy Or cnis rule consequent fa aerion ro take lt parameter mean value of action beh_rule For example rule definitions see below Note that the consequent value constants are external integers but they
78. cause the client to abort Exiting shuts down any current connection and exits the application Quitting a connected simulator will usually cause the client to quit as well so it s a good idea to disconnect from the client side first 3 4 6 Information Area The information area at the bottom of the simulator window shows messages about the connection status It also shows the absolute x y position of the robot in meters and the angle of the robot in degrees 3 5 Mouse Actions The left mouse button puts the simulated robot at the position of the cursor This moves the robot in its world and the X Y coordinates at the bottom of the screen will change If the robot becomes stuck against a wall using the left mouse button to move it a little can unstick it The middle button moves the simulated world position at the cursor to the center of the display Saphira Software Manual 4 Using Colbert This section describes the Colbert language and evaluator Colbert is a C like language with a semantics based on finite state machines It has the standard C constructs such as sequences iteration and conditionals interpreted in a way that makes sense for robot programming The main construct of Colbert is the activity schema or act a procedure that describes a set of coordinated actions that the robot should perform Colbert is an interpreted language which means that you write text files containing Colbert activities load them into a Saphi
79. ch linear segments and door openings against its map artifacts This lets you align the robot s global position with the global map The micro task that performs registration is called test matching In the sample Saphira client this micro task is invoked by the function sfInitRegistrationProcs To disable registration either do not start the test matching micro task or set its state to sf SUSPEND using sfTaskSuspend The registration micro tasks will preferentially match a complete doorway or corridor if it has constructed the corresponding hypothesis from sonar readings and there is a suitable map artifact close by Otherwise it will attempt to match single walls or sides of doorways Matching corridors and walls helps keep the robot s angle aligned and also its sideways distance Finding doors helps it to align in a forward back direction Both of these are important to keeping the robot registered but the angle registration is critical since the robot s dead reckoned position quickly deteriorates if its heading is off Corridor junctions can also be important landmarks for registration Ideally junctions should be automatically generated from intersections of corridors However this capability does not currently exist and you have to put them in by hand In the figure Junction 5 is only one of three possible junction artifacts for the corridor intersection It will be used to register the robot as it moves down Corridor 2 just li
80. checksum 84 Saphira Software Manual int calc_chksum unsigned char ptr ptr is array of bytes first is data count Hy aioe iay ime e Op ig aA n 2 don t use chksum word while n gt 1 Ga Cee lt lt oie 5 amp ORIRE n gt 0 return c 7 1 3 Packet Errors Currently the Saphira server interface ignores a client command packet whose byte count exceeds 200 or has an erroneous checksum The client should similarly ignore erroneous server information packets Saphira does The Saphira client server interface does not acknowledge receipt of a command packet nor has any facility to handle client acknowledgment of a server information packet Hence Saphira client server communication is as reliable as the physical communication link UNIX pipes with the simulator or a cable tether between the robot and client computer are very reliable links Radio modem mediated communication is much less reliable Accordingly when designing client applications that may use radio modems do not expect to receive every information packet intact nor have every command accepted by the server The design decision to provide an unacknowledged packet interface is a consequence of the realtime nature of the client server interaction Simply retransmitting server information blocks or command packets would result in antiquated data not at all useful for a reactive client or server For some operations however t
81. creenToWorldY 97 sfSerialBaud 33 sfSetDHeading 101 sfSetDisplayState 96 sfSetFrontBuffer 107 sfSetGlobalCoords 110 sfSetHeading 101 sfSetLineColor 114 sfSetLineType 114 sfSetLineWidth 114 sfSetLocalCoords 110 sfSetMax Velocity 101 sfSetPatchColor 114 sfSetPosition 101 sfSetProcessState 104 sfSetRVelocity 101 140 sfSetSideBuffer 107 sfSetVelocity 101 sfSMessage 96 sfStalledMotor 100 sfStartup 95 sfStopCollision 60 sfSub2Angle 112 sfSubAngle 112 sfSuspendProcess 104 sfSuspendSelf 104 sfTargetHead 101 sfTargetVel 101 sfUnchangeVP 113 sfWaitClientPacket 121 shared library installation 4 shrink 17 27 shut down 91 Simulator connect menu 27 See connect menu connecting 13 Description 25 display menu 27 exit menu 28 General description 2 grow 27 information area 28 load menu 26 27 See load menu Menus 27 mouse actions 28 parameter files 26 pioneer exe 25 recenter menu 28 shrink 27 socket 25 Starting 25 wake 27 Worlds 27 single step 17 Sonar buffers 105 sfFrontMaxRange 107 sfSetFrontBuffer 107 sfSetSideBuffer 107 sonars 94 sonars menu clear buffer 17 sonars on 17 SPac 16 speed 34 SRI International iii 1 6 7 12 65 116 start up 91 startup callback 70 State reflection 98 state reflector 9 10 11 12 70 79 89 97 98 100 101 105 sfRobot 99 Stop collision parameters 60 straight_up 57 support See Customer resources SYNCO 88
82. cribed in Table 5 7 This behavior sets the sfPreferredTurnDir variable depending on how the robot is misaligned with the lane Table 5 7 Follow lane behavior parameters Effect TR sfP1 Lane This is a point or lane artifact representing a line the robot is to follow Parameters below define allowed deviations from the line sfPTR Goal position The robot moves along the lane in the direction of the goal until it reaches it Should be a pointer to a point artifact sfFPLOAT Right edge in mm Distance the robot is allowed to wander from the right side of the line sfFLOAT Left edge in mm Distance the robot is allowed to wander from the left side of the line sfFLOAT Speed off lane in mm sec How fast the robot travels when it is out of the lane sfFLOAT Speed in lane in mm sec How fast the robot travels when it is in the lane sfFLOAT Turn ratio How important it is to be near the center vs aligned in the right direction 0 0 direction overrides 1 0 center overrides behavior sfFollowCorridor Tells the robot to follow a corridor as represented by a corridor artifact The corridor structure is a directed point with a width the width is used to set up a lane down the center of the corridor for the robot to follow There is also a goal point representing a position in the lane that the robot is to achieve When active the behavior draws its lane as a set of dotted lines in the LPS This behavior takes two
83. cription file is a plain text ASCII document typically stored with the w1d filename suffix which describes the size and contents of a simulated world A sample world file can be found in the Section 11 along with instructions on how to create your own worlds We ve also included several sample world files with the Saphira distribution found in the worlds directory If the simulator is connected to a client the client can tell the simulator to load a world file via the sfLoadWorldFile function 3 4 Simulator Menus Several simulator menus control the parameters and actions of the simulated robot There are controls for loading world and parameter files for adjusting the display and for changing the connection type for example Not all menus are implemented in every version 3 4 1 Load Files Menu The File Load Params item brings up a file selection dialog to load a robot parameter file The parameter file changes the characteristics of the simulated robot such as the number and placement of the sonars By default the Pioneer robot parameters are loaded The File Load World item brings up a file selection dialog to load a world file 3 4 2 Connect Menu The Connect menu controls the port that the simulator listens on and also disconnects the simulator from an aborted client By default the simulator is listening on the interprocess communication port waiting for a client on the same machine The simulator can also listen
84. ctivities to check it the robot server is currently connected to the client The user should not change the value of this variable The variable sf IsExited is set to 1 when the user requests Saphira to exit from the Connect Exit menu item This variable is useful for async user code which calls sfStartup in nonblocking mode and then continues execution The code can check the sf IsExited flag to see if there is an exit request None of these callbacks are obligatory in user code usually the connect callback is registered The startup callback should include any relevant initialization code such as menu or directory settings in this function The connect callback should start micro tasks behaviors and other Saphira control routines The disconnect callback can be used to clean up after the Saphira client disconnects from a robot 93 8 94 Saphira API Guide void sfSetDisplayState int menu int state Use the sfSetDisplayState function to change the state of a display mode in the Saphira window interface If you call this function before connecting to the robot in the startup callback it will set the default state for the display function Thereafter the preset display values are sticky Saphira automatically resets them to the preset values perhaps different from the defaults given in Table 8 1 whenever a new connection is made with the robot Table 8 1 Optional states for various Saphira display functions State Gnt
85. d colbert direct act lt cr gt 29 4 30 Using Colbert Colbert exampl XeceusinGmehemc lac cemmotrTonmed lulkcums ace Dew rolka a l GO back ame rorta Yaw wwimes S while Tune oC ONE a gail move 1000 EERME ONA move 1000 act square move im asguarei 1 cela meciiy a 4 while a call them sequentially patrol patrol 4 square square sfSetDisplayState sfGLOBAL 1 put display into global coords start aa tant Who ila ioysileweall gicicslyaniey a Figure 4 1 A direct motion application in the Colbert language in MS Windows the forward slashes will be backslashes Loading the file defines three activities and starts one of them aa which calls the other two A listing of the file is in Figure 4 1 Activity schemas are defined in a manner similar to C functions using the keyword act Just as with C functions acts take arguments which are given when the activity is called or instantiated For example in the act aa the patrol activity is called with an argument of 4 which means that the robot will go back and forth 4 times The direct motion commands in pat rol and square are executed by the evaluator which waits until they complete before moving on to the next statement The same thing is true of the calls to the pat rol and square activity within aa This is an example of blocking execution of motion commands or activities which is generally desi
86. d children are also resumed Hierarchical invocation makes it easy to turn sets of activities on and off Saphira Software Manual 4 8 6 Activity Window Activities and behaviors can be controlled from the activity window invoked from the Functions Activities menu The activity window shows the state of all activities and behaviors in the system Double clicking on the activity or behavior will change its state from running to interrupted suspended or from suspended to resumed 4 8 7 Tracing and Error Recovery Activities can be traced by sending them the trace signal For a traced activity as each statement is evaluated its value is printed in the user interaction area along with the source line of the statement The source line is an offset from the beginning of the activity schema definition To cut down on the amount of output the executive only prints information when the state of a traced activity changes For example nothing is printed while an activity is waiting for completion of a direct motion command until the command finishes and the activity goes on to the next statement The evaluator traps all fatal errors in micro tasks i e all fatal user errors An error message is printed and the offending command is exited In the case of an error caused by a statement in an activity the line number of the activity relative to the top of the activity is printed and the activity is suspended 4 9 Colbert Language The Colbert l
87. e FindAndFollow and BumpAndGo activities are not present 72 Saphira Software Manual The code starts by defining the main function setting callbacks as in the saphira client and then calling sfStartup to initiate the Saphira system The startup callback just sets the display update rate to 5 Hz On connection to the robot the registration and interpretation micro tasks are started up just as in the Colbert file In addition a user micro task is invoked This micro task is defined below The demo client z include saphira h void myConnectFn void void myStartupFn void int myKeyFn int ch any user key processing here TIE MMPSUNEIE OMIA MIME Se GME WW LNE Jo Sine am vore WEIL Gime aicge Chai euceny set up user button and key processing sfButtonProcFn myButtonFn sftKeyProcFn myKeyFn sfOnConnectFn myConnectFn sftOnStartupFn myStartupEn Jes BENE Wis Ciya slic CONLE Ol SES use wo NONE void myStartupFn void sfSetDisplayState sfDISPLAY 2 Jee BEC sie CO 5 GA S void myConnectFn void start those processes sfInitRegistrationProcs sfInitInterpretationProcs Stine COMECON oes H F sfInitAwareProcs sfInitProcess test_control_proc User Process The user micro task test_control_proc Is very simple it starts up several behaviors then puts itself into a suspended state You can change the state of the invoked behaviors from
88. e and subject to false readings setting it lower will make them less sensitive 8 6 2 Occupancy functions The following functions look at the raw sonar readings to determine if there is an obstacle near the robot Other Saphira interpretation micro tasks use the sonar readings to extract line segments representing walls and corridor Saphira has several functions for testing whether sonar readings exist in areas around the robot The different functions are useful in different types of obstacle detection routines for example when avoiding obstacles in front of the robot it s often useful to disregard readings taken from the side sonars The detection functions come in two basic flavors box functions and plane functions Box functions look at a rectangular region in the vicinity of the robot while plane functions look at some portion of a half plane int sfOccBox int xy int cx int cy int h int w int sfOccBoxRet int xy int cx int cy int h int w float x float y When using these functions it helps to keep in mind the coordinate system of the LPS They look at a rectangle centered on cy cy with height h and width w sfOccBox returns the distance in millimeters to the nearest point to the center of the robot in the X direction xy sfFRONT or Y direction xy sf SIDES The returned value will always be a positive number even when looking on the right side of the robot negative Y values If there is no sona
89. e available The return type rt ype is the C index of a Colbert type see Section 4 9 3 The predefined types are ime sfINT float sfFLOAT void sfVOID string sfSTRING act SHA Daal behavior sfBEHAVIOR Vonia SEER In addition pointers to types can be defined with the function sfTypeRef int type For example to define a pointer to an integer use sfTypeRef sfINT The function sfTypeDeref performs the inverse operation giving the type of the reference of a pointer but this is less useful in defining argument types nargs is the number of arguments of the function currently a maximum of 7 If the function takes a variable number of argument then use a negative number here where nargs is the number of required arguments of the function Each argument to the function is described by the C index of its type For example the library function void sfSMessage char format is made accessible with sfAddEvalFn sfSMessage sfSMessage sfVOID 1 sfSTRING sfSMessage has one required argument a string and returns void 4 10 3 Making Native C Variables Accessible Native C variables in user code and the Saphira library are made accessible in Colbert with the sfAddEvalVar function This function can only be called from loaded C object files not from the Colbert evaluator It must always be compiled and loaded from a shared object file usually as a call in the sfLoadInit function see the example in t
90. e centered version takes a center point of the rectangle and a width and height The non centered version takes the lower left corner position a width and a height Saphira s graphics routines now use a state machine model in which color line thickness and other graphics properties are set by a function and remain for all subsequent graphics calls until they are set to new values Note that you cannot depend on the state of the graphics context when you make a graphics call and should set it appropriately void sfSetLineWidth int w void sfSetLineType int w void sfSetLineColor int color void sfSetPatchColor int color int sfRobotColor int sfSonarColor int sfWakeColor int sfArtifactColor int sfStatusColor int sfSegmentColor For lines set the width w to the desired pixel width This width affects all lines drawn in rectangles and vectors You may select one of two line types Set the w function parameter to sfLINESOLID for a 112 Saphira Software Manual solid line and sfLINEDASHED for a dashed line The patch and line colors accept a color value as shown in Table 8 5 Saphira drawing colors for the robot icon and various artifacts can be set using the variables shown above Table 8 5 Saphira colors Color Reference lorYellow rLightYellow Red rLightRed DarkTurquoise rDarkOliveGreen rMagenta rSteelBlue rBrickRed rBlack rWhite O O O O lorOrangeRed O O
91. e from the evaluator and are an alternate way of issuing direct motion commands move int mm Move the robot mm millimeters forward positive or backwards negative Blocking turn int deg Turn the robot deg degrees clockwise negative or counter clockwise positive degrees from the current heading Blocking turnto int deg Turn the robot to the heading deg degrees Positive values are counter clockwise negative values are clockwise Blocking speed int mms Move the robot at a speed of mms millimeters per second forward positive or backwards negative Nonblocking Saphira Software Manual rotate int degs Move the robot at a rotational speed of degs degrees per second counter clockwise positive or clockwise negative Nonblocking halt Halts all direct motion commands 4 8 Activity Schemas Activity schemas are Colbert language programs for controlling the robot They are interpreted using the Colbert evaluator Activities execute similarly to normal C functions evaluating statements in order with sequences loops and conditionals However the underlying execution model is quite different it is a finite state machine Each statement of the activity is a node that can potentially wait for a condition to hold before going on to the next statement or can change the flow of execution For example every primitive action move turn etc that is invoked causes the program to stay at that statement until the actio
92. e function 58 window 18 Behaviors window 18 Behaviors 54 Bxx connecting 13 C programs 66 cd 33 Channel modes 123 checksum 86 chroma h 123 Client Activities window 21 artifacts 15 bat 16 battery 16 Behaviors window 18 commands 87 See Client commands connect menu 17 connect menu See files menu See connect menu control point 14 CPU 16 display 13 display menu 17 See display menu files menu 17 functions menu 17 grow 17 Information area 15 interaction area 16 keyboard 18 main window 13 MPac 16 obstacle sensitivity 14 position 16 Processes window 20 shrink 17 sonars 14 sonars menu 17 See sonars menu 135 Index SPac 16 Starting 1 status 15 velocity 16 velocity vectors 14 VPac 16 Client commands argument types 88 communication rate 87 composition 88 General 87 PSOS 88 saphira h 88 Client installation See Installation Clients async example 81 direct example 77 nowin example 83 packet example 79 saphira example 73 Colbert 1 29 Activities 11 35 evaluator 16 29 example 29 help facility 31 interaction area 16 31 Language 11 load directory 32 33 loading files 29 32 loading shared object files 33 sample applications 33 syntax errors 31 Colbert commands cd 33 connect 33 connection 33 direct motion 34 disconnect 33 exit 33 halt 34 move 34 pwd 33 rotate 34 set baud 33 set serial
93. e most ambitious robotics project the simulator does have the distinct advantage of having a single step mode in which you can reenact every detail of your programs including a robotics fatality And too the simulator has realistic error models for the sonar sensors and wheel encoders so that in general if a client program works with the simulator it will work on the physical robot The simulator also lets you construct a simple world in which the simulated robot navigates You can even change the robot s operating characteristics to simulate your own robot designs And since the packet interface of the simulator is the same as the physical robot no changes to the client program are required in switching between the two The disadvantage of the simulator is that the environment model is an abstraction of the real world with simple 2 D linear segments in place of the complex geometrical objects the real robot will encounter in the real world For example the simulator assumes all objects are sensor high so it can t simulate a door stop something the real robot will have to overcome to traverse rooms in a real building 3 1 Starting the Simulator Execute the program named pioneer exe in the Saphira bin directory By default the simulator acts like the Pioneer 1 Mobile Robot hence its name We tell you how to simulate other robots in a following section of this chapter Normally the simulator connects to the client using an int
94. eadings to keep Higher values mean the interpretation routines can find longer side segments 127 Saphira Software Manual 11 Sample World Description File Worlds for the simulator are defined as a set of line segments using absolute or relative coordinates Comment lines begin with a semicolon All other nonblank lines are interpreted as directives The first two lines of the file describe the width and height of the world in millimeters The simulator won t draw lines outside these boundaries It s usually a good idea to include a world boundary rectangle as is done in the example below to keep the robot from running outside the world Any entry in the world file that starts with a number is interpreted as creating a single line segment The first two numbers are the X Y coordinates of the beginning and the second two are the coordinates of the end of the line segment The coordinate system for the world starts in the lower left with Y pointing up and X to the right Figure B 1 Y 90 degrees X 0 degrees 0 0 Figure B 1 Coordinate system for world definition files The position of segments may also be made relative to an embedded coordinate system The push x y theta directive in the world file causes subsequent segments to use the coordinate system with origin at x y and whose x axis points in the direction The theta push directives may be nested in which case the new coordinate system is defined with resp
95. ecial modifiers such as const or extern are permitted int a ptr tonowhere float f robot r Variables can be declared at the beginning of acts and at the top level of a Colbert source file Top level variables have global extent and are accessible by all Colbert activities Variables declared within an act are local to that act and function as static variables copy of the local variables All variables are initialized to zero Colbert variables can also be declared by linking Each invocation of an activity schema gets its own them to a native C variable with the sfAddlI EvalVar function These variables need not have an explicit Colbert declaration although it is legal to give them one The value of the Colbert variable reflects the value of the C variable 4 9 7 Statement Grouping Statements are grouped by using curly braces lt stmt1l gt lt stmt2 gt lt stmt3 gt Grouping is useful in Colbert specific forms such as update in act definition that take only a single statement The empty statements oo r and are valid statements 43 4 Using Colbert 44 4 9 8 Conditional Statements Colbert has the standard if statement for conditionals if lt c_exp gt lt stmts gt else lt stmts gt There is a special Colbert from for waiting until some condition holds waitfor lt c_exp gt timeout lt n gt This statement effectively causes the Colbert executive to sus
96. ect to the previous one A pop directive reverts to the previous coordinate system The position x y theta directive positions the robot at the indicated coordinates The following is a fragment of the simple wld world description file found in the worlds directory of Saphira 33 Fragment of a simple world width 38000 height 30000 0 0 0 30000 World frontiers 0 0 38000 0 38000 30000 0 30000 38000 30000 38000 0 129 11 World Description File push 10000 14000 0 3 upper corridor length 14 600 width 2 000 0 12000 3000 12000 EJ 231 J Lee 3900 12000 4200 12000 EJ 233 D Moran 5100 12000 8000 12000 EJ 235 J Bear 8900 12000 9200 12000 EJ 237 E Ruspini 10000 12000 12000 12000 EJ 239 J Dowding 12800 12000 14600 12000 Starting position position 17500 14000 90 130 Saphira Software Manual 12 Saphira API Reference Artifacts Page void sfAddAngle 110 void sfAdd2Angle 110 void sfAddPoint point p 109 void sfAddPointCheck point p 109 void sfChangeVP point pl point p2 point p3 LAA point sfCreateGlobalPoint float x float y float th 108 point sfCreateLocalPoint float x float y float th 108 point sfFindArtifact int id 109 point sfGlobalOrigin 109 void sfMoveRobot float dx float dy float dth nial 8 void sfNormAngle 110 void sfNorm2Angle 110 void sfNorm3Angle 110 void sfP
97. ecting the real time nature of robot control Finally because all Saphira routines are in a library user programs that link to these routines only need to include those routines they will actually use So a Saphira client executable can be a compact program even though the Saphira library itself contains facilities for many different kinds of robot programs 2 1 3 Packet Communications Saphira supports a packet based communications protocol for sending commands to the robot server and receiving information back from the robot Typical clients will send an average of to 4 commands a second and all clients receive 10 packets a second back from the robot These information packets contain sensor readings and motor movement information see Section 7 3 The amount of data sent is typically only 30 to 50 bytes per packet so even a relatively modest 9600 baud channel can accommodate it Saphira has the capability of connecting to a robot server over a tty line the ethernet with TCP IP or a local IPC link Since the data channel may be unreliable e g a radio modem packets have a checksum to determine if the packet is corrupted If so the packet is discarded which avoids the overhead of sending acknowledgment packets and assures that the system will receive new packets in a timely manner But the packet communication routines must be sensitive to lost information and have several methods for assuring that commands and information are eventu
98. ectly as FSMs in the C language it s much more convenient to write activities in the Colbert language The activity language has a rich set of control concepts and a user friendly syntax that makes writing control programs much easier Activities are a special type of micro task and run in the same 100 ms cycle as other micro tasks Activities are interpreted by the Colbert executive so the user can trace them break into and examine their actions and rewrite them without leaving the running application Developers can concentrate on refining their algorithms rather than dealing with the limitations of debugging in a compile reload re execute cycle Since they are invoked every 100 ms micro tasks must partition their work into small segments that can comfortably operate within this limit e g checking some part of the robot state and issuing a motor command For more complicated tasks such as planning more time may be required and this is where the second kind of user routine is important Asynchronous routines are separate threads of execution that share a common address space with the Saphira library routines but are independent of the 100 ms Saphira cycle The user may start as many of these separate execution threads as desired subject to limitations of the host operating system The Saphira system has priority over any user threads thus time consuming operations like planning can coexist with the Saphira system architecture without aff
99. ed object file used only under MS Windows for unloading DLLs Colbert source files can have an arbitrary extension except for so or d11 but by convention their extension is act This extension must be included in the file name Evaluator files can be changed and reloaded as often as desired If an activity schema is redefined by reloading then all instances of the schema are changed This has implications for how the user should handle instantiated activities that are being debugged The state of the activity is not changed by the redefinition so the activity will continue execution at its current line This may not make sense if the line numbering has changed However the standard states sf INTERRUPT sfSUSPEND etc are the same for all activities so these are safe states for redefinition In general it s probably best to suspend an activity if you re going to change its definition Redefining an instantiated activity does not change its arguments or internal state Normally this is what you would like since the activity can resume operation with the same arguments and internal variables However if the number of arguments or their ordering is redefined or internal variable declarations are changed then the instantiated activity may be confused as to how to find the values of these entities In this case it is better to remove the activity and restart it 4 5 1 Loading Shared Object Files Some API functions wi
100. ed time without completing The fixed cycle time of micro task invocation means that micro tasks can have guaranteed response time for critical tasks a controller can issue a command every 100 millisecond for example Of course response time depends on the conformity of all micro tasks the combined execution time of all micro tasks must never exceed 100 milliseconds If it does the cycle time will exceed 100 milliseconds for all micro tasks Hence allow around 2 5 milliseconds compute time per micro task and either divide large micro tasks into smaller pieces each able to execute within the 2 5 millisecond time frame or run them as concurrent threads Here is an example of a typical interpretation micro task function It starts by setting up some housekeeping variables then proceeds to alternate door recognition with display of its results every second or so define FD_FIND 20 define FD_DISPLAY 21 void find_doors void ine roundi one switch process_state case sfINIT Come here on startup found_one 0 PS process STATS EDSERENDP break case sfRESUME Come here after suspend Process STEATE HD_F IND break case sfINTERRUPT Interrupt request found_one 0 process_state sfSUSPEND break case FD_FIND Ja WiOOlkkiLing ror Coors E calll WACOCMALEALCIN EUNET process_state FD_DISPLAY break case FD_DISPLAY Now we display it if found_one E eati clisjuleny UNGE om proces
101. ed to from standard C code For examples of the use of these indices see Sections 4 10 2 4 10 3 and 4 10 4 4 9 5 Expressions Expressions use ANSI C syntax The following are valid expressions 1 253 a string 1 4 3 2 gt a fn argl exp slot exp gt slot exp amp exp sizeof type b arg2 Saphira Software Manual Expressions typed at the command window and followed by a semicolon are evaluated and the result printed with the type of the result given in parentheses Pointer arithmetic is not implemented The and operators are not implemented The type of an expression is determined by the ty casting in the following cases gt In arithmetic operations and comparisons all pe of its components Colbert performs implicit type numbers are converted to floating point if any one of the components is floating point Pointers are converted to integers gt gt gt In logical operations floating point numbers 4 9 6 Variables Variables are defined using ANSI C syntax The predefined types or by a type imported with s fAdd and pointers are cast to integers In assignments the value to be assigned is cast to the type of the variable being assigned In function evaluation the arguments are cast according to the function prototype type of the variable is given by one of the five EvalStruct Pointers and pointers to pointers etc are legal but no sp
102. eliability of the communication channel Saphira Software Manual 2 It implements a set of checking functions for determining when the motion commands are finished 3 It has a position control mode which moves the robot a specified distance forward or backward Direct control of the two control channels translation and rotation is independent and commands to control them can be issued and will execute concurrently The direct motion functions require the state reflector to be operational that is the function sfInitBasicProcs must be called This is done automatically by sfStartup so the user need not call it explicitly void sfSetVelocity int vel void sfSetRVelocity int rvel Set the translational and rotational setpoints in the state reflector If the state reflector is active these setpoints are transferred to the robot Values for translational velocity are in mm sec for rotational velocity degrees sec void sfSetHeading int head void sfSetDHeading int dhead The first function sets the absolute heading setpoint in the state reflector The argument is in degrees from 0 to 359 The second function increments or decrements the heading setpoint The argument is in degrees from 180 to 180 If the state reflector is active the heading setpoint is transferred to the robot void sfSetPosition int dist void sfSetMaxVelocity int vel The first function sets the distance setpoint in the state reflector Argumen
103. ence and sends the results to the client via the server information packet Details about the configuration and firing sequence of the sonars are found in the robot s operation manual Use the sfCOMPOLLING command to change the polling sequence of the sonars sfRobotComStr sfCOMPOLLING str where str is a null terminated string of bytes at most 12 bytes long Each byte is 1 sonar number For example the string 001 002 001 006 starts the sonar polling sequence 0 1 0 5 Note that sonar numbers can be repeated If the string is empty all sonars are turned off Saphira Software Manual 8 Guide to the Saphira API This chapter details the current library of functions for development of a Saphira client Additional information about prototypes structures and variables can be found in the various header files in the handler include directory of your Saphira distribution Also study the sample source files in handler src apps as examples of working Saphira applications Most of these functions and variables are available in the Colbert evaluator Those that are not are indicated in the text 8 1 Saphira OS Functions Use the following functions to initialize configure and operate the Saphira OS see Section 2 for a summary of OS properties void sfStartup int async void sfStartup HANDLE hInst int cmdShow int async void sfPause int ms The first form is for UNIX systems the second for MS Windows When invoked
104. enter 113 sfPointDist 113 sfPointDistPoint 113 sfPointMove 113 sfPointNormalDist 113 sfPointNormalDistPoint 113 sfPointPhi 113 138 sfPointXo 113 sfPointXoPoint 113 sfPointYo 113 sfPointYoPoint 113 sfRemPoint 111 sfRobotOrigin 111 sfSetGlobalCoords 110 sfSetLocalCoords 110 sfSub2Angle 112 sfSubAngle 112 sfUnchangeVP 113 port names 91 types 91 port types and names 120 Predefined Behaviors 59 Procedural Reasoning System 10 Processes sfFindProcess 104 sfInitProcess 104 sfInterruptProcess 104 sfInterruptSelf 104 sfResumeProcess 104 sfSetProcessState 104 sfSuspendProcess 104 sfSuspendSelf 104 state values 102 window 20 Processes window 20 PSOS 85 88 pwd 33 README 66 Real World Interface Inc 1 recenter menu 28 Registration 10 12 93 98 115 robot configuration 92 rotate 34 RWI See Real World Interface Inc sample applications 33 demo act 33 direct act 33 packet act 33 Saphira API 95 See API Behaviors 11 53 See Behaviors colors 115 compiling clients 66 General description 1 Global Map Space GMS 10 maps 115 multiprocessing 102 Occupancy functions 107 See occupancy packet functions 120 See packet functions Path 4 processes 97 102 104 See Saphira processes Quick start 4 Representation of space 10 Robots 1 Servers 85 vision 123 Saphira behaviors 53 Saphira colors 115 SAPHIRA environment variable 3 4 66 68 69 120 saphira e
105. er s Interface API of calls to the Saphira library Programming details are in the following chapters of this manual 1 2 Colbert Robot Programming Language With Version 6 x Saphira has added a C like language Colbert for writing robot control programs With Colbert users can quickly write and debug complex control procedures called activities Activities have a finite state semantics that makes them particularly suited to representing procedural knowledge of sequences of action Activities can start and stop direct robot actions low level behaviors and other activities Activities are coordinated by the Colbert executive which supports concurrent processing of activities Colbert comes with a runtime evaluation environment in which users can interactively view their programs edit and re run them and link in additional standard C code Users may program interactively in Colbert which makes all of the Saphira API functions available in the runtime environment Future additions to Colbert include a compiler for efficient execution of debugged programs and multiple robot coordination 1 3 Behavior Compiler and Executive Saphira uses fuzzy control rules for implementing low level control programs or behaviors Behaviors are defined using standard C structures and functions To make writing and debugging behaviors easier Saphira has a behavior compiler that translates a simple fuzzy control rule syntax into the required C code As of Sa
106. er references 3 A write up of this event is in AI Magazine Spring 1997 for a summary see http www ai sri com konolige saphira aaai html 2 Saphira System Overview Saphira Client User micro tasks and activities State reflector Packet communications Synchronous micro tasking OS TTY or TCP IP connection Figure 2 1 Saphira System Architecture Blue areas represent routines in the Saphira library red routines are from the user The left hand side are all routines that get executed synchronously every 100 ms Additional user routines may also execute asynchronously as separate threads and share the same address space 2 1 1 Micro tasking OS The Saphira architectures are built on top of a synchronous interrupt driven OS Micro tasks are finite state machines FSMs that are registered with the OS Each 100 ms the OS cycles through all registered FSMs and performs one step in each of them Because these steps are performed at fixed time intervals all the FSMs operate synchronously that is they can depend on the state of the whole system being updated and stable before they are called it s not necessary to worry about state values changing while the FSM is executing FSMs can also take advantage of the fixed cycle time to provide precise timing delays which are often useful in robot control Because of the 100 ms cycle the architecture supports reactive control of the robot in response to rapidly changing enviro
107. erprocess communications channel on the same machine It is also possible to run multiple copies of the simulator on the same machine with different communication channels handy for class work or to have the simulator listen on a tty port or a TCP IP port on a remote machine If for some reason the client terminates abnormally the simulator can be disconnected using the Disconnect option from the Quit menu Disconnecting or quitting the simulator while the client is connected will cause the client to quit Once connected with a client the simulator displays a window of its activity A sample window is shown in Figure 3 1 The simulated robot is the circular icon in the center of the screen the straight lines are simulated world segments walls corridors rooms and so on A collection of segments a world may be defined in a simple text file see below and loaded from the simulator s Load Files menu 3 1 1 Listening on Other Ports The simulator listens on an interprocess communication channel for connections from a server In Unix systems this is a local Unix socket under Windows it is a mailbox Default names for these sockets are supplied by the simulator Only one simulator may be connected at a time to that socket or mailbox In some cases it is convenient to start up multiple copies of the simulator or for some reason the socket may be busy or unavailable In these cases the simulator can be started with an alternative socke
108. erredTurnDir This global variable controls the default direction of turn when the front is blocked Values are sf LEFTTURN or sfRIGHTTURN behavior sfStopCollision Slows the robot sharply to avoid immediate obstacles This behavior differs from sfAvoidCollision in that it doesn t turn the robot some other behavior must do that Takes three parameters listed in the Table 7 4 59 5 60 Behavioral Control Table 5 3 Stop collision behavior parameters Effect sfFLOAT Front sensitivity to obstacles Value from 0 5 not sensitive to 3 0 very sensitive sfFLOAT Side sensitivity to obstacles Value from 0 5 not sensitive to 3 0 very sensitive fF sfFLOAT Standoff Defines the avoidance bubble around the robot Value from flakey_radius at the robot to flakey_radius standoff standoff mm from the robot behavior sfKeepOff Gently steers the robot around and away from distant objects The behavior takes two parameters and uses the global variable sfPreferredTurnDir described in the Table 5 4 The priority for sfKeepOff should always be less than higher priority number than that for sfAvoidObstacle when they are invoked together Table 5 4 Keep off behavior parameters distant obstacles are detected Value in mm sec to 2 0 very sensitive sfPreferredTurnDir This global variable controls the default direction of turn when the front is blocked Values are sfLEFTTURN or sfRIGHTTUR
109. erver information packet informs the client about a number of the robot s operating parameters and readings using the order and data types shown in Table 7 5 Your client application may use the Saphira library function sEProcessClientPacket to parse the server information and deposit the results in various buffers of the state reflector See the section on the state reflector in the API reference for information about these structures 87 7 Saphira Servers Table 7 5 Saphira server information data packet minimum contents Exactly OxFA OxFB than 201 OxC9 Status sSfSTATUSSTOPPED Motors stopped sfSTATUSMOVING Xpos unsigned int 15 Is bits Wheel encoder integrated coordinates platform dependent units multiply by Ypos unsigned int 15 Is bits DistConvFactor in the parameter file to convert to mm roll over 3 m Th pos signed int Orientation in platform dependent units multiply by AngleConvFactor for degrees L vel signed int Wheel velocities respective Left and Right in platform dependent units R vel signed int multiply by VelConvFactor to convert to mm sec Battery charge in tenths of volts 2 bytes L and R Motor stall indicators Bumpers wnsigmedim o O Control signed int Setpoint of the server s angular position servo multiply by AngleConvFactor for degrees PTU Pulse width of position servo Sa bye o byte verbal sound clues Sonar byte Number of new sonar readings readings included
110. es the end result of combining the active behaviors according to their priority It is the summation that ultimately controls the robot server s actions 19 2 Saphira System Overview It s often useful to view an individual behavior s activity in more detail Individual behavior windows can be opened by shift clicking on the behavior name UNIX systems or left clicking just to the right of the name MS Windows Figure 2 6 shows a typical behavior window while active The invocation Keep Off Params Running un 100 0000 Priority 1 0 250000 Act Turn 0 2 2 Act Vel 0 2 Goal 0 0 Progress 1 0 Rules Obstacle Right 0 0 lt 5 0 Obstacle Left 0 0 gt 5 0 Obstacle Front 0 7 lt 5 0 Obstacle Cauti 0 5 177 1 Figure 2 6 Behavior window for Keep Off parameters of the window are in the upper left pointer parameters have their addresses printed The right hand side of the window shows the state variables of the behavior whether it s active or not activity levels and so on Finally at the bottom of the window the rules are printed showing their antecedent values and control sets The format of the rules is Name Antecedent Direction Value The antecedent value determines how strongly the rule applies The direction is a single character gt or lt for right or left turn or for speed up or slow down The value indicates the desired control signal a left turn of 5 0 degrees for example
111. ese functions automatically add the artifact to the pointlist So if you want to create a point and add it to the pointlist use the sfPOINT type here instead of the sfCreateXPoint functions Not all types use all of the parameters length and width are ignored for sfEPOINT length is ignored for sfDOOR and sf JUNCTION and width is ignored for sfWALL In general the x y th coordinates are for a point in the middle of the artifact The following figure shows the geometry of the constructed artifacts 109 8 Saphira API Guide length length A A dth idth wi wi x y th x y th Corridor Lane length I n o gt width width x y th x y th x y th Wall 7 7 Door Junction Figure 8 4 Geometry of artifact types The defining point for the artifact is shown as a vector with a circle at the origin Artifacts are most often used in constructing maps for the robot and registering it based on sensor readings see Section 8 10 8 7 3 Geometry Functions Saphira provides a set of functions to manipulate the geometric parameters of artifacts These functions typically work on the local coordinates of the artifact To update an artifact properly after changing its local coordinates you should call the sfSetGlobalCoords function float sfNormAngle float ang float sfNorm2Angle float ang float sfNorm3Angle float ang float sfAddAngle float al float a2 float sfSubAngle float al float a2 float sfAdd2Angle float al float a2 float sfSu
112. esent objects in the environment or internal structures such as paths A collection of objects such as corridors doors and rooms can be grouped together into a map and saved for later use The GMS is not displayed as a separate structure but its artifacts appear in the LPS display window 2 2 2 Direct Motion Control The simplest method of controlling the robot is to modify the robot motion setpoints in the state reflector A motion setpoint is a value for a control variable that the motion controller on the robot will try to achieve For example one of the motion setpoints is forward velocity Setting this in the state reflector will cause the communications routines to reflect its value to the robot whose onboard controllers will then try to keep the robot going at the required velocity There are two direct motion channels for rotation and translation of the robot Any combination of velocity or position setpoints may be used for these channels see Section 8 4 2 2 3 Behaviors and Fuzzy Control For more complicated motion control Saphira provides a facility for implementing behaviors as sets of fuzzy control rules Behaviors have a priority activity level and other well defined state variables that mediate their interaction with other behaviors and with their invoking routines For example a routine can check whether a behavior has achieved its goal or not by checking the appropriate behavior state variable There are several majo
113. essClientPacket int type sfProcessClientPacket parses a client packet into the sfRobot structure and sonar buffers Typically a client will call sfWaitClientPacket or sfHaveClientPacket to be sure a packet is waiting to be parsed The argument to sfProcessClientPacket is a byte the type of the packet This byte can be read using sfReadClientByte By examining this byte the client can determine if it wishes to parse the packet itself or send it on to sEProcessClientPacket int sfClientBytes void int sfReadClientByte void int sfReadClientSint void int sfReadClientUsint void int sfReadClientWord void char sfReadClientString void These functions return the contents of packets if you want to dissect them yourself rather than using sfProcessClientPacket sfClientBytes returns the number of bytes remaining in the current packet The other functions return objects from the packet bytes small integers 2 bytes unsigned small integers 2 bytes words 4 bytes and null terminated strings 119 Saphira Software Manual 9 Saphira Vision Current versions of Saphira have both generic vision support and explicit support of the Fast Track Vision System FTVS which is available as an option for the Pioneer 1 Mobile Robot The FTVS is a product from Newton Labs Inc adapted for Pioneer The generic product name is the Cognachrome Vision System Details about the system manuals and development libraries can be found at Newto
114. etenv the SAPHIRA path parameter The Saphira client window will appear with a graphics display of the robot internals a text information display and an interaction window You can type help in the interaction window for a list of command classes that can be queried for further information Have a robot server or the simulator readied for a Saphira connection For example execute the saphira ver61 bin pioneer exe robot simulator on the same computer or simply turn on your Pioneer robot and connect its serial port or radio modems to your basestation computer running the Saphira demonstration program In the Saphira interaction window type connect serial to connect on the standard serial port If your radio modem is connected to a different serial port use connect serial lt port gt where lt port gt is the name of the serial port e g dev ttyS1 or COM2 If you re using the simulator you can connect using connect local which opens a local port to the simulator and starts things up You should have started the simulator first by executing pioneer exe from the same bin directory Saphira Software Manual Bxx users can connect using either a TCP IP connection or a local connection typically the Saphira server will start listening on a local port Run the Saphira client on the same machine as the server and use connect local to make a connection You can also connect via the Connect menu on the main Saphira window Once you in
115. fInitProcess API call see Section 8 5 Both the micro task function and the instantiation name given by the init function are described here The instantiation name is used to refer to the running micro task and is shown in the Function Processes window To remove a micro task with instantiation name iname you can type remove iname in the interaction window or an activity or use sfRemoveTask iname from C code void sfInitBasicProcs void Starts up a set of basic communication display motor and sensor control processes Among other activities these processes implement the client state reflector The processes invoked are Function Description pulse_proc pulse communication pulse every second motor_proc motor coordinates keyboard and behavior motor commands clamp_proc clamp rotates the world around the robot sonar_proc sonar adds new sonar readings to the sonar buffer wake_proc wake draws a wake of the robot s motion draw_proc draw updates Saphira display window process_waiting_packets packets parses information packets from robot server Drawing wake and clamping processes are affected by variables that users can set from Saphira s main window s Display menu sfInitBasicProcs is invoked by sfStartup so the user should not have to call this function Not available in Colbert void sfInitControlProcs void Starts up a process for evaluating all active behaviors If you want to run without using the fuzzy beh
116. fected However you can t rely absolutely on this behavior as sometimes both motors will stall even when the obstacle is on one side or the other Motor stall information is returned in the bumpers field 97 8 98 Saphira API Guide int sfStalledMotor int which Return 1 if the motor which is stalled and 0 if it isn t The argument which is sELEFT or sfERIGHT 8 3 2 Sonar buckets The current range reading of a sonar sensors is held in an sdata structure defined below The structures for all the sonars are in an array called sbucket e g sbucket 2 is the sdata structure for sonar number 2 Sonars start at number 0 This variable is not defined in Colbert which doesn t have arrays instead use the convenience function sfSonarBucket Fields in the sdata structure indicate the robot s position when the sonar was fired the range of the sonar reading and the position in robot coordinates of the point on the sonar axis at the range of the reading The field snew is set to OXFFFF when a new reading is received the client program can poll this field to ascertain if the reading is new and set it to 0 to indicate that it has been read A value of 5000 for the sonar range indicates that no echo was received after the sonar fired and waited for a return Some convenience functions for accessing current sonar readings are described below Sonar readings are accumulated over short periods of time into a set of buffers in the
117. ge and the end of the range straight_up returns 0 0 if the value is below the range and 1 0 if it is above and interpolates linearly between them Figure 5 1 down_straight is the opposite values below the start return 1 0 above 0 0 and intermediate ones are linearly interpolated 70 0 30 0 Figure 5 1 The straight up function The functions f_smaller f_greater and f_eq compare two numbers and return a fuzzy value based on whether the first is smaller than larger than or equal to the second The delta argument is the range over which the fuzzy value will vary 5 6 2 Fuzzy variable combination functions Combine fuzzy variables by using the T norm functions max for disjunction min for conjunction and unary minus for negation The utility functions f_and f_or and f_not are provided to implement these operators float f_not float x float f_and float x float y float f_or float x float y 5 7 Implementing Behaviors For reference we include descriptions of the parts of behaviors defined using structures and functions in C If you use the behavior syntax to write behaviors you generally won t have to worry about these details 57 5 Behavioral Control 5 7 1 Input parameters The variables that constitute the input to the behavior are contained in a structure called beh_params Each parameter is either a floating point number or a pointer pointers are used for complex variables such as goal points The
118. havior parameters Table 5 8 Follow corridor behavior parameters Table 5 9 Follow door behavior parameters Table 5 10 Turn to parameters Table 7 1 Main elements of PSOS communication packet protocol Table 7 2 Communication packet data types Table 7 3 Client command communication packet Table 7 4 PSOS 4 2 supported client commands Table 7 5 Saphira server information data packet minimum contents Table 7 6 Port types and names for clinet server connections Table 7 7 Robot configuration information Table 7 8 Server motion command types Table 7 9 Motion command arguments Table 8 1 Optional states for various Saphira display functions Table 8 2 Definition of the sfRobot structure Table 8 3 Saphira multiprocessing reserved process state values Table 8 4 Artifact creation types Table 8 5 Saphira colors Saphira Software Manual 18 27 59 60 60 60 61 61 62 62 62 84 84 86 87 88 89 90 90 91 94 97 100 109 113 List of Figures Figure 2 1 Saphira System Architecture 8 Figure 2 2 Saphira Control Architecture 10 Figure 2 3 Saphira client LPS in local mode 15 Figure 2 4 Saphira Behaviors window Linux Motif version 19 Figure 2 5 Keep Off behavior display expanded 19 Figure 2 6 Behavior window for Keep Off 20 Figure 2 7 A sample Saphira Processes window 21 Figure 2 8 An example Saphira Activities window 22 Figure 3 1 A sample window of the simulator 26 Figure 4 1 A direct motion application in the Colbert lang
119. he Section 4 10 1 The format of sfAddEvalVar is sfAddEvalVar char name int type fvalue amp cvar name is the name of the variable as seen by Colbert cvar is the variable being made available Note that a pointer to the variable is required and it is cast to the type fvalue This is so the Colbert Saphira Software Manual executive can change the value of the variable The type of the variable t ype is the C index of a Colbert type see Section 4 9 3 The predefined types are ME sfINT float sfFLOAT void sfVOID string sfSTRING act SEACHEN behavior sfBEHAVIOR vonoi Sibert In addition pointers to types can be defined with the function sf TypeRef int type For example to define a pointer to an integer use sfTypeRef sfINT The function sfTypeDeref performs the inverse operation giving the type of the reference of a pointer but this is less useful in defining argument types A C variable made available in Colbert is both accessed and changed by the appropriate Colbert expression For example sfRobot is available in the bin saphira executable having been defined by sfAddEvalVar sfRobot sfSrobot fvalue amp sfRobot The type sfSrobot is also defined see the next section The robot s current global position is available in Colbert as the members sfRobot ax sfRobot ay and sfRobot ath For example the following Colbert statement will increment the global x position by 1 mete
120. he data do not decay as rapidly some commands are not overly time sensitive such as those that perform housekeeping functions like changing the sonar polling sequence It would be useful to have a reliable packet protocol for these operations and we are considering this for a future release of Saphira server interface In the meantime the Saphira client server interface provides a simple means for dealing with ignored command packets Most of the client commands alter state variables in the server By examining those values in the server information packet client software may detect ignored commands and reissue them until achieving the correct state 7 2 Client Commands Saphira client server interface implements a structured command format for receiving and responding to directions from the client for control and operation of the robot or its simulator You may send client commands to the robot at a maximum rate of once every 100 milliseconds The client must send a command at least once every two seconds otherwise the server will stop the robot s onboard drives The client command is comprised of a one byte command number optionally followed by if required by the command a one byte description of the argument type and the argument To work of course the client command and its optional argument must be included as the data component of a client communication packet Table 7 3 also see earlier sections of this chapter Table 7 4 contai
121. he sfRobot structure are reflected from the robot server back to the client providing information about the robot s state In this way it is possible to tell if a command has been executed For example the digout put field reflects the actual value of the digital output bits set on the robot The interpretation of some of the values in the structure is robot dependent e g the bumpers field reflects motor stall information for the Pioneer robots The Saphira library provides some convenience functions for interpreting these fields see the following subsections This variable is defined in Colbert as well as the robot structure and most of the fields are available Table 8 2 Definition of the sfRobot structure x y th mm mm degrees robot s location in robot coords Ce eee ee leftv rightv left and right wheel velocities int status robot status STATUS_STOPPED robot stopped STATUS_MOVING robot moving STATUS_NOT_CONNECTED client not connected STATUS_NO_HIGH_POWER robot motors stalled motor_packet_count counts per second packet communication information sonar_packet_count vision_packet_count type help robot for a list of fields 8 3 1 Motor Stall Function On Pioneer class robots the motors stall if the robot encounters an obstacle Each motor can stall independently and this can yield information about where the obstacle is e g if the right motor stalls then the right wheel or right side of the robot is af
122. he start command void sfStartBehavior behavior b char iname int timeout int priority int suspended The first argument of the sfStartBehavior function is a pointer to the behavior structure as defined below The second is the instance name of the behavior In Saphira 6 x behaviors are also micro tasks and so they are referred to by their instance name The timeout value is the number of 100 ms cycles the behavior will run use 0 for no timeout The next argument is the priority of the behavior closure relative to others Lower values get higher priority 0 is the highest priority and should be used for the most important emergency maneuvers such as collision avoidance Saphira treats all behaviors with the same priority equally in terms of competing for control of the robot ones with larger priority numbers lower priority are suppressed by activity of higher priority behaviors The suspended argument is 0 if the closure is started in an active state and 1 if itis suspended A closure that is suspended is present but not active and does not affect the robot s movements The suspended state of a behavior can be changed by using Colbert signals Section 4 8 3 or with the library function sfSetTaskState Section 8 5 3 The remaining arguments to this function set up the parameters of the closure They must be the same number and have the same type as the parameters specified in the behavior definition Here is an example i
123. hich it checks for objects in front for a motor stall and for the state of the patrol activity If it determines that patrol has timed out or if a motor stalls indicating the robot ran into something immovable then approach exits in a failure state The activity executive keeps track of the dependencies among activities in this case since approach called patrol exiting approach automatically exits patrol Thus if the motor stalls all activity started by approach will be suspended If on the other hand approach determines that there is an object less than 2 meters in front by calling the perceptual routine sfOb3jInFront which returns the distance to the nearest object then it suspends the patrol activity and moves to within 20 cm of the object The patrol activity must be suspended otherwise the move action will conflict with the actions being issued by patrol After the robot moves near the object the approach activity exits with the success state Saphira Software Manual In this example two activities execute concurrently and coordination is achieved by signals that are sent between them Activities can examine each others state and take appropriate action As the monitoring activity approach has the responsibility of checking the state of patrol to see if it has timed out and also of checking for other conditions that would cause the suspension of patrol and the initiation of new activities Finally if approach is itself pa
124. icating the robot s front in mm from the front end Sorry about the name RobotWidth float Width of nonholonomic robot in mm RobotLength float Lenght of nonholonomic robot in mm MaxVelocity float Maximum velocity of the robot in mm sec MaxRVelocity float Maxcimum rotational velocity of the robot in degrees sec MaxAcceleration float Maximum acceleration of the robot in mm sec sec Class string Robot class pioneer b14 b21 Not case sensitive Useful only for the simulator which will assume this robot personality The client gets this info from the autoconfiguration packet Subclass string Robot subclass For the Pioneer indicates the type of controller and body combination Values are psos41m psos41x or pionat Not case sensitive Useful only for the simulator as for the Class parameter Name string Robot name Useful only for the simulator as for the Class 126 SonarNum SonarUnit FrontBuffer SideBuffer integer n x y th integer integer Saphira Software Manual parameter Number of active sonars Description sonar unit n The x y th arguments describe the pose of the sonar on the robot body relative to the robot center There should be one such entry for each active sonar unit Used by both the simulator and client Number of front sonar readings to keep Higher values mean the robot will be more sensitive to obstacles but slower to get rid of moving obstacle readings Number of side sonar r
125. iebuaeieakein A sfStartup 1 start up the Saphira 0S7 and then keep going while sfIsConnected sfPause 0 wait until connected sfSetRVelocity 100 in mm sec on each wheel sfPause 4000 sfSetRVelocity 0 sfPause 4000 for i 0 i lt 280 i 20 printf Turn d degrees n i sfSetDHeading i turn i degrees ce while sfDoneHeading 10 sfPause 0 wait till we re within 10 degrees sfSetDHeading i turn i degrees c while sfDoneHeading 10 sfPause 0 wait till we re within 10 degrees sfSetVelocity 300 move forward at 300 mm sec oie GE O 2 lt il s gsr Ouest Xe oes moe SRO Oitmaa xims PROMO may ny sfPause 1000 DON T USE SLEEP sfSetDHeading 10 sfSetVelocity 0 move forward at 300 mm sec sfPause 4000 sfDisconnectFromRobot Le GIVES CCIM 5 o 4 7 7 Saphira Software Manual 6 3 7 The Nowin Client Like the async client this client makes use of the asynchronous execution of user routines But instead of starting up the Saphira interface window it just connects to the robot by a function call and then starts executing direct motion commands If this client is linked with the non window library s fx then no interface window will appear In MS Windows you specify a console application instead of window application and use the main function instead of WinMain In sfStartup you must s
126. ient are shown in Figure 7 1 Client Application Communxations Server __ fy Client Serial reer Local Fipe Communication Packets Velocity amp Positi Sonar osition Angle Integration oTa Controls Schedules PMI Encoder Control Counting Robot Specific Functions r o Control Figure 7 1 Saphira client robot server architecture High level robotics applications developers do not need to know many details about a particular robot server because the Saphira client insulates them from this lowest level of control Some of you however may want to write your own robotics control and reactive planning programs or just would like to have a closer programming relationship with your robot This chapter explains how to communicate with your robot via the Saphira client server interface The functions and commands of course are supported in the Saphira C libraries that came with your robot but not every robot supports all commands Please consult your robot s operation manual or Saphira supplement for those details 7 1 Communication Packet Protocol The Saphira mediated robot or its simulator communicates with a client application using a special packet protocol It is a bit stream consisting of four main elements Table 7 1 a two byte header a one byte count of the number of data and checksum bytes in the packet a client command including arguments or a server information data block and ending with a two byte checksum 8
127. ient moves the robot back and forth along a two meter line The direct client comes in two forms a loadable Colbert language file direct act anda standalone native C code file We encourage you to use the Colbert language as it s more understandable and easier to work with and modify The two activities patrol and square are straightforward realizations of the robot routines in Colbert A third activity aa turns on tracing and sequences the two activities Statements at the end set global mode on the display and initiate the aa activity Note that you should be connected to the robot before loading this file otherwise there will be an error when the direct actions are attempted JAHRE HEE EHH EE HEE HEH E E E EE EE E E E E E E EH direct act exercising the direct motion API AEH EH HE HEE HE EH FE AE AE EE EE HE HE EH EEE E E E E HE HH wif act patrol int a WO back incl torta Ve tines while a a ail move 1000 CANE S move 1000 CUNEO LEO 5 act square move in a square int a a 4 75 6 Creating Clients and Load Files call them sequentially patrol patrol 4 square square sfSetDisplayState sfGLOBAL 1 put display into global coords start aa start up the toplevel activity The standalone client direct c has the same form as the previous ones In the main function startup and connection callbacks are registered and then the Saphira s
128. if their viewable slot is greater than 0 8 8 Sensor Interpretation Besides the occupancy functions the Saphira library includes functions for analyzing a sequence of sonar readings and constructing artifacts that correspond to objects in the robot s environment We are gradually making these internal functions available to users as we work on tutorial materials illustrating their utility Currently the only interpretation routines are for wall hypotheses wall sfLeftWallHyp wall sfRightWallHyp These wall structures contain the current wall hypothesis on the left and right sides of the robot using the side sonar buffers If a wall structure is found then the viewable flag is set non zero in the structure and the wall dimensions are updated to reflect the sensor readings In order for wall hypotheses to be found the wall finding routines must be invoked with sfInitInterpretationProcs 8 9 Drawing and Color Functions Use the following commands function to display custom lines and rectangles on the screen and to control the screen colors All arguments are in millimeters in the global LPS coordinate system void sfDrawVector float x1 float yl float x2 float y2 void sfDrawRect float x float y float dx float dy void sfDrawCenteredRect float x float y float w float h sfDrawVector draws a line from x1 yltox2 y2 This line is in global coordinates To draw a rectangle use the function sf DrawCenteredRect or sfDrawRect Th
129. imulator server on a remote machine set server lt netaddr gt Sets or returns the remote server net address sfComServer These addresses may be network names e g flakey ai sri com or numbers e g 128 18 65 12 disconnect Disconnects from the currently connected robot server exit Exits from the Saphira executable disconnecting from any robot server first 4 7 Direct Motion Commands The evaluator provides a set of direct motion commands that can move and rotate the robot These commands are Colbert language statements and can be typed in the interaction window The direct motion commands are not C functions and do not return any value They also have a syntax for specifying a timeout value and a non blocking mode The general form of a direct motion command is command int arg timeout n noblock where timeout n specifies a time in 100 ms increments for the command to complete and noblock means that the command will be executed without blocking i e control will continue with the next statement Some motion commands are implicitly nonblocking speed and rotate In the interaction window all direct motion commands are issued nonblocking whether or not noblock is specified Nonblocking motion commands can be checked for completion with the sfDonePosition and sfDoneHeading commands More information on direct motion control as well as C library functions can be found in Section 8 4 These API calls are availabl
130. imulator or robot server the communication connection will fail and a message describing the problem will appear in Saphira s main window information area Some typical causes for failure with either the simulator or the actual robot and their solutions include v Bxx robots Make sure the physical robot s Saphira compatible server software is properly installed and running and that no other Saphira client is connected to it v Make sure the simulator is running and there is no other Saphira client or simulator server running on the same machine v Inrare cases the communications pipe may be blocked This can occur if either the server or client exits abnormally from a previous connection without shutting it down properly Try deleting the pipe file and starting again If this doesn t work the only remedy is to reboot the machine v Make sure that the communications tether or radio modem is plugged into the correct serial port with the correct cable Remove the serial tether cable from the robot s serial port if you use the radio modem v Make sure the client radio modem is within range of robot is on the correct channel and has a strong link signal v Make sure the serial port is not in use by another application Once connected the Saphira client will display information about the state of the robot and allow you to command the robot from the menu and keyboard 2 3 3 LPS Display The Saphira client s display contains mo
131. in information packet readings follow Sonar num Sonar number Sonar range Sonar reading multiply by RangeConvFactor for mm User input timer reading User analog input reading User digital input pins User Output ser digital output pins Checksum see previous section eS 88 Saphira Software Manual In future versions server information packets may contain additional appended data fields To remain compatible have your client application accept the entire data packet even though it may use only a few selected fields 7 4 Start Up and Shut Down Before exerting any control a client application must first establish a connection to the robot server via an RS 232 serial link 9 600 baud a interprocess connection UNIX pipe for example or MS Windows mailslot or TCP IP network Table 7 6 Port types and names for clinet server connections Over that established communication link the client then sends commands to and receives back operating information from the server Table 7 6 Port types and names for clinet server connections Port types sf COMZ tty port 2 dev ttyb or dev cual for UNIX COM2 for MSW printer for Mac SAPHIRA_SERVER hostname IP address of server not for Pioneer sfCOM1 tty port 1 dev ttya or dev cua0 for UNIX COM1 for MSW modem for Mac 7 4 1 Synchronization sfCOMSYNC When first started the Saphira aware server including the simulator is in a wa
132. ing accordingly trying to keep heading towards the control point 2 3 3 4 Velocity vectors Two lines emanating from the center of the robot icon indicate the translational and rotational velocity of the robot as returned from the robot server The length of each vector is directly proportional to the velocity Also each vector points in the respective direction of motion For example when the robot is turning clockwise as in Figure 6 3 the rotational vector points to the right 2 3 3 5 Obstacle sensitivity areas Several obstacle avoidance behaviors temporarily draw large open rectangles in the LPS indicating detected obstacles that they are actively avoiding Obstacle avoidance rectangles appear just ahead and to the sides of the robot in robot centric coordinates In the global view these rectangles do not appear in the proper place near the robot icon Saphira Software Manual as fa Saphira Pioneer server on simulator Connect Files Grow Shrink Display Sonars Functions LPS Values Invoking activity BumpAndGo In corridor 1 100001 Found door 1 on right In corridor 1 121 Found door 1 on left oll Figure 2 3 Saphira client LPS in local mode The corridor and door artifacts are the robot s internal map Small squares are sonar readings The larger rectangles are sensitivity areas used by the obstacle avoidance behaviors The lines drawn at the center of the robot are angular and forward velocity The small rectangle
133. ing system dependent Return 0 if you don t handle the keypress return if you do particularly to over ride any of Saphira s built in key processing routines see Table 8 1 Not available in Colbert Saphira Software Manual void sfButtonProcFn int fn int myButtonFn int x int y int b int m int sfLeftButton sfMiddleButton sfRightButton int sfShiftMask sfControlMask sfAltMask float sfScreenToWorldX int x int y float sfScreenToWorldY int x int y The sfButtonProcFn registers an optional user button process callback with the prototype of myButtonFn It is called by Saphira whenever the user clicks the mouse when the main Saphira window is active The x and y arguments are the screen position of the cursor b is the mouse button with the values sfButtonLeft sfButtonRight and sfButtonMiddle The shift mask argument m is an integer that has bits set indicating which modifier keys were pressed Return O if you don t handle the mouse click return if you do to over ride any of Saphira s built in mouse processing routines To convert from screen to global robot coordinates use the sf ScreenToWorld functions which return their answers in mm Not available in Colbert 8 2 Predefined Saphira Micro tasks We ve provided a variety of predefined Saphira micro tasks for control of the robot You may initiate these micro task sets using the API functions described here or invoke them individually using the s
134. ion area appears at the left of the window the menu bar at the top and a text interaction window at the bottom 2 3 1 Loading an Activity File The Saphira client in bin saphira has only a bare set of micro tasks loaded you can see the source code in handler src apps saphira c The capabilities of the client are increased by loading in Colbert files which contain activity schemas and invocations of API functions A sample activity file colbert demo act is used as an example in the rest of this section the act extension signifies a Colbert language file When the saphira client starts it looks for the file init act in the current load directory which by default is SAPHIRA colbert The initialization file loads the demonstration file demo act To load your own init file you can either change the load directory by setting the environment variable SAPHIRA_LOAD or change the init act file in the colbert directory The demo act file defines several activity schemas then invokes them and some pre defined behaviors for obstacle avoidance Please refer to the code for more details 2 3 2 Connecting to a Robot As we mentioned earlier connecting Saphira with either the simulator or the actual robot is similar First if you are using the simulator make sure that the correct robot parameters are loaded the simulator defaults to using Pioneer parameters see Chapter 3 Otherwise the Saphira client auto detects the robot Saphira Soft
135. ion for deleting micro tasks suspend it if it is no longer needed sfprocess sfFindProcess char name The sf FindProcess function searches for and returns the first micro task instance it finds with the name name A micro task instance pointer is returned if successful else NULL void sfSetProcessState sfprocess p int state void sfSuspendProcess sfprocess p int n void sfSuspendTask char iname int n void sfSuspendSelf int n void sfInterruptProcess sfprocess p void sfInterruptTask char iname void sfInterruptSelf void void sfResumeProcess sfprocess p void sfResumeTask char iname void sfRemoveProcess sfprocess p void sfRemoveSelf void void sfRemoveTask char iname The sfSetProcessState function sets the state of micro task instance p to state The argument p must be a valid micro task instance pointer returned from sfFindProcess or sfInitProcess The other functions are particular calls to sf Set ProcessState The other functions are convenience functions for signaling micro tasks to set certain states 8 5 4 Invoking Behaviors Behavior micro tasks can be invoked from Colbert with the st art command or from C code with the following function 102 Saphira Software Manual sfStartBehavior behavior b char in int tout int pri int suspend The sfStartBehavior function instantiates a behavior micro task using the behavior schema b The instantiation name is in and the priority
136. ira which means the toplevel Saphira directory will be c saphira ver 61 This is the directory that the sample MSVC projects assume For all systems upon decompression a hierarchy of folders and files will appear inside a newly established version related Saphira directory ver 61 for Saphira version 6 1 for example The distribution directory for the Windows95 NT Saphira version 6 1 looks like this ver61 bin saphira exe direct exe pioneer exe btech exe bgram Si eda 1 msvert40 dll colbert handler src samples apps basic behavior beh include obj maps worlds params readme update license Saphira Colbert runtime application direct motion control example simulator Pioneer Fast Track Vision system demo behavior grammar compiler DLL library for MS 95 NT required MS Windows DLL Colbert language samples tutorial examples application source examples behavior examples header files UNIX library files Saphira example maps simulator world files parameters for different robots explanation text file comparison of versions operation license The uncompressed Saphira software typically requires 20 megabytes of hard disk drive storage space IMPORTANT NOTICE All Saphira operations require that the environment variable SAPHIRA be set to the top level directory e g usr local saphira ver61 on a Unix system note there is no final slash or c saphira ver61 onan MS Windows system Ifyou do
137. is the artifact id and it must be a positive integer The first three coordinates are the X Y and O position of the center of the 113 8 Saphira API Guide corridor in millimeters and degrees The fourth coordinate is the length of the corridor and the fifth is the width DOOR entries are defined in much the same way except that the third coordinate is the direction of the normal of the door which is useful for going in an out The fourth coordinate is the width of the door JUNCTION entries are like doors but delimit where corridors meet T junctions should have three junction artifacts and X junctions four It s not necessary to put in any junctions but they can be useful in keeping the robot registered see below The WALL entry does not have an id The first two coordinates are the X Y position of the center of the wall the third is the direction of the wall and the fourth is its length Wall segments are used where a corridor is not appropriate the walls of rooms or for large open areas for example The map file when loaded into a Saphira client using the Files Load Map menu or the function sfLoadMapF ile creates the artifact structure shown in Figure 8 5 For illustration the defining point of the artifact is also shown as a small circle with a vector These points will not appear in the Saphira window 0 0 Y 4 0 3 0 2 0 1 0 Figure 8 5 Sample map created from the map file above as shown in a Saphira client
138. it state listening for communication packets over its designated port See your robot operating manual for details about your robot s servers To establish a connection the client application sends a series of three synchronization packets through the host communication port sfSYNCO sfSYNCJ and sfSYNC2 in succession The server responds to each forming a succession of identical synchronization packets The client should listen for the returned packets and only issue the next synchronization packet after it has received the echo A string may be used for unusual port names if there is a serial communications card with extra tty ports for instance With Macintosh it s best to use the modem port if it s available rather than the printer port 7 4 2 Autoconfiguration The Saphira aware servers PSOS v4 1 or later for example send configuration information back to the client in the last sync packet sfSYNC2 After the sync byte there are 3 null terminated strings that represent the robot name robot class and robot subclass Table 7 7 You can read these strings with the library function sfReadClientString The following table shows what these strings are for different robots 89 7 Saphira Servers Table 7 7 Robot configuration information Name string Given name for Pioneer class robots Computer name for Bxx class robots Simulator for the simulator Class string Pioneer B14 or B21 Subclass
139. ith incompatibilities between MSVC and Borland LIB files 63 6 64 Creating Clients and Load Files 6 2 Compiling and Linking C Source Files To compile a loadable shared object file or Saphira client you must have installed the Saphira distribution according to the directions in the readme file In particular the environment variable SAPHIRA must be set to the top level of the distribution we recommend usr local saphira ver 1 ina UNIX system for example Once the Saphira distribution is installed there are three steps to creating a client or a shared object file 1 Write a C or C program containing your code including calls to Saphira library functions 2 Compile the program to produce an object file 3 Link the object file together with the relevant Saphira library to create an executable or shared object file As of Saphira 6 0 all the Saphira library routines are contained in a shared library In MS Windows this is sf d11 in UNIX systems it is the shared library Libsf so 6 x y where x and y are the major and minor versions of Saphira The symbolic link 1ibsf so points to the current shared object library In MS Windows shared libraries DLLs cannot be relinked unless no application is using them If you have loaded a DLL then make changes to the source code and try to relink it you will get an error saying that the DLL file is busy The unload command can be used to unload the DLL from Saphira so the link can
140. itiate the connection the Saphira client and robot server perform a synchronization routine and if successful will establish a connection We provide a number of clues on both the client and server so you can follow the synchronization process Success is distinct The Saphira main window comes alive with sonar readings and the robot s sonars begin a rhythmic audible ticking We detail Saphira client operation in the following chapter For now we leave it to you to find the manual drive keys and take your robot for a joyride hints arrows move and the spacebar stops the motors The demonstration Colbert program colbert demo act is loaded automatically in the sample application and has more activities you can try out by starting them from the Function Activities window 1 8 Additional Resources Every new Saphira customer gets three additional and valuable resources a private account on RWI s Internet server for download of Saphira software updates and manuals the opportunity to register on one or more of private robotics newsgroups and email access to the Saphira support team 1 8 1 FTP Software Archive RWI has a server connected full time to the Internet where customers may obtain Saphira software and support materials Access is restricted to RWI ActivMedia customers including Pioneer 1 Bxx and ATRV 1 owners The RWI server name is ftp rwii com To gain access use the username and password that are written on the Regist
141. ities in the sample Saphira client Some of these are wrappers for a behavior that is their sole purpose is to control a single behavior The reason for this is to provide behaviors with the same facilities as activities e g timeouts signaling and hierarchical invocation see Section 4 8 3 21 2 Saphira System Overview The activity BumpAndGo is an example of an activity that produces direct action It waits until the robot bumps into something and its motors stall out then it turns off all behavior output and maneuvers the robot in a short back and turn sequence to get it out of the stall This activity is traced so you ll see the results of its evaluated statements printed in the interaction area Beware it s hard to make the robot run into something unless you turn off the obstacle avoidance behaviors Another activity follow a found corridor has the robot find and follow corridors The activity monitors the robot environment until it detects a corridor then starts a sub activity a behavior that projects a path for the robot down the middle of the corridor Intention follow a found corridor Follow it Figure 2 8 An example Saphira Activities window 2 3 11 System Environment Variables Several environment variables can be set to control defaults in Saphira clients This is a complete list of them and their effects In MS Windows environment variables are set in AUTOEXEC BAT or via the user profiles Windows N
142. ity is invoked by the scheduler Typically the update block is used to set the values of variables to reflect some change in the state of the robot goto labels and iteration are illegal in the update statement Body statements are executed in accordance with the finite state machine semantics described in Section 4 8 2 Labels are allowed only at the top level of body statements Some special labels indicate places to start execution on exceptional conditions oninterrupt Branch location for an interrupt signal onresume Branch location when an activity resumes because it was sent an sfRESUME signal 4 9 13 Direct actions Direct actions are statements that result in robot motion These statements may appear anywhere a statement is allowed in an activity schema The general form is command int arg timeout n noblock where timeout n specifies a time in 100 ms increments for the command to complete and noblock means that the command will be executed without blocking i e control will continue with the next statement Some motion commands are implicitly nonblocking speed and rotate move int mm Move the robot mm millimeters forward positive or backwards negative Blocking turn int deg Turn the robot deg degrees clockwise negative or counter clockwise positive degrees from the current heading Blocking turnto int deg Turn the robot to the heading deg degrees Positive values are counter clockwise
143. ive front two vr 7 Sonar parameters SonarNum N is number of sonars SonarUnit I X Y TH is unit I 0 to N 1 description X Y are position of sonar in mm TH is bearing in degrees 125 10 Parameter Files r RangeConvFactor 0 1734 sonar range mm per 2 usec tick vr SonarNum 7 X vr ver SonarUnit SonarUnit SonarUnit SonarUnit SonarUnit SonarUnit SonarUnit SonarUnit 100 120 130 130 130 120 100 100 90 0 0 0 ANDO BWNEF CO Number of readings to keep in circular buffers FrontBuffer 20 SideBuffer 40 Floating point parameters can be in any standard format and do not require a decimal point Integer parameters may not have a decimal point Strings are any sequence of non space characters Parameter Type Description AngleConvFactor float Converts from robot angle units 4096 per revolution to radians VelConvFactor float Converts from robot velocity units to mm sec DistConvFactor float Converts from robot distance units to mm DiffConvFactor float Converts from robot angular velocity to rads sec RangeConvFactor float Converts from robot sonar range units to mm Holonomic integer Value of 1 says the robot is holonomic can turn in place value of 0 says it is nonholonomic front wheel steering Holonomic robot icon is octagonal nonholonomic is rectangular RobotRadius float Radius of holonomic robot in mm RobotDiagonal float Placement of the horizontal bar ind
144. ke a doorway To register the robot as it moves in Corridor 1 you would have to put in the other two junctions at right angles to Junction 5 115 8 Saphira API Guide 8 10 3 Map Element Creation A by product of the registration micro task is that sometimes a corridor or doorway is found that does not match any map artifact In this case Saphira will by default create a new artifact and add it to the map To turn off this feature set the variable add_new_features to FALSE In finding corridors Saphira by default attempts to align them on 90 degree angles which is typical for a office environments To turn off this feature set the variable snap_to_right_angle_grid to FALSE Map elements can also be created by hand using the artifact creation functions of Section 8 7 8 11 File Loading Functions This section describes functions for loading Colbert files shared object files parameter files and simulator world files Map file loading functions can be found in the previous section int sfLoadEvalFile char name char sfLoadDirectory int sfLoadParamFile char name char sfParamDir int sfLoadWorldFile char name All the load functions return 0 if successful and 1 if not sfLoadEvalFile loads a Colbert language file or loadable shared object file into Saphira The load directory sfLoadDirectory is set by default to the value of the environment variable SAPHIRA_LOAD if it exists or to the working di
145. lbert is robot and it has 22 available members each with its own name and type Not all members need be declared to Colbert The sfAddEvalStruct returns a new type index which is stored in the C variable sfSrobot This index is used in making the definition of the Saphira variable sfRobot available in Colbert As with the other sfAddEvalXXxX functions sfAddEvalStruct must be compiled into a shared object file and then loaded into Colbert C indexes for pointer types are constructed using the functions sfTypeRef and sfTypeDeref For example in C code to get a type index for a pointer to the robot structure use Saphira Software Manual sfTypeRef sfSrobot The size of a structure is returned in Colbert by the sizeof typename function The currently loaded structures are printed with the help structs command The robot and point types are predefined in the bin saphira executable 4 10 5 Compiling and Loading C Files Section 6 has more detailed information about the particulars of compiling native C files and making them into shared object files Under UNIX the object files must be converted into a shareable object file so The shareable object file is loaded with the load command e g load testload so A dynamically loaded file may be recompiled and reloaded at any point Under MS Windows C code is compiled into a Dynamic Link Library DLL The DLL is then loaded into Saphira again with the Load command DLL
146. les 5 7 5 Behavior schema 5 8 Pre Defined Saphira Behaviors 6 CREATING LOAD FILES AND CLIENTS 6 1 Host System Requirements 6 2 Compiling and Linking C Source Files 6 2 1 Writing C or C Client Programs 6 2 2 Compiling and Linking Client Programs under UNIX 6 2 3 Compiling and Linking Client Programs under MS VC 6 3 Client Examples 6 3 1 The Basic Saphira Client 6 3 2 The Demo Client 6 3 3 The testload so Loadable Object File Example 6 3 4 The Direct Client 6 3 5 The Packet Client 6 3 6 The Async Client 6 3 7 The Nowin Client 7 SAPHIRA SERVERS 7 1 Communication Packet Protocol 7 1 1 Packet Data Types 7 1 2 Packet Checksum 7 1 3 Packet Errors 7 2 Client Commands 7 2 1 Client Command Argument Types 7 2 2 Saphira Client Command Support 7 3 Server Information Packets 7 4 Start Up and Shut Down 7 4 1 Synchronization sfCOMS YNC 7 4 2 Autoconfiguration 7 4 3 Opening the Servers sfCOMOPEN vi 49 51 53 53 53 55 55 56 57 57 57 57 58 58 58 58 58 59 7 4 4 Keeping the Beat sfCOMPULSE 7 4 5 Closing the Connection sfCOMCLOSE 7 4 6 Movement Commands 7 5 Robot in Motion 7 5 1 Position Integration 7 6 Sonars 8 GUIDE TO THE SAPHIRA API 8 1 Saphira OS Functions 8 2 Predefined Saphira Micro tasks 8 3 State Reflection 8 3 1 Motor Stall Function 8 3 2 Sonar buckets 8 4 Direct Motion Control 8 5 Saphira Multi tasking 8 5 1 Micro task Definition 8 5 2 State Inquiries
147. lity on each individual process to complete its task in a timely and reasonable manner Each process is called a micro task because it accomplishes just a limited amount of work Compute intensive processes that take a long time to complete but can execute asynchronously with the Saphira system can be implemented as concurrently executing threads Accordingly use the Saphira sfStartup function with an async argument of and prepare your processes so they execute as a concurrent thread as we describe below Colbert activities and behaviors are also micro tasks and are defined using the Colbert language or behavior compiler Sections 4 and 5 Some of the micro task control functions described below are useful for these tasks as well To distinguish behaviors and activities from other micro tasks we call the latter simple micro tasks 8 5 1 Micro task Definition Simple micro tasks are functions with no arguments together with state information Micro tasks access their state through a global integer variable process_state Processes are initiated by an API call sfInitProcess which places the function onto the process stack Once they are initialized Saphira will call them with an initial state of sf INIT The micro task can change its state by setting the value of process_state User defined state values are integers greater than 10 values less than 10 are reserved for special states Table 8 3 Table 8 3 Saphira multiprocessing reserved
148. ll only work in compiled C code and cannot be called from the evaluator These include functions like sfAddEvalVar and sfAddEvalStruct which access underlying C constructs In addition application code which performs significant computation should be compiled as C code for efficiency The loader will load compiled C code in the form of shared object files so extension in UNIX d11l in MS Windows These files are loaded and dynamically linked with the running Saphira system See Sections 4 10 and 6 for information on how to compile shared object files and for some examples The Saphira Software Manual loader recognizes the extension and calls appropriate dynamic loading routines If present the function sfLoadInit is evaluated after the file is loaded Under MS Windows it is impossible to relink a DLL file that is in use by an application Therefore you must unload the DLL file first using the unload command For convenience unload with no arguments unloads the most recently loaded file 4 5 2 Load Directory Files are loaded based on the current load directory The following commands query and set this directory The argument to cd does not use C syntax and can contain any non blank characters pwd Print working directory value of variable sfLoadDirectory cd lt path gt Changes the working directory according to path Path may be an absolute or relative path Prints the new working directory Affects sfLoadDirectory B
149. me step Pressing the s key in the client window signals the next time step The Wake item if on deposits breadcrumbs in the display showing the last ten seconds of robot travel The Occ Grid item if on displays the occupancy grid constructed using the MURIEL algorithm This item is not implemented on Macintosh or machines with no color capability On some machines it may consume a large percentage of available CPU time for display 2 3 6 5 Sonars Menu The Clear Buffer item clears all of the accumulated sonar readings from the client internal buffers The Sonars On item toggles the sonar capability of the robot server Currently not implemented on the robot server the sonars are always on 2 3 6 6 Functions Menu The Functions menu toggles the display of the Behaviors Processes and Activities windows 5 The MURIEL algorithm is described in a paper that can be found at http www ai sri com konolige saphira 17 2 18 Saphira System Overview 2 3 7 Keyboard Actions In addition to the Saphira pulldown menus you may control some of the functions of the robot server directly from the client keyboard Table 2 1 These keys work only when the main Saphira window is active Table 2 1 Keyboard enabled Saphira drive and behaviors Incremental left turn Incremental right turn g Constant Velocity oF The sample Saphira client we provide defines a set of keyboard actions for robot motion and f
150. messages Currently there is no limit on the amount of text kept 4 3 Evaluator Help The evaluator has a simple help facility to remind you of commands help List of help topics help topic Help on this topic help lt fn gt Help on an API function or list of API functions containing fn Topics include utility commands file loading directories communication direct motion commands and information on particular API functions Not all API functions have associated help text we are adding them in future versions Help text can be added using the sfAddEvalHelp function and retrieved with sfGetEvalHelp Both these functions are available in Colbert and from compiled C code 4 4 Syntax Errors As much as possible Colbert uses ANSI C syntax But it also extends this syntax with new commands and constructions for robot control and omits some parts such as embedded assignments and arrays If the parser cannot understand the input it will print an error message in the interaction area and abort the loading of any file currently in progress Determining the reason for a syntax error is a difficult problem and the parser does not even try to do this Instead it will print the token that it was trying to parse when the error occurred and the line number in the file if a file was being loaded For example the ill formed C expression Tob 23 produces the error message 31 4 32 Using Colbert xxx Parsing error at
151. mm sec int 400 400 sfRobotComInt SfCOMVEL 150 sfCOMVEL2 4 mm sec int 100 100 sfRobotCom2Bytes sfCOMVEL2 40 50 The Saphira aware robot server will try to make the robot achieve the desired velocity and heading as soon as the commands are received using its internal de acceleration managers Check your robot s operation manual to find its absolute maximum achievable motion and rotational velocities 7 5 Robot in Motion When the Saphira aware robot server receives a velocity command it accelerates at a constant rate set internally to the speed you provided as the argument for sf COMVEL Rotational headings are achieved by a trapezoidal velocity function Figure 7 2 This function is re computed each time a new heading command is received making on the fly orientation changes possible short turn max velocity max velocity f not reached rotational velodty start position position position achieved achieved Figure 7 2 Trapezoidal turning velocity profile 7 5 1 Position Integration Depending on your robot it keeps track of its position and orientation based on dead reckoning from wheel motion which is an internal coordinate position A server command sfCOMSETO resets the robot server s internal x y position coordinates to 0 0 0 Registration between external and internal coordinates deteriorates rapidly with movement due to gearbox play wheel imbalance and slip
152. mple code is handler src basic behavior beh in your Saphira distribution software Note that the variables in the example are pointers to behavior structures and can be used directly in the sfStartBehavior function See the sample behavior definition file behavior beh for examples behavior sfConstantVelocity Sets the velocity setpoint on the robot server to its first parameter an integer in millimeters per second behavior sfStop Sets the velocity setpoint to zero No parameters behavior sfAvoidCollision Slows and turns the robot sharply to avoid immediate obstacles Takes four parameters listed in the Table 5 2 Additionally the default turn direction when it is completely blocked is given by the global variable sfPreferredTurnDir which should be set to either sfLEFTTURN or sfRIGHTTURN User programs and other behaviors can set this variable to change the action of this behavior Table 5 2 Avoid collision behavior parameters Effect sfFLOAT Front sensitivity to obstacles Value from 0 5 not sensitive to 3 0 very sensitive LOAT Side sensitivity to obstacles Value from 0 5 not sensitive to 3 0 very sensitive LOAT Turning gain controls how rapidly the robot turns away from obstacles Value from 4 0 slow turn to 10 0 fast turn LOAT Standoff Defines the avoidance bubble around the robot Value from flakey_radius at the robot to flakey_radius standoff standoff mm from the robot sfPref
153. mply issue the client s fCOMCLOS command GI 7 4 6 Movement Commands As of PSOS 4 2 the robot server accepts several different types of motion commands You can set the turn angle or velocity and the forward back velocity or you can control the two wheel velocities independently Table 7 8 summarizes the command modes available Table 7 8 Server motion command types SECOMEAD absolute heading a AD sfCOMDHEAD differential heading from control pt sfCOMDCHEAD differential heading from current sfCOMVEL _ forward back velocity sfCOMRVEL rotational velocity sfCOMVEL2 left and right wheel velocities The robot server automatically switches to the required motion control mode when it receives one of these commands For example if it is in two wheel velocity mode and it is sent an sfCOMHEAD command it abandons two wheel velocity mode and starts controlling the heading and velocity of the robot 90 Saphira Software Manual The arguments for these commands are given in Table 7 9 below The heading commands are with respect to the robot s internal coordinate system see the section below Table 7 9 Motion command arguments Argument s Typical invocation sfCOMHEA degrees int 0 360 sfRobotComInt SfCOMHEAD 320 D s COMD C HEAD degrees int 180 180 sfRobotComInt sfCOMDHEAD 10 sfCOMRVEL degrees sec int 200 200 sfRobotComInt sfCOMRVEL 80 sfCOMVEL
154. n The following are valid assignment operations point p int a Ine Abe float c a 2 b amp a b 3 p x 1 0 C amp p xX xo SAD If a Colbert variable is linked to a native C variable by the sfAddEvalVar function then changing the value of the Colbert variable will also change the value of the linked C variable Saphira Software Manual 4 9 11 Function Statements Function expressions are also considered as statements 4 9 12 Activity Schemas Activity schemas are defined using the special keyword act They are similar to function definitions but are interpreted by the Colbert executive as a special type of micro task act lt aname gt lt type gt lt param gt lt type gt lt local var gt update lt stmt gt lt stmts gt The activity name is any symbol The symbol cannot be declared as a variable or function If the name was previously assigned to an activity schema the old definition is replaced by the new one Note that any instances of the schema running as micro tasks are unaffected by the redefinition you must re invoke the activity schema to get the new definition The activity schema takes a set of parameters which are variables local to the activity If there are no parameters the parentheses may be omitted Optional local variables are declared only at the beginning of the activity schema An optional update block is a statement that is evaluated every time the activ
155. n Labs Web site http www newtonlabs com With Saphira the FTVS intercepts packet communication from the client to robot server interprets some commands from the client and sends new vision information packets back to the client Saphira includes support for setting some parameters of the vision system but not for training the FTVS on new objects or for viewing the output of the camera For this please see the FTVS user manual about operating modes In the future we intend to migrate some of the training functions to the Saphira client We also intend to have Saphira display raw and processed video Saphira also includes built in support for interpreting vision packet results If your robot has a vision system Saphira will automatically interpret vision packets and store the results as described below 9 1 Channel modes The FTVS supports three channels of color information A B and C Each channel can be trained to recognize its own color space Each channel also supports a processing mode which determines how the video information on that channel is processed and sent to Saphira A channel is in one of three modes 1 BLOB_MODE 0 2 BLOB_BB_MODE 2 3 LINE_MODE 1 Note these definitions as well as other camera definitions can be found in handler include chroma h To change the channel mode from a Saphira client issue the command sfRobotComStr VISION_COM pioneer _X _mode N where the mode N is 0 1 or 2 and the ch
156. n files handler include conf xxx h for library and header file definitions Saphira provides a way to call user functions whenever it is started up or connects to the robot It does this by registering user functions as callbacks with sfOnStartupFn and sfOnConnectFn Whenever a startup or connect event takes place Saphira calls the registered user function The startup callback can be used to initialize various features of Saphira s display such as the display rate or local global mode You can t set these before calling sfStartup because the windows aren t created yet If you don t want to do any special processing here there s no need to define a startup callback In this application mySt artupFn is invoked when the Saphira OS is initialized and it sets the display rate to 5 Hz see the sfSetDisplayState function in the API reference myConnectFn is invoked when the client connects to the robot server using the Connect menu or connect command here it is empty because there is no special processing to be done on connect There is no need to register this callback if you don t do any special processing on connect it s here for illustration purposes In the main function the callbacks are registered and then the Saphira OS is started by sfStartup Since the argument is 0 this function does not return and all computation takes place in the micro tasks The Saphira main window system passes keystrokes to your process via the callb
157. n from the menu Programming in MSVC is similar except that the form of the main function changes to MS Windows programming standards Saphira Software Manual include saphira h fe n ercloie ies Ike ror aphia kibra ry gt definition of startup connect and disconnect callbacks int PASCAL WinMain Handle hInst HANDLE hPreviInstance LPSTR lpszCmdLine int nCmdShow register callbacks sfOnConnectFn myConnectFn sfOnStartupFn myStartupFn start up Saphira micro tasking OS sfStartup hInst nCmdShow 0 RECENO In this case control does return to the main program after the Saphira client exits and the user should return 0 to indicate that the exit was normal For most robot programming all operations can be handled in micro tasks If a more compute intensive task must be done concurrently then sfStartup should be called with an argument of 1 which means that the Saphira micro tasking OS is started and immediately returns control to the main program The Main Saphira thread Execute micro tasks async routines Execute micro tasks Execute micro tasks Figure 6 1 Concurrent execution of Saphira OS and user asynchronous tasks user can now run any routines concurrently with the Saphira OS which is executing its micro tasks every 100 ms The micro tasks and the asynchronous user routines share the same address space and can communicate via global variables Figure 6
158. n is completed or times out 4 8 1 Act Definition Activities are defined as sets of statements in the Colbert language see below The syntax is similar to that of C functions with the keyword act as the first token e g act actname parameters variable declarations update statements body statements Figure 4 2 shows a sample activity that moves the robot in a square There is one internal variable a that keeps track of the 4 legs of the square The main body of the act is a while loop that just decrements a turns the robot 90 degrees and moves it along on the next leg The act s parameter len specifies the length of each side of the square act square int len move in a square Liat 27 update sfSMessage a is d a a 4 while a arca ly move len ieWien QO Figure 4 2 A sample activity schema definition We haven t explained yet how the act is executed the next subsection explains this in some detail But note that the move and turn actions halt the activity until they are completed The Colbert evaluator accomplishes this by calling the activity periodically to check and see if it can proceed On each call the 35 4 36 Using Colbert update statement is evaluated This statement prints the following sequence of messages in the interaction area ais 0 ais ais 4 A Ww a is a is 3 Initially the variable a is set to O when the act is first sta
159. n which they are not In this event go into the os h file and change the definitions under your OS Saphira Software Manual One peculiarity of os h is that it relies on the conditional preprocessing facilities of gnu make gmake Not all native makes support this facility If you get errors during the preprocessing phase of the compilation from os h switch to gmake The compile command makes saphira o fromthe saphira c file It is important that the variable D CONFIG is passed to the compiler because this tells the header files what particular variant of UNIX is being used The include directories are the Saphira header directory and the X11 directory The link command takes the object file generated by the compile command and links it with the Saphira library and system libraries to form the executable The Saphira library is indicated by 1sf This is the library that opens a graphics window and has all the user interface functions If you don t want a window use the lsfx library The LLIBS variable indicates other system libraries that may be needed by this particular UNIX system The executable is deposited in the same directory as the source file and can be invoked by typing its name at the shell prompt The file test load so is an example of a shared object file which is loadable under Colbert The C source is compiled as usual but the linking step is different Instead of creating an executable file the LD command is invoked
160. ng 34 101 sfDonePosition 34 101 sfDrawCenteredRect 114 sfDrawRect 114 sfErrMessage 96 sfErrS Message 96 sfFindArtifact 111 sfFindProcess 104 sfFollow 61 sfFollowCorridor 62 sfFollowDoor 62 sfFrontMaxRange 107 sfGlobalOrigin 111 sfGoToPos 61 sfHaveClientPacket 121 97 sfInitBehavior 54 sfInitBehaviorDup 54 sfInitControlProcs 97 sfInitInterpretationProcs 98 sfInitProcess 104 sfInitRegistrationProcs 98 sfInterruptProcess 104 sfInterruptSelf 104 sflsConnected 95 sfKeepOff 60 sfKeyProcFn 96 sfLeftWallHyp 114 sfLoadDirectory 33 sfLoadMapFile 117 sfMessage 96 sfMoveRobot 113 sfNorm2Angle 112 139 Index sfNorm3Angle 112 sfNormAngle 112 sfOccBox 107 sfOccBoxRet 107 sfOccPlane 108 sfOccPlaneRet 108 sfOnConnectFn 95 sfOnDisconnectFn 95 sfOnStartupFn 95 sfPause 95 sfPointBaricenter 113 sfPointDist 113 sfPointDistPoint 113 sfPointMove 113 sfPointNormalDist 113 sfPointNormalDistPoint 113 sfPointPhi 113 sfPointXo 113 sfPointXoPoint 113 sfPointYo 113 sfPointYoPoint 113 sfProcessClientPacket 121 sfReadClientByte 122 sfReadClientSint 122 sfReadClientString 122 sfReadClientUsint 122 sfReadClientWord 122 sfRemPoint 111 sfResetRobotVars 121 sfResumeProcess 104 sfRightWallHyp 114 sfRobot 99 sfRobotCom 121 sfRobotCom2Bytes 121 sfRobotComInt 121 sfRobotComStr 121 123 sfRobotComStrn 121 sfRobotOrigin 111 sfRunEvaluator 31 sfScreenToWorldX 97 sfS
161. ng else Figure 2 4 shows a typical Behaviors window The first two behaviors in our sample client are active that is they can contribute to the control of the robot their running parameter is 1 The other two are inactive The active state of a behavior may be changed by signaling its invoking activity in the Activities window Note this is a change from version 5 x in which the buttons were active in the behavior window Saphira Software Manual F Avoid Collision F Keep OFF i Constant Vel al Stop Summation Figure 2 4 Saphira Behaviors window Linux Motif version The dark bar next to each behavior name indicates the state of the behavior There are two vertical lines representing the behavior s outputs for turning and forward backward movement For example the Keep Off behavior in Figure 2 4 is fully active for both turning and moving as indicated by the horizontal activity bars going through the vertical lines details in Figure 2 5 This behavior instructs the robot to turn right and to move backwards slow down in this example as indicated by the direction bars on either side of the vertical lines Move A E Keep Off Behavior name Direction E On off indicator Activity bars Figure 2 5 Keep Off behavior display expanded The behaviors appear in order of their priority in influencing the robot s actions with the highest priority behaviors at the top of the window At the bottom the Summation line giv
162. ng used to update the robot s global position as it moves contains errors and the robot s global position gradually decays in accuracy To bring it back into alignment with stationary artifacts registration routines use sensor information to align the robot with recognized objects These functions are described in a subsequent section You may add and delete artifacts in the LPS There are two types of artifacts that users add Map artifacts are permanent artifacts representing walls doorways and so on in the office environment Goal artifacts are temporary artifacts placed in the LPS when a behavior is invoked The artifact functions as an input to the behavior for example there is a behavior to reach a goal position and the goal is represented as a point artifact in the LPS These artifacts are usually deleted when the behavior is completed 107 8 Saphira API Guide The system also maintains artifacts of different types There is an artifact representing the origin of the global coordinate system There are various hypothesis artifacts representing hypothesized objects extracted by the perceptual routines and used by the registration routines 8 7 1 Points and Lines All artifacts are defined as C structures Each has a type and a category The type defines what the artifact represents the simplest artifacts are points and lines while corridors are a more complex type You may define your own artifact types The category of an
163. nmental conditions The micro tasking OS involves some limitations each micro task must accomplish its job within a small amount of time and relinquish control to the micro task OS But with the computational capability of today s computers where a 100 MHz Pentium processor is an average microprocessor even complicated processing such as the probability calculations for sonar processing can be done in milliseconds The use of a micro tasking OS also helps to distribute the problem of controlling the robot over many small incremental routines It is often easier to design and debug a complex robot control system by implementing small tasks debugging them and them combining them to achieve greater competence Saphira Software Manual 2 1 2 User Routines User routines are of two kinds The first kind is a micro task like the Saphira library routines that runs synchronously every Saphira cycle In effect the user micro task is an extension of the library routines and can access the system architecture at any level Typically the lowest level that user routines will work at is with the state reflector which is an abstract view of the robot internal state Saphira and user micro tasks are written in the C language and all operate within the same executing thread so they share variables and data structures User micro tasks have full access to all the information typically used by Saphira routines Although user micro tasks can be coded dir
164. nomous Systems e information about SRI robots and projects that use Saphira including the integration of Saphira with SRI s Open Agent Architecture o links to other sites using Pioneer robots and Saphira The entry to the SRI Saphira web pages is http www ai sri com konolige saphira 1 8 4 Support Have a problem Can t find the answer in this or any of the accompanying manuals Or know a way that we might improve our robots and software Share your thoughts and questions directly with us support rwii com Your message goes to our team of developers who will help you directly or point you to where you may find help Because this is a support option not a general interest newsgroup we must reserve the option to reply only to questions about bugs or problems with RWI manufactured robots or Saphira 1 8 5 Acknowledgments The Saphira system reflects the work of many people at SRI starting with Stan Rosenschein Leslie Kaelbling and Stan Reifel who built and programmed Flakey in the mid 1980 s Major contributions have been made by Alessandro Saffiotti Karen Myers Enrique Ruspini Didier Guzzoni and many others Saphira Software Manual 2 Saphira System Overview Saphira is an architecture for mobile robot control It was originally developed for the research robot Flakey at SRI International and after being in use for over 10 years has evolved into an architecture that supports a wide variety of research and application
165. not set this variable correctly Saphira clients and the simulator will fail to work or fail to work properly Please set this as soon as you install the distribution If you have a previous installation of Saphira your SAPHIRA environment variable will be set to the old toplevel directory YOU MUST RESET IT to the toplevel directory of the new distribution All new clients will complain and fail to execute until you do A useful method on UNIX systems is to make a soft link to usr local saphira ver61 using the file current The environment variable can be set to usr local saphira current and will remain unchanged when installing a new system only the soft link current need be reset Saphira Software and Resources Unix systems should use one of the following methods preferably in the user s cshre or other default shell script parameter file export SAPHIRA usr local saphira ver61l bash shell setenv SAPHIRA usr local saphira ver 6 l csh shell In Windows 95 and NT 3 51 assuming the top level Saphira directory is c saphira ver61 add the following line to the C AUTOEXEC BAT file SET SAPHIRA C saphira ver61 In Windows NT 4 0 go to Start Settings System and click on the Environment tab Add the SAPHIRA variable in either the user or system wide settings The Saphira library is now in a sharable form on both UNIX and MS Windows machines This means that a Saphira application will link into the library at runtime rather
166. ns the list and brief descriptions of the currently implemented Saphira client commands which we discuss in detail in following sections These and additional server operating commands used by most but not all Saphira enabled robots also appear in the Saphira header file 85 7 Saphira Servers handler include saphira h Check your robot s operation manual Saphira supplement and Saphira distribution UPDATE text file for the latest details Table 7 3 Client command communication packet OxFA OxFB Packet header same for client and server must be lt 200 total bytes long Fed Bll a Number see Table 4 4 Arg Type Data type of command argument if included optional Ox3B or sfARGINT positive integer Ox1B or sfARGNINT negative int or absolute value 0x2B sfARGSTR string null terminated Argument N data Command argument integer or null optional terminated string Packet integrity checksum 7 2 1 Client Command Argument Types There are three different types of client command arguments positive integers two bytes long negative integers two bytes long and strings of up to 195 characters long 200 byte limit on packets terminated with a 0 NULL Byte order is least significant byte first Negative integers are transmitted as their absolute value unlike information packets which use sign extension for negative integers see below The argument is either an integer a string or nothing depending on the command 7
167. nt Colbert activities Figure 2 8 Open it from the Functions Activities menu in the main window The Activities window contains a scrolled list similar to the Processes window and each line contains the Activity name and its state The state information is updated in real time as the Activity state changes Relationships between activities are indicated by line indentations For instance in the example Figure 2 8 the second activity follow it is indented to show that it is a child of the first activity The two activities combine to invoke a corridor following sequence for the robot The top level activity waits until the robot has found a corridor then invokes its child activity to select a path to follow down the center of the corridor In addition the top level activity monitors the state of the robot and when it is no longer in the corridor or gets turned sideways to the corridor it disables the follow it activity As with micro tasks you may manually interrupt an activity by selecting it and pressing the Enter key or by double clicking it with the mouse If the activity is running this will force it into the INT interrupt state Normally an activity will respond to this state by suspending Use the same action to reactivate an interrupted suspended activity This will invoke the RES resume state Normally an activity will respond to this state by reinitializing and starting its characteristic behaviors There are several activ
168. nvocation of the pre defined behavior sfKeepOff sfStartBehavior sfKeepOff keep off instance name 0 no timeout 1 priority 0 no suspension 100 0 caution speed 0 4 sensitivity Behaviors can be suspended or killed by sending them signals using the task signal facility Section 4 8 3 All of the behavior functions from version 5 3 such as sfInitBehavior and sfKillBehavior are not available in 6 x Saphira Software Manual A Behavior Grammar The behavior grammar is a convenient syntax for defining behaviors The BNF for the grammar is given below For reference here is an example of a typical behavior using this syntax This behavior sends the robot towards a goal position BeginBehavior myGoto Params Rules Activity EndBehavior behavior name SEPTR goal PE We joxaliniceie tO goal joyoaliaic sfFLOAT radius how close we come in mm IE OO klere Maem Wwe IRG If too_right Then Turn Left If Not near_goal Or too_left Or too_right Then Speed 200 0 If near_goal Or too_left Or too_right Then Speed 0 0 PILOENE joa SEROREN las efeell_ joe A ILOEIE Cher grome DLS Coal joe p COO _ lere wje_Sitweuelatc jai Oi O 6 lt s COO wieme Sicweaeiaic clowm jelaa 0 6 O iL Pp near_goal straight_down dist radius radius 2 Turn Not near_goal Speed Not near_goal Goal near_goal Sample behaviors can be found in the file handler src
169. ointBaricenter point pl point p2 point p3 EIE float sfPointDist point p 111 float sfPointDistPoint point pl point p2 111 void sfPointMove point pl float dx float dy point p2 111 float sfPointNormalDist point p TNL float sfPointNormalDistPoint point p point q Irri float sfPointPhi point p 109 float sfPointXo point p 111 float sfPointXoPoint point p point q 111 float sfPointYo point p 111 float sfPointYoPoint point p point q ILI void sfRemPoint point p 109 point sfRobotOrigin 109 void sfSetGlobalCoords point p 108 void sfSetLocalCoords point p 108 void sfSubAngle 110 void sfSub2Angle 110 void sfUnchangeVP point pl point p2 point p3 111 Behaviors BEHCLOSURE sfFindBehavior char name Error Bookmark not defined BEHCLOSURE sfInitBehavior behavior b int priority int running Error Bookmark not defined BEHCLOSURE sfInitBehaviorDup behavior b int priority int running Error Bookmark not defined int sfBehaviorControl 56 void sfBehaviorOff BEHCLOSURE b Error Bookmark not defined void sfBehaviorOn BEHCLOSURE b Error Bookmark not defined void sfKillBehavior BEHCLOSURE b Error Bookmark not defined void sfSetBehaviorState BEHCLOSURE b int state Error Bookmark not defined Behaviors Predefined Saphira behavior sfAttendAtPos 60 behavior sfAvoidCollision 59 behavior sfConstantVelocity 59 131 12 API Reference beh
170. ontrol programs Saphira provides a set of graphics routines that can be called by micro tasks A set of pre defined micro tasks display information about the state reflector and other data structures such as the artifacts of the GMS User programs may also invoke the graphics routines directly to display relevant information 2 2 7 1 Agent Interface A Saphira client can communicate with other Internet based agents through its agent interface to the Open Agent Architecture OAA The OAA is an agent based architecture for distributed information gathering and control and has extensive facilities for user interaction such as speech and pen based agents Currently the OAA interface is under development at SRI and issues concerning its use in Saphira outside SRI have to be resolved before it can be released 2 3 Running the Sample Client This section exercises some of the capabilities of Saphira through a sample client It also illustrates the graphical user interface for interacting with clients To run the sample application execute the file saphira exe inthe Saphira bin distribution directory This executable requires only runtime files found on your system and the relevant loadable libraries from Saphira sf d11 or libsf so 6 1 x You should have installed these as directed in Section 1 6 The Saphira client will initialize an interface window showing the LPS Figure 2 3 The robot is in the center of the display pointing up An informat
171. or activities processing resumes at the onresume label If no such label exists the activity resumes at its first statement succeed and fail are special commands for stopping an activity The activity is considered to be finished no more processing takes place as in the case of suspension But other activities can check for these states to determine if the activity accomplished its job or not When an activity falls through and finishes its last statement it will enter the sESUCCESS state by default Normally sub activities those started from other activities are not removed from the active process list when they finish This is so other activities can check on their progress determine if they finished and so on An activity can be explicitly removed from the active process list by giving it the special state sfREMOVE with the remove command It s a good idea to remove activities and behaviors when they re done Toplevel activities those with no parents are removed automatically when they finish The trace and untrace signals change the tracing state of activities see Section 4 8 7 Note that all of the signaling commands can be issued in the user interaction area which is the normal way to start stop and trace activities These commands are also used inside activities as a means of coordinating their action 39 4 40 Using Colbert 4 8 4 Accessing Activity States There is no special construct
172. or component details Mobile robot with Saphira enabled servers Radio modems or Ethernet radio bridge optional v Computer Power Macintosh Pentium or 486 class PC with Microsoft Windows 95 or NT FreeBSD or Linux operating system or UNIX workstation Open communication port TCP IP or serial v Four to five megabytes of hard disk storage v PKUNZIP PCs GUNZIP PCs and UNIX StuffIt Lite or compatible archive decompression software Optional v C program source file editor and compiler Note Current Windows95 NT version of Saphira supports only Microsoft s Visual C C software not Borland s Turbo C C products Necessary for compiling new subroutines in standard C v Motif GUI and libraries for FreeBSD Linux UNIX Necessary only to compile new clients with Colbert users may instead operate in an already compiled application environment 1 6 Saphira Client Installation The latest information for installing and running Saphira can be found on the readme file in the distribution please examine this file carefully before and during installation The update file has information about major changes in the latest releases of the Saphira system and should be consulted as a general guide for updating older programs The Saphira distribution software including the saphira demonstration program Colbert simulator and accompanying C libraries come stored as a compressed archive of directories and files either on a 3 5
173. or turning some behaviors on and off In a user application the function sfProcessKey lets you intercept keystrokes and initiate your own hot key actions 2 3 8 Behaviors Window Saphira s Behaviors window shows graphically the state of all current behaviors It is invoked from the Functions Behaviors menu in the main window To understand the contents of this window it may be useful to review the previous section in this chapter on Saphira behaviors Our sample Saphira client invokes four behaviors two for obstacle avoidance one for going forward at a constant velocity and one for stopping The obstacle avoidance behaviors are called Avoid Collision and Keep Off Avoid Collision prevents the robot from banging into obstacles at close range by initiating a sharp turn and slowing down the robot The Keep Off behavior deflects the robot from longer range obstacles The Constant Velocity behavior attempts to keep the robot going forward at a fixed speed of about 300 millimeters per second The Stop behavior not surprisingly stops the robot It is useful when you want the robot to stop if no other behavior is managing the robot s movements For example if the Constant Velocity behavior is invoked and then killed the robot will still have a residual forward velocity In the absence of any other behaviors it will keep moving forward Invoking the St op behavior at a low priority assures that the robot will stop if it is not doing anythi
174. ore returning control to the scheduler For example it is impossible to go through a loop without encountering at least one break On the other hand sequences of ordinary statements such as variable assignments will all execute in the same cycle thus making act evaluation efficient Evaluation of acts is similar to the execution of finite state machines In fact you can view activity schemas as a shorthand for finite state machines with special syntax for sequences conditionals and iterations Figure 4 3 shows the finite state execution model of the patrol activity The states of the finite state machine map to states of the activity the wait conditions are represented by transition arcs that are satisfied when the wait condition holds One of the most interesting characteristics of the Colbert language is its ability to represent finite state machines in a compact readable form start a done Move done a a 1 Turnto 180 Turnto done Move done c d Move 1000 Turnto 0 e Turnto one f Move 1000 Figure 4 3 The finite state machine for the patrol activity The current state of an act is an integer since acts are micro tasks Section 8 5 The state is an index into the body of the act and shows where the next statement to execute is The Saphira system maintains a mapping between these internal states and the source lines of the activity schema definition so that it can indicate source lines during tr
175. ory input building world models and controlling the actions of the robot As with the system architecture the routines in the control architecture are tightly integrated to present a coherent framework for robot control The control architecture is flexible enough so that users may pick among various methods for achieving an objective for example using a fuzzy control regime vs more direct control of the motors It is also an open architecture as users may substitute their own methods for many of the pre defined routines or add new functionality and share it with other research groups In this section we ll give a brief overview of the two architectures as well as the main concepts of Saphira More in depth information can be found in the documentation at the SRI Saphira web site Attp www ai sri com konolige saphira 2 1 System Architecture Think of Saphira s system architecture as the basic operating system for robot control Figure 2 1 shows the structure for a typical Saphira application Saphira routines are in blue user routines in red Saphira routines are all micro tasks that are invoked every Saphira cycle 100 ms by Saphira s built in micro tasking OS These routines handle packet communication with the robot build up an internal picture of the robot s state and perform more complex tasks such as navigation and sensor interpretation 2 See http www ai sri com people flakey for a description of Flakey and furth
176. p and connect callbacks are defined and then registered in the main function Then sfStartup is called with an argument of 1 which starts up the Saphira OS but continues executing the user s program in the main function It s important that the Saphira OS be operating because its default micro tasks handle communication and motor control to the robot server keeping the state reflector current The direct action calls of the user program depend on these micro tasks The program waits in a while loop until the user connects to a robot then starts to issue a series of direct motion commands The motion commands are synchronized using the s fDoneXXX functions to wait for completion and sfPause to wait for a time interval Finally it closes the connection to the robot and exits When the main program exits the Saphira OS is also automatically exited If you want to keep the micro task OS operating then just start a while loop whose body is sfPause 1000 Note that the packet communication and state reflection micro tasks are initiated in the connect callback myConnectFn It s important to do this since the direct motion commands rely on state reflection to control the robot 79 6 Creating Clients and Load Files include saphira h void myStartupFn void sfSetDisplayState sfGLOBAL TRUE use the global view WoC meula line aroge Cee SECC meae 0s sfOnStartupFn myStartupFn je TeRgalsicere StartUp
177. p files by selecting the appropriate item from the Files menu A file selection dialog appears for choosing the file Loading a new map does not delete any old map artifacts use the Delete Map item for this The current map can be saved to a file using the Save Map item which invokes a file save dialog All artifacts in the current map can be deleted with the Delete Map item The load menu does not load Colbert files use the Colbert evaluator commands in the interaction area 2 3 6 3 Grow and Shrink Clicking either the Grow or Shrink menu causes the LPS display to grow or shrink in scale respectively 2 3 6 4 Display Menu The first item in the Display menu is another pulldown menu controlling the display update rate On some systems high update rates consume significant portions of available CPU time and lowering the update rate will increase performance If the number of motor packets Mpacs per second goes significantly below 10 and there is a good connection to the robot server than a high display update rate may be the culprit The Local item controls the LPS viewpoint When on the view is robot centric when off the view is world centric global Note that this only controls the display of information all internal geometric structures remain the same Single Step mode is useful for debugging and can only be used with the simulator When on it causes the simulator to wait for a signal from the client at each 100 millisecond ti
178. page and many other real world factors You can rely on the dead reckoning ability of the robot for just a short range on the order of several meter and one revolution depending on the surface carpets tend to be worse than hard floors Also moving either too fast or too slow tends to exacerbate the absolute position errors Accordingly consider the robot s dead reckoning capability as a means of tying together sensor readings taken over a short period of time not as a method of keeping the robot on course with respect to a global map 91 92 Saphira Servers The orientation commands sfCOMHEAD sfCOMDHEAD and sfCOMDCHEAD turn the robot with respect to its internal dead reckoned angle Figure 7 3 On startup the robot is at the origin 0 0 pointing towards the positive x axis at 0 degrees Absolute angles vary between 0 and 360 degrees As the robot moves it will update this internal position based on dead reckoning The x y position is always positive and rolls over at about 3 000 millimeters So if the is at position 400 2900 and moves 400 millimeters along the y axis and 600 millimeters along the x axis its new position will be 2800 300 180 Figure 7 3 Saphira aware server internal coordinate system 7 6 Sonars When opened by the appropriate client command see sf COMOPEN above the Saphira aware robot server automatically coordinates and begins firing the robot sonars in a pre defined default sequ
179. pend further sequential execution of the act until the condition c_exp becomes nonzero On each cycle of the scheduler c_exp is evaluated and if zero control states at the waitfor statement The waitfor statement without the optional timeout parameter is equivalent to while c_exp The timeout parameter is very handy for preventing blocks in an activity After n cycles if the condition still has not been satisfied the wait for completes and execution continues with the next statement Unlike suspension wait for does not affect any child activities which keep executing normally 4 9 9 Iteration and Branching Statements The only iteration construct in Colbert is the while statement while lt c_exp gt lt stmts gt c_exp is evaluated and if false a single break occurs so control returns to the scheduler On the next cycle execution continues with the statement after the while Control may also be transferred in an act using the goto statement and labels lt label gt goto lt label gt Labels may only occur at the top level of an activity schema goto s cause a single break when they are executed so that control returns to the scheduler 4 9 10 Assignment Statements Values may be assigned to any expression that represents a storage location This includes variables and locations described by pointers and structure members Implicit type conversion is made to convert the value to the type of the storage locatio
180. phira 6 1 behaviors are a type of activity and can be turned on and off from the activities window The behavior window is output only and shows more detail on behavior execution 1 Saphira Software and Resources 1 4 Robot Simulator Saphira also comes with a software simulator of your physical robot and its environment This feature allows you to debug your applications conveniently on your computer The simulator has realistic error models for the sonar sensors and wheel encoders Even its communication interface is the same as for a physical robot so you won t need to reprogram or make any special changes to the client to have it run with either the real robot or the simulator But unlike the real thing the simulator has a single step mode which lets you examine each and every step of your program in detail The simulator also lets you construct 2 D models of real or imagined environments called worlds World models are abstractions of the real world with linear segments representing the vertical surfaces of corridors hallways and the objects in them Because the 2 D world models are only an abstraction of the real world we encourage you to refine your client software using the real robot in a real world environment 1 5 Required and Optional Components The following is a list of components that you 1I need as well as some options you may desire to operate your robot with Saphira Consult your mobile robot s Operation Manual f
181. point in the direction of a goal position The robot always turns in the direction that makes the smallest turn Parameters are Table 5 10 Turn to parameters Effect sfPTR Goal position The robot turns until it points towards this goal Should be a pointer to a point artifact sfFLOAT Success angle in degrees If the robot is within this angle of pointing towards the goal it will have succeeded sfFPLOAT Turn speed How fast the robot turns to the goal Value of 0 5 is slow speed 2 0 is fast Saphira Software Manual 6 Creating Load Files and Clients This chapter describes how to create Saphira clients and gives some example clients There are three types of clients gt Loadable clients Loadable clients are created by loading files into a base system typically bin saphira The files may be Colbert language interpreted files or compiled C code in shared object files gt Standalone clients Standalone clients are created by compiling C code and linking it with the Saphira libraries to create a standalone executable gt Foreign clients These clients are called from a program written in another language e g PROLOG or LISP The foreign language executable loads and executes routines from the Saphira libraries and compiled user C code Which method to use is up to the user With Colbert the user stays within an interactive debugging environment and can debug and re execute procedures without the burdensome debug
182. proceed 6 2 1 Writing C or C Client Programs To develop a standalone Saphira application or to load C routines into Colbert you write one or more C or C programs that contain your own functions and make calls to the Saphira library routines It may help to review Section 2 for an explanation of micro tasks and asynchronous user routines For a standalone client the main file will always have the following structure in UNIX systems include saphira h fe Insecleic Tike i ie Syeyolnialicey Ililoieeicy definition of startup connect and disconnect callbacks WOW MEU sine Bice Claw A ENA c wegisrer calillogicks sfOnConnectFn myConnectFn sfOnStartupFn myStartupFn start up Saphira micro tasking OS SHeciectiate tion GON ey The Saphira library headers as well as other relevant system and graphics headers are loaded by the handler saphira h file This file is always included whether creating a standalone client or loadable shared object files The callbacks are defined to start up Saphira or user micro tasks when the client connects to or disconnects from the robot The main function is the entry to the client it registers the callbacks and then starts up the Saphira micro tasker with the call to sfStartup The argument of 0 to this function means that control does not return to the main program all processing is done using micro tasks and the client exits when the File Exit item is chose
183. r or activity and wait for its completion In the square activity both turn and move caused the act to wait Acts will wait for two reasons gt A direct motion action a behavior or a sub activity is blocking execution Direct motion actions are discussed in Section Direct Motion Commands4 7 behaviors and sub activities are invoked with the start command discussed in the next section Most such actions are implicitly blocking until completion unless the noblock keyword is given on invocation gt An explicit wait is issued with the waitfor statement This statement waits for its condition to hold before continuing execution e g waitfor a b waits until either a orb is nonzero Besides explicit and implicit waiting conditions an act can be suspended or interrupted by signals from other acts or itself These special states are described in Section 4 8 3 on signaling To prevent an act from taking too much computation time single breaks also occur in many situations A single break causes the act to return control to the scheduler but does not initiate a waiting condition In the next micro task cycle the act continues execution at the current state Single breaks are issued at the end of the following statements goto the last statement in a while body the condition of a while statement being false start signal Saphira Software Manual Single breaks ensure that an act does not evaluate large numbers of statements bef
184. r this is just an example the recommended way to change the global position is with sfMoveRobot sfRobot ax sfRobot ax 1000 0 Besides variables constants can be defined in Colbert with sfAddEvalConst The format is similar to adding variables sfAddEvalConst char name int type fvalue val where val is a constant expression For example here are some of the predefined constant loaded into bin saphira sfAddEvalConst sfConstantVelocity sfBEHAVIOR sfConstantVelocity sfAddEvalConst SfVSLOWLY sfFLOAT 3 0 sfAddEvalConst SfSLOWLY sfFLOAT 4 0 The only way to start a behavior from Colbert is to define it in C and then make it accessible with sfAddEvalConst or sfAddEvalVar 4 10 4 Making Native C Structures Accessible Native C structures are made accessible in Colbert with the sfAddEvalStruct function New structures cannot be defined in Colbert they must already exist in some loaded C object file sfAddEvalStruct can only be called from loaded C object files not from the Colbert evaluator It must always be compiled and loaded from a shared object file usually as a call in the sfLoadInit function see the example in the Section 4 10 1 The format of sfAddEvalStruct is sfAddEvalStruct char name int size char amp s int num 49 4 50 Using Colbert name is the name of the structure as seen by Colbert size is the size in bytes of the structure s is
185. r changes in behavior management from Version 5 3 Behaviors are no longer invoked with sfInitBehavior or sfInit IntendBeh instead use sfStartBehavior which takes a variable number of arguments for the behavior or the st art command from the Colbert interaction window Behaviors can be turned on and off by sending them signals either from the interaction window or from the Function Activities window Behaviors can not be controlled from the Function Behaviors window the check box that appears there just shows whether a behavior is active or not 2 2 4 Activities and Colbert To manage complex goal seeking activities Saphira provides a method of scheduling actions of the robot using a new control language called Colbert With Colbert you can build libraries of activities that sequence actions of the robot in response to environmental conditions For example a typical activity might move the robot down a corridor while avoiding obstacles and checking for blockages Activity schemas are the basic building block of Colbert When instantiated an activity schema is scheduled by the Colbert executive as another micro task with advanced facilities for spawning child activities and behaviors and coordinating actions among concurrently running activities Activity schemas are written using the Colbert Language The language has a rich set of control concepts and a user friendly syntax similar to C that makes writing activities much easier Bec
186. r reading in the rectangle it returns 5 000 5 meters For example in the case of an LPS shown in Figure 8 2 sfOccBox sfSIDES 1000 600 900 800 1 returns 300 sEOccBox sfFRONT 1000 600 900 600 0 returns 600 105 8 Saphira API Guide sfOccBoxRet returns the same result as sfOccBox but also sets the arguments x and y to the closest reading in the rectangle if one exists cx 1000 cy 600 Figure 8 2 Sensitivity rectangle for the sfOccBox functions int sfOccPlane int xy int source int d int sl int s2 int sfOccPlaneRet int xy int source int d int sl int s2 float x float y The plane functions are slightly different Instead of looking at a centered rectangle they consider an infinite rectangle defined by three sides a line perpendicular to the direction in question and two side boundaries Figure 8 3 shows the relevant areas for sfOccPlane sfFRONT sfFRONT 600 400 1200 The first parameter indicates positive X direction for the placement of the rectangle The second parameter indicates the source of the sonar information the front sonar buffer s fFRONT the side sonar buffer s SIDES or both s FALL The rectangle is formed in the positive X direction with the line X 600 forming the bottom of the rectangle The left side is at Y 400 the right at Y 1200 The nearest sonar reading within these bounds is at an X distance of 650 and that is returned 106 Saphira Sof
187. ra client and then start them up The Colbert evaluator parses and executes the activities and reports back results and errors Having an evaluator is very convenient for development and debugging because you can try out code without having to re compile and re link an entire client and then try to get back to the state you re interested in The Colbert evaluator has the following capabilities gt Direct execution of control statements from a running Saphira client gt Tracing of activities users can see exactly what statements are being executed gt Signaling between activities activities may start sub activities or interrupt already running activities gt Trapping of errors fatal errors such as divide by 0 just disable the offending activity and print an error message gt Error correction buggy activities can be edited with a text editor and reloaded without exiting the running Saphira client A technical paper describing Colbert is available from the website http www ai sri com konolige saphira in the pubilications section 4 1 A Colbert Example We ll introduce the Colbert language with a short example using the direct motion calls to the robot The example file isin colbert direct act The first step is to start the Saphira client and connect to a robot or the simulator see Section 2 3 After you ve successfully connected try typing the following statement in the interaction area at the bottom of the clien
188. rable for sequences of actions In other cases you may want to start several activities in parallel e g a monitoring activity and a direct action activity In this case Colbert provides a nonblocking instantiation mode In addition to direct motion calls activities can reference standard C variables and functions A number of library variables and functions are available initially and more can be added through the use of the sfAddEvalXxx functions There are some limitations on the C syntax of the evaluator for example you can t have assignments embedded within a C expression Unlike standard C files Colbert files allow you to execute statements from within the file In the example the last two statements are executed One is a call to a library function for setting the state of the LPS display The other starts up the aa activity So loading the file has the effect of defining three acts then setting the display state and starting the toplevel activity Saphira Software Manual 4 2 Evaluator Interaction Area The interaction area is at the bottom of the Saphira client window This area is always present in a Saphira client for output of messages s fMessage and sfSMessage library calls If the micro task sfRunEvaluator has been invoked then it is also available for user text input to the Colbert evaluator The sample client bin saphira invokes the evaluator At the beginning of a session several lines are written to the inte
189. raction window showing the Saphira toplevel directory and the current working directory for loading Colbert files There is an input prompt gt You may type input at this prompt and edit it using standard editing commands e g delete and backspace keys The characteristics of text editing are set by the XKeysymDB file and the X resources databases in UNIX If you have trouble getting text editing to work in UNIX systems please check with a local X guru The evaluator accepts commands and activity definitions from the user Commands are always just a single line but you can extend a line by typing a backslash V as the last character and continuing on subsequent lines A carriage return lt cr gt is needed to input the line The cursor need not be at the end of a line in order to use a lt cr gt At the command line a terminating semicolon is optional for all statements For convenience some of the utility commands do not adhere to C syntax For example the load command accepts its string argument without quotes so you can type load src test act for example There is a history list of previous input You can cycle through previous lines by using the ctrl P back and ctrl N forward keys Once a line is retrieved it can be edited Text may be selected cut and pasted using the standard mouse keys As in C case is significant A scroll bar on the right sight of the interaction area lets you scroll back through previous
190. ration and Account Sheet that accompanied this manual Saphira software as well as the variety of support literature including this manual currently are stored in subdirectories under the pathname pub robots Pioneer Saphira Consult your computer system manual for connection software and operation for downloading files via the UNIX file transfer protocol ftp or equivalent service 1 8 2 Saphira Newsgroup RWI ActivMedia also maintain several special email based newsgroups for robot owners to share ideas software and questions To find out more about these special newsgroups send an email message with your reply email address as follows 1 Saphira Software and Resources To majordomo rwii com From lt your return email address goes here gt Subject help Subject always ignored body of message choose one or more commands help returns instructions lists returns list of newsgroups subscribe lt list name here gt waddayatink unsubscribe lt list name here gt ditto end The groups currently are unmoderated so please confine your comments and inquiries to those concerning robot operation and programming 1 8 3 SRI Saphira Web Pages Saphira is under continuing active development at SRI International SRI maintains a set of web pages with more information about Saphira including e tutorials and other documentation on various parts of Saphira e class projects from Stanford CS327B Real World Auto
191. rectory if it doesn t The load directory is used as a prefix on relative path names absolute path names are always loaded with no modification Parameter files for different robot servers can be loaded with the sfLoadParamFile function Since Saphira clients autoload the correct parameter file when they connect to a robot server the user should call this function only in special circumstances The load directory is in sfParamDir which is set by default to the directory params at the top level of the Saphira distribution A Saphira client if it is connected to the simulator can cause the simulator to load a world file through the sfLoadWorldFile command 8 12 Colbert Evaluator Functions Several library functions add functionality to the Colbert evaluator by linking the evaluator to native C functions variables and structures For examples see Section 4 on the Colbert language int sfAddEvalFn char name void fn int rtype int nargs int sfAddEvalVar char name int type void v int sfAddEvalConst char name int type int sfAddEvalStruct char name int size char ex int numslots These functions all return the Colbert index of the defined Colbert object Generally this index is not useful in user programs and can be ignored The exception is the sfAddEvalStruct function which returns the type index of the Colbert structure sfAddEvalFn makes the native C function fn available to Colbert as name The
192. relative to a geometric world model and a set of action routines that map robot states to control actions Registration routines link the robot s local sensor readings to its map of the world and the Procedural Reasoning System sequences actions to achieve specific goals The agent interface links the robot to other agents in the Open Agent Architecture 2 2 1 Representation of Space Mobile robots operate in a geometric space and the representation of that space is critical to their performance There are two main geometrical representations in Saphira The Local Perceptual Space LPS is an egocentric coordinate system a few meters in radius centered on the robot For a larger perspective Saphira uses a Global Map Space GMS to represent objects that are part of the robot s environment in absolute global coordinates The LPS is useful for keeping track of the robot s motion over short space time intervals fusing sensor readings and for registering obstacles to be avoided The LPS gives the robot a sense of its local surroundings The main Saphira interface window Figure 2 2 displays the robot s LPS In local mode Saphira Software Manual from the Display menu the robot stays centered in the window pointing up and the world revolves around it Keeping the robot fixed in position makes it easy to describe strategies for avoiding obstacles going to goal positions and so on Structures in the GMS are called artifacts and repr
193. return type of the function is rt ype and the number of parameters is nargs The additional arguments are the types of each of the parameters There is a maximum of seven parameters to a Colbert function Functions with variable number of parameters should set nargs to the negative of the number of fixed parameters and give the types of the fixed parameters 116 Saphira Software Manual sfAddEvalVar makes a native C variable of type type available to Colbert as name A pointer to the variable should be passed in v as type fvalue For example if the variable is myVar use fvalue amp myVar The value of the C variable can be modified from Colbert sfAddEvalConst defines a constant in Colbert with name name and type type There should be one additional argument which is the constant value either an integer floating point number or pointer sfAddEvalStruct makes a native C structure available to Colbert with name name The size of the structure in bytes should be given in size A pointer to an example structure should be passed in ex The number of structure elements is given by numslots The additional arguments are triplets describing the elements in any order A sample element description x amp ex x SfFLOAT Here x is the Colbert name of the element amp ex x is a pointer to the example element and sfFLOAT is an integer describing the type of the element This function returns the Colbert inde
194. ring activity running in parallel with a movement activity Here both activities are active and performing some task In other cases it may be useful to sequence a set of activities waiting for one to complete before starting another A parent activity controls the sequence by starting each sub activity in turn Colbert supports an execution model in which activities may be invoked as children of an executing activity The technical term for this is hierarchical task decomposition an important method for robot control Consider the task of moving an object from one place to another It s natural to decompose this into three subtasks picking up the object going to the destination and dropping the object In Colbert we would write the activity of Figure 4 5 act move_object int dest start pickup start goto dest start drop Figure 4 5 An activity with subactivities The subactivities pickup goto and drop are executed in turn The move_object activity stops at each until it finishes then goes on to the next This default execution model is the same as for primitive actions The hierarchical structure of activities is important for signals Any signal sent to a parent is reflected to its active children For example if an activity is interrupted all of its children also receive interrupt signals This means that any behaviors or direct motion commands are suspended Similarly if an activity is resumed all of its suspende
195. rnto untrace update waitfor 4 9 4 Types The type system of Colbert is simplified from ANSI C The predefined types are LNE sfINT float sfFLOAT void sfVOID string sfSTRING acs SPACAN behavior sfBEHAVIOR vonata Sie WR The first six are basic types Note that there is no double type all floating point numbers in Colbert are single precision This decision was made to keep all types the same size on 32 bit machines For the same reason there areno char orbitfield types Users can always provide access to C data with non Colbert types by writing native C functions to convert them to Colbert types string is equivalent to char but is atomic i e str is illegal if str is a string The last sfPTR is a convenience definition for a generic pointer The sfACTIVITY and sfBEHAVIOR types are special basic types for activities and behaviors similar to functions Activity schemas are defined with the act keyword Section 4 9 12 Behavior types are not input by Colbert instead behaviors are defined using the behavior compiler Section 5 and made available in Colbert with the sfAddEvalVar or sfAddEvalConst functions Section 4 10 3 There are function pointer types but the user has no access to them from Colbert so they are omitted here It is often necessary to refer to Colbert types from C code e g when defining C functions for Colbert All types in Colbert have a corresponding C index an integer so they can be referr
196. rrent sonar readings and decide to slow down or turn the robot As of Saphira 6 x behaviors are treated as a type of activity and are invoked and disabled using the same commands as activities In particular while the behavior window still exists for displaying information about behaviors you cannot turn a behavior on and off from this window Instead use the activities window Also we recommend starting and controlling behaviors using Colbert which provides a convenient interface to behavior activation and a uniform view of behaviors direct actions and activities 5 1 Behaviors and Fuzzy Control Every behavioral control scheme must decide on representations for the output action and must include a method for arbitrating among competing outputs when several behaviors want to control the robot In Saphira we use fuzzy control rules to define output actions and competing outputs are merged based on priorities and degree of activation of a behavior A fuzzy control rule maps states of the LPS into control actions for the robot A tutorial on Saphira s fuzzy control system can be found in the Saphira documentation please refer to it for explanations of the concepts referred to here This section describes how to define and execute behaviors in the Saphira system Behaviors are specified using the behavior grammar which simplifies the task of writing behavior control rules Specifications in the behavior grammar are translated into C code by
197. rs void void sfRobotCom int com void sfRobotCom2Bytes int bl int b2 void sfRobotComInt int com int arg void sfRobotComStr int com char str void sfRobotComStrn int com char str Processes process sfFindProcess char name process sfInitProcess void fn void void sfInterruptProcess process p void sfInterruptSelf void void sfResumeProcess process p void sfSetProcessState process p int int n char name state void sfSuspendProcess process p int n void sfSuspendSelf int n int async 0 OO OO O 00 O0 LO OO OO 1O WO WO WO O I1O Oo 105 int s2 106 int sl int s2 106 95 94 95 94 94 93 93 93 95 95 94 94 93 93 93 93 102 102 102 102 102 102 102 102 Saphira Software Manual Bookmark not defined 133 12 API Reference Processes Predefined void sfInitBasicProcs void void sfInitControlProcs void void sfInitInterpretationProcs void void sfInitRegistrationProcs void Sensor Interpretation wall sfLeftWallHyp wall sfRightWallHyp Sonars float sfFrontMaxRange void sfSetFrontBuffer int n void sfSetSideBuffer int n int sfSonarRange int num int sfSonarNew int num float sfSonarXCoord int num float sfSonarYCoord int num State Reflection struct robot sfRobot int sfStalledMotor int which void sfTargetHead void void sfTargetVel void Vision 134 void draw_blobs void void find_blob void
198. rt Konolige who developed the Pioneer mobile robot platform Saphira operates in a client server environment The Saphira library is a set of routines for building clients These routines perform the work of communications and housekeeping for the robot server And the Saphira library integrates a number of useful functions for sending commands to the server gathering information from the robot s sensors and packaging them for display in a graphical window based user interface In addition Saphira supports higher level functions for robot control and sensor interpretation including fuzzy control behavior and reactive planning systems and a map based navigation and registration system The Saphira client connects to a robot server with the basic components for robotics sensing and navigation drive motors and wheels position encoders and sensors The server handles the low level details of robot sensor and drive management sends information and respond to Saphira commands through a special communications packet protocol we describe in detail in Chapter 7 Some of the server details are robot specific so we encourage you to consult your robot s operation manual and Saphira supplementary materials for details as well The Saphira client library is available for Microsoft Windows NT and 95 and for most UNIX systems SunOS Solaris SGI OSF FreeBSD and Linux Saphira sources and libraries are written in ANSI C There is an Application Programm
199. rt of a larger activity then by exiting with success or failure it can signal other activities of its result Signals are sent by one of the following commands If the optional argument is given it is the instantiation name of the activity or behavior to signal If not the activity signals itself It may seem strange for an activity to send itself some of these signals e g interrupt but it does make sense since the effect of the signal is also communicated to the children The only signal that can t be sent to self is resume since an activity can t send a signal when it is suspended stop lt symbol suspend lt symbol succeed lt symbol reaL lt symbol interrupt lt symb resume lt symbol gt remove lt symbol trace lt symbol untrace lt symbol The stop and suspend commands both put the activity or behavior into a suspended state where no evaluation is performed The interrupt command is similar but instead signals an interrupt state in which the activity can perform special processing before suspending Within an activity processing for interrupts is indicated by the special oninterrupt label For example in the following code fragment the activity will remove the follow corridor behavior before suspending itself start sfFollowCorridor e p priority 2 iname follow noblock oninterrupt remove foll suspend The resume command resumes an activity or behavior F
200. rted The update code then prints this value in the interaction area and the body code starts In the first statement a is set to 4 and then the turn action starts While the robot turns the activity is polled by the Colbert evaluator each time it is polled the update code is evaluated and the value of a is printed 4 8 2 Colbert Evaluator and Activity States Activity schemas once instantiated are called activities or acts Each act is a micro task that runs in the normal 100 ms control cycle of Saphira But unlike standard micro tasks acts have special facilities for robot control including task completion timeouts hierarchical invocation and signaling When an activity schema is invoked it is added to the micro task schedule On each cycle of the scheduler the act is given to the Colbert evaluator for evaluation The act s current state is the statement that will be executed next The evaluator starts evaluating statements starting from the current state until it hits a break condition at which point it returns control to the scheduler Thus each act gets a small amount of computation time on each cycle and its current state keeps track of where execution should resume The state of an activity may be retrieved with sfGetTaskState function see Section 4 8 4 Break conditions are designed to fit naturally into the execution cycle of acts Typically an act will perform some simple computations then invoke a robot action behavio
201. s cannot be relinked or reloaded unless they are first unloaded From Colbert use the unload command to unload a DLL that you are going to relink 51 Saphira Software Manual 5 Behavioral Control There are two different ways of controlling robot motion The direct motion commands were introduced in Sections 2 2 2 and 4 7 Direct motion control is appropriate for moving the robot through simple sequences of action e g the BumpAndGo activity backs and turns the robot when it bumps into something But there are also cases where the trajectory of the robot must satisfy complicated demands from the task and various maintenance policies For example in navigating from one room to another in an office environment the trajectory in large part is defined by goal positions at corridor intersections The robot should achieve these positions as quickly as possible subject to safety and power considerations On a more local scale the robot should avoid obstacles and respond to contingencies such as closed doors or blocked corridors One approach to complex control is to decompose the problem into a set of small actions to accomplish particular goals which can then be combined into a more comprehensive control strategy Each such small action with its associated goal is called a behavior A behavior looks at some set of sensor information and outputs a desired action based on its goal For example an obstacle avoidance behavior might look at the cu
202. s_state 8 J ouspen ror 9 cicks break 8 5 2 State Inquiries The state of a micro task can be queried with the following functions int sfGetProcessState sfprocess p int sfGetTaskState char iname int sfSuspended sfprocess p 101 8 Saphira API Guide int sfTaskSuspended char iname int sfFinished sfprocess p int sfTaskFinished char iname These functions come in two varieties those that take a micro task pointer as an argument and those that take an instantiation name The latter first look up the micro task in the task list using the instantiation name sfGetProcessState returns the state of the process as an integer if it exists else it returns 0 sfSuspended is 1 if the micro task is suspended and 0 if it is active sfFinished ts if the task has completed to a success failure or timeout state it is 2 if the micro task is not on the scheduler s list and it is 0 if the micro task is still active 8 5 3 Micro task Manipulation When instantiating a micro task give it a unique string name and later refer to it by name or pointer The following Saphira functions initiate suspend and resume micro tasks sfprocess sfInitProcess void fn void char name The sfInitProcess function starts up a micro task with the name name and function fn and returns the micro task instance pointer which can be used in micro task manipulation functions There is no corresponding funct
203. st of the items likely to be found in the robot s LPS Section 8 6 It is a bird s eye view of the environment around the robot The LPS may be switched between a robot centric display and global coordinates using the Display Local menu item The main Saphira window components include 2 3 3 1 Robot icon The robot icon in the center of the screen shows the robot in relation to its environment If in local view the LPS appears in robot centric coordinates the robot remains at the center of the screen and the environment moves around it In GMS global mode local mode off the environment becomes fixed and the robot icon wanders around the screen The size of the robot icon is controlled by the RobotRadius and RobotDiagonal values in the robot s parameter file Section 10 13 2 14 Saphira System Overview 2 3 3 2 Sonar readings Accumulated sonar readings appear onscreen as small open rectangles Current sonar readings are slightly larger open rectangles The number of sonar readings accumulated can be set by the user see Section 8 6 1 for more information about the buffers 2 3 3 3 Control point The elongated open rectangle directly in front of the robot icon is its heading control point as returned by the server in robot centric coordinates Normally this control point is positioned directly ahead of the robot veering to one side or the other in response to a turn directive from the client The robot adjusts its head
204. t gt move 500 lt cr gt This is an example of a direct motion command which tells the robot to move immediately see Section 8 4 You must type the semicolon to indicate the end of the command just as in C otherwise the evaluator will complain about a syntax error The robot should move forward 1 2 meter 500 mm If you execute this command without connecting to the robot there will be an error message indicating that the command cannot be executed You can try other commands such as turn a complete list of the direct motion commands is in Section 4 7 or you can type he lp movement to have the list printed in the interaction area Utility commands such as help and load do not follow normal C syntax and there is no need for a semicolon You can enlarge or shrink the interaction area by grabbing the separator handle located at the left between the LPS and interaction windows with the mouse and moving it up or down The next step is to load the sample file First check the current load directory with the pwd command in the interaction area By default it is the directory of the shell from which you started Saphira the default can be changed by setting the environment variable SAPHIRA_LOAD The load directory can be changed with the cd command or you can just give the load path directly in the load command either relative to the current directory or as an absolute path For example if you re in the bin directory just use loa
205. t is in mm either positive forwards or negative backwards If the state reflector is active it sends motion commands to the robot to move the required distance The maximum velocity attained during motion is given by sfSetMaxVelocity in mm sec int sfDonePosition int dist int sfDoneHeading int ang Checks whether a previously issued direct motion command has completed The argument indicates how close the robot has to get to the commanded position or heading before it is considered completed Arguments are in mm for position and degrees for heading On a Pioneer robot you should use at least 100 mm for the distance completion and 10 degrees for angle The robot may not move enough to trigger the completion function otherwise Note that even though the robot may not achieve a given heading very precisely if it is just turning in a circle as it moves forward or backward it will track the heading better float sfTargetVel void float sfTargetHead void These functions return the current reflected values for the velocity and heading setpoints respectively Values are in mm sec and degrees 99 8 Saphira API Guide 8 5 Saphira Multi tasking One problem facing any high level robotics controller is developing an adequate real time base for the many concurrent processes that must be run Rather than depend on the machine OS for this capability we have implemented a simple round robin cooperative scheme that places responsibi
206. t leaves it in a suspended state pending a resume signal Note that the activity schema must be defined with the act command before the start command is executed or an error will result Currently the only way to invoke an activity is to use the activity schema name For behaviors the behavior schema must be defined with the behavior compiler and loaded into Colbert and its name made available with sfAddEvalVar or sfAddEvalConst Activities and behaviors are sent signals with the signal statement signal lt symbol gt The optional argument specifies the instantiation name of an activity or behavior to signal If there is no such activity or behavior an error is issued The available signals are Command Signal Description stop lt symbol gt sfSUSPEND Suspends execution of the activity Execution can be resumed with the resume command suspend lt symbol gt sfSUSPEND Same as stop succeed lt symbol gt sf SUCCESS Causes the activity to finish in the sf SUCCESS state fail lt symbol gt sfFAILURE Causes the activity to finish in the sf FAILURE state interrupt lt symbol gt SfINTERRUPT Interrupts the activity branching to the oninterrupt label if it exists If not the activity stays in the sf INTERRUPT state and no further execution occurs resume lt symbol gt sfRESUME Resumes a suspended or interrupted activity If the onresume label exists starts at this point else starts at the
207. t name Set the environment variable SAPHITRA_COMP IPE to the name of the desired socket before starting the simulator and it will be used instead of the default The simulator window shows which socket it s listening on To connect to a particular socket from the client side set the SAPHITRA_COMP IPE environment variable to the name of the desired simulator socket before trying to connect Under UNIX and Windows NT different users can set these variables in a unique way so that several users logged in to the same machine can start up their private versions of the simulator The simulator can also listen on a tty port for debugging tty access or TCP IP socket for remote machine access In these cases the simulator must be started with command line arguments specifying the type of access There are two choices pioneer tcp pioneer dev ttyl 25 26 The Simulator The first form starts the simulator and listens on a TCP IP socket for network connections from a client On the client side you must specify the network address or network name of the machine the simulator is running on using the set server command or the SAPHIRA_COMSERVER environment variable The second form accepts any argument that is not tcp This argument is assumed to be the name of a tty port and the simulator listens for connections on that port In MS Windows you can start the simulator with command line arguments by using the Run i
208. tem in the Start menu E pioneer server OF x Connect Files Grow Shrink Wake Recenter SSS Client open request World Moran wid 59 Figure 3 1 A sample window of the simulator 3 2 Parameter File The default operating parameters for the simulator are for the Pioneer 1 You may reset these working parameters to simulate nearly any mobile robot by constructing then loading a special robot parameter file into the simulator from the Load Files menu Find a variety of prepared parameter files in the Saphira params directory The newly loaded model is active for as long as you run the simulator or until you load another parameter file You prescribe a variety of simulated robot characteristics in a parameter file such as placement of sonars and drive error tolerances Once constructed normally store your parameter file in common text ASCII format in the params directory usually with a p suffix to the filename A sample annotated parameter file listing is in the Appendix A and the parameter file can be found in the Saphira collection as params pioneer p Three important parameters control the amount of error in the simulated robot s motion Table 3 1 Consult the listing in Section 10 for more details Saphira Software Manual Table 3 1 Example drive error tolerance values for a parameters file Error in angular position 0 003 Angular drift with forward movement 3 3 World Description File A world des
209. ter derau em packetmprOCcesst fRemoveTask motor get rid of default motor control S S S S MOPCH kp her connect On e Or ehen imu lacon Orr ODOC sfConnectToRobot sfLOCALPORT sfCOMLOCAL Pe thais is tor the simulator lt 7 sfConnectToRobot sfTTYPORT sfCOM1 this 1s for Proneer void myTask void Statie Me a iz switch process_state case sfINIT Saphira Software Manual if sfIsConnected process_state 10 break case 10 connected sfRobotComInt SfCOMOPEN 1 Roopen t Me mMO Poem Omens MeT sfResetRobotVars reset all app variables sfRobotCom sfCOMPULSE ask for data sfRobotComInt SfCOMVEL 300 move forward at 300 mm sec process_state 20 break case 20 read 100 packets if i gt 100 process_state 30 while sfWaitClientPacket 0 poll for packets Tane sfProcessClientPacket sfReadClientByte a 10 0 sfRobotCom sfCOMPULSE keep asking sfRobotComInt s COMVEL 300 fe VASO ae GOLN sg a i sfSMessage d packets received i sfSMessage X sf Y f sfRobot ax sfRobot ay break case 30 sfRobotComInt SfCOMVEL 0 fe eroa Che Tooo S sfDisconnectFromRobot process_state sfSUCCESS break 6 3 6 The Async Client This client demonstrates asynchronous control of the robot that is outside the micro task loop As in the direct client the startu
210. ter is within delta pixels of the center of the image If no blob is found with these parameters it returns 1000 void draw_blobs void Process for drawing any blobs found by the vision system The blob is drawn as a rectangle centered at the correct angular position and at a range where a surface two feet on a side would produce the perceived image size The size of the rectangle is proportional to the image area of the blob void find blob void Activity for turning left until a blob is found in the center of the image on channel A or until 20 seconds expires 122 Saphira Software Manual void search_and_go_blob void Activity for finding a blob using find_blob on channel A then approaching it Uses sonars to detect when it is close to the blob 123 Saphira Software Manual 10 Parameter Files This section describes the parameter files used by the Pioneer simulator and Saphira client to describe the physical robot and its characteristics 10 1 Parameter File Types There are four parameter files for the Pioneer robots pioneer p psos41x p psos41im p psosat p The 41 refers to PSOS versions equal to or greater than PSOS version 4 1 Early versions of the Pioneer that have not been upgraded to at least version 4 1 should use the pioneer p parameter file These Pioneers do not send an autoconfiguration packet therefore Saphira clients by default are configured for pre PSOS 4 1 robots and will correctl
211. terial is the example clients and shared object files that are defined in the Saphira distribution and in the tutorial documentation at the SRI Saphira website http www ai sri com konolige saphira The sample clients and shared objects are found in the handler src apps directory and they are explained in more detail below 6 1 Host System Requirements Saphira libraries are available for most UNIX systems including SunOS 4 1 3 Solaris 5 x SGI Irix DEC OSF Linux and FreeBSD as well as MS Windows 95 and NT 3 51 and 4 0 For UNIX systems we recommend using the Gnu gcc compiler and linking tools from the Free Software Foundation These tools provide a uniform base for making clients and the sample programs are all made with them In addition if you want to create clients that use any of the graphics or user interface routines you will need the following libraries and headers 1 X11R5 or later 2 Motif 2 0 or later These libraries are not required if you are just compiling shared objects for loading into the Colbert evaluator since the library functions are already present in the client For MS Windows the libraries have been compiled with MS Visual C 4 x tools There is a DLL file and an associated LIB file For the best compatibility we recommend using MSVC 4 0 or later all of the sample clients are given with MAK files for MSVC 4 0 It may be possible to use Borland tools but they have not been tested there may be a problem w
212. till pass three arguments but the first two which are window parameters should be NULL Note that even though windows are not being displayed the Saphira OS is operating and the basic set of micro tasks are managing communication and control include saphira h omitted callback definitions voio meigi agep Cldeie ecto Ay sige aL Ole sfOnConnectFn myConnectEn Ma SG ALSIcSie A COlnial FNEL S sfOnStartupFn myStartupFn Js regs A SiceucicUijs Conen EDESSE NILA Ee Ja erare Uo cas Samira OS and then keep going sfConnectToRobot SfLOCALPORT sfCOMLOCAL fa wnis au rOn the Salam llencoie while sfIsConnected sfPause 100 sfSetVelocity 300 move forward at 300 mm sec row SOs LEOA ast PEENE EO ue ee ue Voll S ERODO t a2 SiclNolooie ay sfPause 1000 DON T USE SLEEP sfSetDHeading 10 sfSetVelocity 0 PO Eo 7 sfPause 4000 sfDisconnectFromRobot Is WIS IAS Cina 4 7 81 Saphira Software Manual 7 Saphira Servers In the Saphira client server model the robot server works to manage all the low level details of the robot s systems including operating the drives firing the sonars and collecting echoes and so on on command from and reporting to a separate client application such as Saphira With Pioneer this is the Pioneer Server Operating System PSOS The capabilities of the Pioneer robot server and its connection to the cl
213. tly cast to integers via an assignment and then printed out At the end of the activity the robot velocity is set to 0 and the client disconnects from the robot At the top level of the file the connect function acts to connect the client to the robot simulator and then the DoPackets activity is invoked A E FEE TE HE TE FE E FE E FE HE R EER HE EERE HE REE EE HH HE packet act routines for connecting and reading packets E E RE E FE HE TE FE E FE E AE EERE EERE EERE EE E E HE HH HH x act DoPackets Wigic abf PAE o lt p NE remove packets remove motor waitfor sfIsConnected timeout 100 if sfIsConnected fail sfRobotComInt SfCOMOPEN 1 J Csi tela RMO Or COmicicolliler sfResetRobotVars je reset eulil zisjo variables z sfRobotCom sfCOMPULSE ask ror ceite sty sfRobotComInt sfCOMVEL 300 move forward at 300 mm sec while i100 if sfWaitClientPacket 1000 wait 1 second for a packet al alepils sfProcessClientPacket sfReadClientByte sfRobotCom sfCOMPULSE keep asking sfRobotComInt SsfCOMVEL 300 fe IkeGig se COsinGis a sfSMessage d packets received i 77 78 Creating Clients and Load Files ESES FRODO cep y sfRobot ay sfSMessage X We Cl xr Wie sfRobotComInt sfCOMVI 5 FES OD the robota sfDisconnectFromRobo Connect local connec to simulator fe se ie WWlakey Pioneer onkant ey NENS
214. token since the parser could not fit the token into the C expression it was trying to form There are several common sources of syntax errors to be aware of gt C constructions not supported by Colbert These include embedded assignments variable initializations the comma operator arrays etc see Section 4 9 1 gt Colbert keywords that are not ANSI C keywords There are many of these e g connect waitfor see Section 4 9 3 gt Functions not defined in Colbert Most C library functions are not initially available in Colbert although you can make them accessible see Section 4 10 2 Using one of these functions will give a syntax error 4 5 Evaluator File Loading Colbert source files may be input from text files using the Load command Any errors in the source are indicated in the interaction window and the file loading is aborted at that point Load files can contain definitions of activities as well as commands to be executed including any commands that can be typed in the user interaction area So for example it s possible to load a file that loads other files load lt file gt Loads file from the current load directory file is actually a path from the current directory e g colbert demo act is a legal file name C syntax does not apply to file names so any non blank characters are allowed With no arguments prints a list of loaded shared object files unload lt file gt Unloads a shar
215. ts and motor micro tasks are turned off so that the user program can take over these functions The packet client comes in two forms a loadable Colbert language file packet act and a compilable native C code file We encourage you to use the Colbert language as it s more understandable and easier to work with and modify The example starts out by defining an activity schema for packet communications DoPackets This activity first turns off the default Saphira packet and motor micro tasks which are invoked by sfStartup It then waits until the client connects to the robot and tells the robot server to open its motor control and start traveling forward at 300 mm sec If the connection does not succeed in 10 seconds the activity exits with failure Note the use of a timeout in the wait for statement to accomplish this After this initialization the activity reads packets in a while loop calling the default packet processor sfProcessClientPacket for each packet Default processing updates the client state reflector in sfRobot so that position integration values are available to the client Every 10 cycles new commands are sent to the robot server to keep the information packets coming and to keep going at the requested velocity Also the robot position is printed in the information area Note that because Colbert has no explicit type casts and the sfSMessage function does not handle floats correctly the robot coordinates are first implici
216. tware Manual X s2 1200 s1 400 o o oo o d 600 return 650 90 Figure 8 3 Sensitivity rectangle for sfOccPlane functions Note that the baseline of sfOccP lane is always a positive number To look to the rear use an xy argument of sfBACK left side is xy sfLEFT right side is xy sfRIGHT As with s OccBox a value of 5000 is returned if there is no sonar reading And to return the coordinates of the nearest point in the rectangle use the sfOccPlaneRet function 8 7 Artifacts Through Saphira you can place a variety of artificial constructs within the geometry of the LPS and have them registered automatically with respect to the robot s movement Generally these artifacts are the result of sensor interpretation routines and represent points and surfaces in the real world But they can also be purely imaginary objects for example a goal point to achieve or the middle of a corridor Artifacts like the robot exist in both the LPS and the global map space Their robot relative coordinates in the LPS x y th can be used to guide the robot locally e g to face towards a goal point Their global coordinates ax ay ath represent position and orientation in the global space As the robot moves Saphira continuously updates the LPS coordinates of all artifacts to keep them in their relative positions with respect to the robot The global positions of artifacts doesn t change of course But the dead reckoni
217. uage 30 Figure 4 2 A sample activity schema definition 35 Figure 4 3 The finite state machine for the patrol activity 37 Figure 4 4 An activity that monitors another 38 Figure 4 5 An activity with subactivities 40 Figure 5 1 The straight up function 57 Figure 6 1 Concurrent execution of Saphira OS and user asynchronous tasks 65 Figure 7 1 Saphira client robot server architecture 83 Figure 7 2 Trapezoidal turning velocity profile 91 Figure 7 3 Saphira aware server internal coordinate system 92 Figure 8 1 Saphira s LPS coordinate system 104 Figure 8 2 Sensitivity rectangle for the sfOccBox functions 106 Figure 8 3 Sensitivity rectangle for sfOccPlane functions 107 Figure 8 4 Geometry of artifact types The defining point for the artifact is shown as a vector with a circle at the origin 110 Figure 8 5 Sample map created from the map file above as shown in a Saphira client 114 Saphira Software Manual 1 Saphira Software amp Resources This Software Manual provides the general and technical details you will need to program and operate your Real World Interface Inc RWI Pioneer 1 available through ActivMedia Inc B14 B21 or ATRV 1 Mobile Robot with Saphira software 1 1 Saphira Client Server Saphira is a robotics application development environment written maintained and constantly updated at SRI International s formerly Stanford Research Institute Artificial Intelligence Center notably under the direction of Dr Ku
218. ver void sfResetRobotVars void Resets the values of all internal client variables to their defaults Should be called after a successful connection void sfRobotCom int com void sfRobotComInt int com int arg void sfRobotCom2Bytes int com int bl int b2 void sfRobotComStr int com char str void sfRobotComStrn int com char str int n These Saphira functions packetize and send a client command to the robot server Use the command type appropriate for the type of argument See Section 7 2 for a list and description of currently supported PSOS commands The string commands send stings in different formats sfRobotComStr sends out a null terminated string its str argument and sfRobotComStrn sends out a pascal type string with an initial string count in this case str can contain null characters The function sfRobotCom2Bytes sends and integer packed from two bytes and upper byte b1 and a lower byte b2 int sfWaitClientPacket int ms int sfHaveClientPacket void Use sfWaitClientPacket to have Saphira listen to the client server communication channel for up to ms milliseconds waiting for an information packet to arrive from the server If Saphira receives a packet 118 Saphira Software Manual within that time period it returns to your application If it times out Saphira returns 0 This function always waits at least 100 ms if no packet is present To poll for a packet use sfHaveClientPacket void sfProc
219. vity schemas one for following down a corridor the other for reacting when the robot bumps into something and the motors stall FindAndFollow is a corridor following activity based on the fuzzy control behavior sfFollowCorridor It starts out by waiting for the current environment of the robot to be a corridor sfInitAwareProcs has the job of updating the environment variables When this occurs it fires up the sfFollowCorridor behavior with a goal position 10 meters ahead of the robot Note that the behavior is started in noblock mode which means the execution of FindAndFollow continues in parallel with the behavior The activity is now in monitor mode checking whether the behavior finishes or the corridor ends If so it removes the behavior and goes back to checking for a new corridor Note the use of waitfor at several points to block execution until certain conditions hold FindAndFollow is started up from within the file using the start command If the activity is interrupted say by double clicking in the Activities window then it first removes the corridor following behavior then suspends itself 71 6 Creating Clients and Load Files Derine sana envy CO ollow tas Current Corridor y act FindAndFollow point e old environment point p f pointe EO GO to NOCORRIDOR a here we hava no Corridor a waitfor sfCurrentEnvironment NULL amp amp sfCurrentEnvironment gt type CORRIDOR wait until we have
220. ware Manual server type and loads its parameters when first connected see Chapter 7 for details so it won t be necessary to load a parameter file into the Saphira application unless you re using a custom configuration You can connect using either the interaction window commands or the menu e Serial port connection to Pioneer radio modem or fixed line In the Saphira interaction window type connect serial to connect on the standard serial port If your radio modem is connected to a different serial port use connect serial lt port gt where lt port gt is the name of the serial port e g dev ttyS1 or COM2 The Connect Serial Port menu item will also work for the standard serial port You can set the standard serial port and baud rate see Section 4 6 for details e Simulator connection If you ve started the simulator it s listening on a local internal port Type connect local which opens the local port to the simulator and starts things up Or choose the Connect Local port menu item e B14 and B21 users Bxx users must start up the Saphira server on a Bxx computer see the instructions that come with the Saphira server software Typically the Saphira server will start listening on the local port Run the Saphira client on the same machine as the server with telnet from a desktop machine and use connect local in the interaction window or the Connect Local port menu item If there is a problem connecting with either the s
221. x of the structure type which should be saved for future reference by the program int sfINT sfFLOAT sfSTRING sfVOID sfPTR int sfSrobot sfSpoint int sfTypeRef int type int sfTypeDeref int type These constants and functions refer to Colbert type indices which are integers The first set of constants are the basic type indices for Colbert the second set are pre defined structures sfTypeRef returns the index of a pointer to its argument while sfTypeDeref returns the index to the type referenced by its argument or 0 if its argument is not a pointer type index void sfAddHelp char name char str char sfGetHelp char name These functions are the C interface to Colbert s help facility SfAddHelp adds the string str asa help string for the Colbert object named name It puts it in alphabetical order so that searching for help entries is easier The help string may have embedded formatting commands such as t and n sfGetHelp returns the help string associated with name or NULL if there is none 8 13 Packet Communication Functions Saphira contains several functions that help you manage communications between your client application and the Pioneer server directly PSOS see Chapter 4 rather than going through the Saphira OS Do not use these functions to parse information packets or send motor control commands if you start up the Saphira OS with sfStartup int sfConnectToRobot int port char name This
222. xample client 73 Saphira maps 115 Saphira processes 97 97 sfInitControlProcs 97 sfInitInterpretationProcs 98 sfInitRegistrationProcs 98 Saphira vision 123 Saphira Local Perceptual Space LPS 10 SAPHIRA_COMPIPE 22 SAPHIRA_COMSERIAL 22 SAPHIRA_COMSERVER 22 SAPHIRA_LOAD 12 22 33 SAPHIRA_SERIALBAUD 22 saphira users See Customer resources save map 17 search_and_go_blob 125 sensor interpretation 98 114 Sensor interpretation routines 11 serial port connecting 13 Server Information packet 89 Server information packet 89 Servers 85 autoconfiguration 91 Pioneer Server Operating System 85 ports 91 See ports position integration 93 sfCOMCLOSE 92 sfCOMDHEAD 92 sfCOMOPEN 92 sfCOMPOLLING 94 sfCOMPULSE 92 sfCOMSETO 92 sfCOMSYNCG 91 sfCOMVEL 92 shut down 91 sonars 94 start up 91 set 33 set_vector_buffer 114 setup_vision_system 124 sfAdd2Angle 112 sfAddAngle 112 sfAddHelp 31 sfAddPoint 111 sfAddPointCheck 111 sfAttendAtPos 61 Saphira Software Manual sfAvoidCollision 59 sfBehaviorControl 56 sfButtonProcFn 97 sfChangeVP 113 sfClientBytes 122 sfCOMCLOSE 92 sfCOMDHEAD 92 sfCOMOPEN 92 sfComPipe 33 sfCOMPOLLING 94 sfCOMPULSE 92 sfCOMRVEL 92 sfComSerial 33 sfComServer 33 sfCOMSETO 92 sfCOMSYNC 91 sfCOMVEL 92 sfCOMVEL2 92 sfConnectToRobot 120 sfConstantVelocity 59 sfCreateGlobalPoint 110 sfCreateLocalPoint 110 sfDisconnectFromRobot 121 sfDoneHeadi
223. y control these robots without explicitly loading a parameter file Pioneer robots with PSOS 4 1 or later send an autoconfiguration packet on connection that tells the Saphira client which parameter file to load Pioneers made before August 1996 use old style motors and these load psos41x p Those made after this date use new style motors and load psos41m p The only difference is in some of the conversion factors for distance and velocity The Pioneer AT has its own parameter file pionat p The only change from psos41m p is that the robot is larger than the other Pioneers There are also parameter files for the B14 and B21 robots from RWI These are b14 p and b21 p 10 2 Sample Parameter File The sample parameter file below illustrates most of the parameters that can be set This is the file psos41lm p An explanation of the parameters is given in the table below vr Parameters for the Pioneer robot 77 New motors ver AngleConvFactor 0 0061359 DistConvFactor 0 05066 VelConvFactor 235332 RobotRadius 220 0 RobotDiagonal 90 0 Holonomic 1 MaxRVelocity 2 0 MaxVelocity 400 0 radians per encoder count diff 2P1 1024 5in PI 7875 counts mm count mm sec count DistConvFactor 50 radius in mm half height to diagonal of octagon turns in own radius radians per meter mm per second Ne Ne Ne Ne Ne Ne vr 7 Robot class subclass a Class Pioneer Subclass PSOS41m Name Erratic These are for seven sonars f
224. y default the initial load directory is the directory of the shell that Saphira was started in The default load directory can be changed by setting the environment variable SAPHIRA_LOAD to a directory The load directory is also available to programs as the API variable sfLoadDirectory whose type is a string Setting this variable causes the load directory to change When started the evaluator will look for a file init act in the initial load directory and load it in This file is used for automatically configuring Saphira on startup 4 5 3 Sample Application Files Sample files that mimic the behavior of the old saphira and direct clients are available in the colbert directory demo act Invokes several behaviors along with some activities bump and go for getting out of stall situations and follow corridor for following a found corridor Some of these activities and behaviors are started in a suspended state double click on them in the Activities window to start them direct act Defines some simple direct motion activities and starts them up Must be connected to a robot or there will be an error when starting the direct motion commands packet act Communicates directly with the robot using the packet protocol 4 6 Communication and Connection Utilities There are several utility commands for setting communication modes and for connecting and disconnecting with the robot Parameters to the connection commands are usually held in libr
225. ystem is started The pat rol activity is implemented as a micro task only part of which is shown here Note the explicit completion testing for the direct actions in contrast to the Colbert implicit waits There are other limitations of micro tasks relative to activities e g no parameters and no timeouts The micro task is initiated using the sfInitProcess function include saphira h void patrol void switch process_state case INIT case 20 sfSetPosition 2000 process_state 21 break case 21 if sfDonePosition 100 process_state 22 break Begi ry void myStartupFn void sfSetDisplayState sfGLOBAL TRUE use the global view void myConnectFn void sfSetMaxVelocity 200 robot moves at this speed S Pinte MOCes si pat hole aie Gola a WOLG ineiiin alinic ciccie Cligic saicepy sfOnConnectFn myConnectFn register a connect function sfOnStartupFn myStartupFn register a startup function SE OCEANON E start up the Saphira window 76 Saphira Software Manual 6 3 5 The Packet Client This client handles low level communication with the robot server It takes advantage of the low level Saphira communication routines which parse packets and put the information into the state reflector structures The Saphira OS is active allowing concurrent execution of micro tasks and activities But the default packet and motor control handlers packe

Software Manual - Research Group on the Foundations of Artificial

Contents

Download Pdf Manuals

Related Search

Related Contents