Upload
nigel-welch
View
214
Download
0
Embed Size (px)
Citation preview
Communicating with CLARAty
Strategies forCommunication and Coordination
Across the Network
Where We Are Today
Caroline Chouinard
presents
The Design and Implementationof the
Functional Layer/Decision LayerConnector Class
Presentation Layout
• Show need for Connector
• Explain parts on DL (comm to and from TDL)
• Explain parts on FL (commanding to rover (sim and real) and feedback)
• Discuss integration (with code examples)
• Propose other alternatives
Why do we need a Connector between DL and FL?
• Defined interface means plug and play– Rocky7, Rocky8, ROAMS, HappySim
• Running DL and FL on separate subnets is easier and sometimes necessary– To be onboard or not? Ground vs. Onboard– DL not compiled for VxWorks
DL to Connector
TDL
Connector
T L
DL
T Talker Thread
L Listener Thread
Socket
Mapping
Creates FL_Interface to allow API for
sending and receivingdata
“Command” maps directly to Rover function
calls
FL_Command
FL_Update
Connector to FL
Connector
LT
T
L
Talker
Listener
FLRover
Data translated to FL_Update
FL_Command translated to
Rover functions
Mapping
Generic Rover to specific
type
Commands turn into actions with
feedback
(contained in FL)
Integration
DL connection to Connector• First create an FL_Interface
• Create an FL_Command and add to queue
• Open socket and send all FL_Commands (one at a time)
• Listen for responses over another socket
• Process any feedback and react accordingly (ex. Low energy update causing replanning)
Connector to FL• Create a generic Rover which is
determined by the compile target
• Create an object mapping from FL_Interface commands to real rover function calls
• Retrieve data manually from the Rover through function calls, as necessary
• Translate data into FL_Update and send across socket
Setting up the system:• Setting the required parameters on the FL• Sending the appropriate DEFINE flags• Choosing the right Rover type
API design:• Base design of the FL_Command and FL_Update (both inherit from FL_Message)
Sample Code and Web Pages
Sample Code and Web Pages (continued)
Start to Finish:• Spawning a Listener thread• Spawning a thread for each command• Choosing the appropriate command mapping• Sample go_to_location command
Commands revisited:• List of all allowed commands
Connector Client
• Simulated DL
• Limited use – no synchronous commanding
• Used for testing commands to the FL through the connector
• See: ConnectorClient Documentation
Propose Other Alternatives - future
Gene Chalfant
Example:Extending CLARAty across Multiple Hosts
The Motor Level Simulation
Example:Extending CLARAty across Multiple Hosts
The Motor Level Simulation
•DIFFERENT:
•Platforms and libraries
•Communication characteristics
•Programming styles and philosophies
Different Communication Characteristics
• Decision support: infrequent requests for highly processed abstracted services (e.g., position estimates, terrain traversal costs), abstract commands (paths, activities)
• Hardware level simulation: small frequent requests (motor control) and steady trickle of status telemetry (encoder counts) and minimally processed, hardware-acquired data (images)
Connector Features
• Provides a “command-line interface” to the Functional Layer for high-level commands
• Connector is the main() object (all other objects are created by Connector)
• Interprets command messages and maps them to object methods using a registry
• Sets up periodic upstream status (telemetry) messages
Connector Issues• Initial design as a message-based conduit between only
two hosts limits scalability• Adding a new command or changing semantics of an
existing command requires modification of Connector class code
• Connector thread is involved in command execution for the entire duration of the command
• Command queueing mechanism adds complexity (channel already queued in transport layer)
• Simulation requires exchange of many small messages, each requiring a queue and thread
A General Solution: Port-based Components
• Components are independent, self-contained computational units that process data
• Ports are the component’s sole means of interacting with the rest of the world
• Connectors transport and adapt data (as untyped byte packets - messages) between components
• The component framework provides any component with communication and scheduling mechanisms
Specific Enhancements
• Cross-host connectiions are inherently scalable: each component sees its own dedicated channel to a remote component on any host
• Message semantics are dealt with by the consuming component; connectors merely transport bytes to the consumer
• Buffers (ports) reside within the consumer: queueing messages inside the connector isn’t necessary
• Messages are immediately copied to the consumer’s buffer - extra “waiting” threads are not necessary
Connector Enhancements using Port-Based Communication
Original ConnectorPort-based Connectors
Single global host-to-host conduit
Connections distributed across all components
Embedded message semantics
Byte transport channel: semantics in consumer
Multiple wait-threads for servicing commands
Connector is done after byte delivery
Central command queueing mechanism
Component queues cmds only if needed
Initial Framework Demonstration: ObstacleMapper
ROAMSSim
DL
ObstacleMapper
FL Comm Framework
Elevation MapGoodness (Traversability) Map
Component Connections Match Deployment Topology
• Each host appears as a port-based component
• Minimal impact adoption - only host-to-host communication is modified
• Remote host library encapsulates communication mechanism as a pair of function calls
Introducing TUIdeas About Structure
The TU Decomposition
AnalysisStack
AnalysisStack
Proprioceptive ControlProprioceptive Control
ActionSelector
ActionSelector
ActionStack
ActionStack
StrategyStrategy
Sensors MotorsThe World
T: Plans & Decisions
Action SelectorObstacle avoidance
NavigationManipulation
Action SelectorObstacle avoidance
NavigationManipulation
StrategyMission planning, Tradeoff Analysis, Contingency planning
StrategyMission planning, Tradeoff Analysis, Contingency planning
Sensors MotorsThe World
U: Sensing and Moving
Analysis StackVision
LocalizationMapmakingEstimation
Analysis StackVision
LocalizationMapmakingEstimation
Proprioceptive ControlMotor servoing, Pose maintenance, Gait
Proprioceptive ControlMotor servoing, Pose maintenance, Gait
Action StackCoordinated motion
LocomotionCamera pointing
Action StackCoordinated motion
LocomotionCamera pointing
Sensors MotorsThe World
Proprioceptive ControlProprioceptive ControlNear horizonReal-time/feedback (msecs)
Data Flow
AnalysisStack
AnalysisStack
ActionSelector
ActionSelector
ActionStack
ActionStack
StrategyStrategy
Medium horizonNavigation
SciActivities(secs, mins)
Far horizonPlanning (hours, days)
Sensors MotorsThe World
Control Flow
AnalysisStack
AnalysisStack
Proprioceptive ControlProprioceptive Control
ActionSelector
ActionSelector
ActionStack
ActionStack
StrategyStrategy
Sensors MotorsThe World
Status
• Phase I: Component, Port, Connector classes about 90% implemented; round-robin scheduling, cross-host and local communication
• ObstacleMapper implemented as a rudimentary component, executes in a standalone testing framework, cross-host communication will initially use shared file system
• Continuing incremental framework design refinement
Sample Code Listings
Component.hHeader file for any module using framework
#ifndef _COMPONENT_H#define _COMPONENT_H
#include <stdlib.h>#include <unistd.h>#include <string.h>#include <pthread.h>
//#include "CL/comm.h"#include "CL/sockets.h"
/*| Port has one-value (overwritable) buffer.| Ports can be many-to-one or one-to-many.| _age is for the Port user's optional use.| _newData is for the Connection's use.| _remote indicates a port on a client host.*/
// different effects of overwrite for many-to-one connectionsenum PortBehavior { minB, maxB, priorityB, sumB };
Component.h (2/7)// Data types handled by this Portenum PortType { shortP, floatP, doubleP, msgP };
typedef unsigned char* byte;
class Port { friend class Connection;
public: Port() {}; Port(char*, int = 1, PortType = shortP, PortBehavior = minB);
void setValue(short); void setValue(float); void setValue(double); void setValue(char*, int);
// accessor must return buffer pointer since type is dynamic unsigned char* getValuePtr() { return _buffer.bytePtr; } PortType getType() { return _type; }
// Aggregate data handlers (eg. serialized class or struct) void loadValueBuffer(char *);
int getAge() { return _age; } bool isDataNew() { return _newData; }
Component.h (3/7)protected: union { // for mult. interp. of ptr w/o cast unsigned char* bytePtr; short* shortPtr; float* floatPtr; double* doublePtr; } _buffer;
char* _name; bool _newData; int _age; // -> 0 each write, incr each cycle int _bufSize; PortBehavior _behavior; PortType _type;};
Component.h (4/7)/*| RepeaterPort is an inter-host Port. It creates a UDP server socket which| listens for client requests for port data transfers.| The software on the remote host can use the updComm/Socket library to| communicate. Currently uses port 2002.*/
class RepeaterPort : Port { public: RepeaterPort(char*, // Port name int = 128, // data buffer size PortType = msgP, // Port data type PortBehavior = minB); // Port behavior private: Socket* _uServer; short _ipPortNum; pthread_t serverThread;};
Component.h (5/7)/*| A list of ports used by Component*/
class PortList { public: PortList(); // allocs 10 ports initially int getLength() { return _length; } int add(Port&); // returns length
private: int _length; Port** _portList; // pointer to buffer Port** _endOfPortList; // pointer to end of buffer};
Component.h (6/7)/*| Components have Ports (which are non-directional), and some code (runOnce())|*/
class Component { public: virtual void runOnce() = 0; int getCycle() { return _cycle; }
private: PortList _portList; // Component has any # of Ports int _cycle; // variable interval scheduling};
Component.h (7/7)/*| Connections connect two Ports*/
class Connection { public: // type of connection enum ConnType { normal, inhibit, inhibitWrite, suppress, overrideIn, overrideOut };
// args: source, dest, type Connection(Port*, Port*, ConnType = normal); Connection(Port*, RepeaterPort*, ConnType = normal); void propagate(); // copy data from source to dest
private: Port* _source; // Ports connected Port* _dest;
enum ConnType _connType; int _priority; // highest priority is 0};
#endif
Test.ccApplication demonstration using framework
#include <stdio.h>#include "Component.h"
/* ===== Test Application Class Declarations ===== */
// Define the talker with a talk portclass Talker : Component { public: Port* talkP; Talker(); // override null constructor void runOnce(); // must declare runOnce()};
// Define the listener with a listen portclass LocalListener : Component { public: Port* listenP; RepeaterPort* repeaterP; LocalListener(); void runOnce();};
Test.cc (2/4)// Define a RepeaterPort which exists outside of a componentRepeaterPort* repeaterP = new RepeaterPort("rport");
/* ==== Test Method Definitions ===== */
// Construct the ports as integer portsTalker::Talker() { talkP = new Port("talker", sizeof(short));}
LocalListener::LocalListener() { listenP = new Port("listener", sizeof(short));}
// Define the behavior of the talkervoid Talker::runOnce(){ // Talker only generates a constant value talkP->setValue((short)3); printf("I said 3!\n");}
Test.cc (3/4)
// Define the behavior of the listenervoid LocalListener::runOnce(){ // LocalListener is only smart enough to know to expect an int int input = *(int*)listenP->getValuePtr();
printf("I heard %d.\n", input);}
Test.cc (4/4)/* ===== Demonstration Program ===== */
int main() {
// Define a talker and listener, and their connections Talker t; LocalListener l; Connection c(t.talkP, l.listenP); Connection r(t.talkP, (Port*) repeaterP);
// Rudimentary & explicit scheduler for (int i=0; i<2; i++) { printf("\n=== Cycle %d ===\n", i);
// Run each component once t.runOnce(); l.runOnce();
// Run each connection once c.propagate(); r.propagate(); } while(1); // Allow checking of RepeaterPorts return 0;}
ObstacleMapper.h
#include "MapDouble.h" // for Map class of doubles#include "nav_grid.h" // for Grid class#include "nav_rover.h" // for Rover class#include "JPLPic.h" // for output elevation map#include <unistd.h>
class ObstacleMapper : Component{public: ObstacleMapper(float gridCellSize = 0.2, // m float roverWheelRad = 67, // = .3*maxObstHeight float roverWidth = 0.84,// m float roverLength = 0.84, float roughnessPercentage = 50); void runOnce();
/*| Rudimentary typed "ports": need to be changed to Port objects*/
JPLPic* goodnessMapPic; // these are essentially JPLPic* certaintyMapPic; // the desired output ports
Grid* gestalt; // have to use these for now Rover* rover; // for communication
MemoryManager* mm; // Should be a singleton};
ObstacleMapper.ccWrapper class for Gestalt Grid class
/*| ObstacleMapper.cc|| Wrapper for Grid object implementing Gestalt algorithm. Gestalt developed by| Mark Maimone.|| Gene Chalfant, May 22, 2002*/
#include "ObstacleMapper.h"
ObstacleMapper::ObstacleMapper(float gridCellSize, // default = 20 cm float roverWheelRad, // =.3*maxObstHeight float roverWidth, // millimeters float roverLength, float roughnessPercentage) {
// Set up MemoryManager options mm = new MemoryManager; mm->SetFlags(MM_FLAG_ALLOW_SYS_FALLBACK); mm->SetVerbosity(0);
// The active Grid object constitutes the Gestalt navigation code gestalt = new Grid(-20, -20, 20, 20, // bounds of map in meters
// (minx, miny, maxx, maxy) gridCellSize, gridCellSize, // cell size in meters (x,y) mm); // new arg (memory manager)
ObstacleMapper.cc (2/2)// Grid object requires an external Rover object rover = new Rover (1.0, 1.0, 1.0, "Rocky8", mm); rover->SetRoverDimensions (roverWheelRad, // wheel radius roverWidth, // body width roughnessPercentage, roverLength, 1);}
void ObstacleMapper::runOnce() { // Make the Grid object process the input if (gestalt->GeneratePlanes (rover) != NO_ERR) printf("GeneratePlanes failed!\n"); else if (gestalt->GenerateEvaluations (rover, 1, 1) != NO_ERR) printf("GenerateEvaluations failed!\n");
// Write ASCII goodness map to stdout gestalt->ShowGoodnessMap( stdout, rover, 1, 0, 0 );
// Extract the maps to public pointer ports goodnessMapPic = gestalt->ExtractGoodnessMap(0); certaintyMapPic = gestalt->ExtractCertaintyMap(0);
// For D*/D-Layer, goodness must be inverted and scaled from 0:255 to 2:252 // (so, multiply by -1 * <newRange>/<fullRange> (gives -250:0) and add 255 - 3) goodnessMapPic->LinearOperator(-(250.0/255.0), 252.0);}
Extra Slides
Strategy Layer
• Mission planning and objectives
• Human control
• High-level value judgements
Analysis Stack
• Abstraction of sensor data
• Estimation, localization
• Mapping services
• Vision processing
Action Stack
• Realization & deabstraction of coordinated motions
• Multi-rover operations
• Example: Locomotor
Action Selector
• Resource/Cost analysis
• Search
• Voting
• “Choosing the best course of action”
Proprioception Layer
• Tight feedback loops within the machine
• No direct environmental interaction
• Raw sensors, encoders
• Example: motor servoing
ObstacleMapper Demonstration Implementation
• Obstacle Mapper functionality developed as a component: will require data transfer with ROAMS simulator (or other source of local elevation maps) and with Decision Layer (consumer of goodness (traversability) maps)