This domain provides drivers for all SCITOS robots and their different modules.
Configuration files
SCITOS robots can be equipped with different kinds of sensors and optional components. For this reason, a global configuration file is used to specify the hardware configuration of the robot. The SCITOS driver looks for this configuration file in the following locations:
/opt/SCITOS/SCITOSRobotAttributes.xml
- somewhere else within the
MIRA_PATH
If no such file exists, a default configuration is used.
It is strongly recommended to create such a file for your robot if it does not yet exist!
The configuration file must contain a set of variables, which describe the following properties of the robot:
- type of the robot
- type of the front laser range finder
- type of the rear laser range finder
- type of sonars
- type of the cover
- color of the robot
- existence of a SCITOS HMI
A typical configuration file looks like this:
<root>
<!-- Type of robot [SCITOS-G3, SCITOS-A5, SCITOS-G5, SCITOS-G6, SCITOS-G6-small] -->
<var robot="SCITOS-A5" />
<!-- Type of the CAN bus [PCAN, MLCAN] -->
<var canType="PCAN" />
<!-- CAN bus device (default=[PCAN: /dev/pcan32, MLCAN:/dev/ttyUSB2]) -->
<var canDevice="/dev/pcan32" />
<!-- Type of mounted front laser [SickS300, LeuzeRS4, Hokuyo-URG-04LX, LZRU901] -->
<var frontLaser="SickS300" />
<!-- Device of the front laser (default=/dev/ttyUSB0) -->
<var frontLaserDevice="/dev/ttyUSB0" />
<!-- Type of mounted back laser [none, SickS300, LeuzeRS4, Hokuyo-URG-04LX, LZRU901] -->
<var rearLaser="none" />
<!-- Device of the rear laser (default=/dev/ttyUSB1) -->
<var rearLaserDevice="/dev/ttyUSB1" />
<!-- Does the robot have sonar -->
<var sonar="false" />
<!-- Body
type for G6 robots [normal, tray] -->
<var bodyType="normal" />
<!-- Cover
type for A5 and G5 robots [2008, 2011, 2012]
2008 = Older robots with cover with stabilizers
2011 = Older robots with cover without stabilizers
2012 = Newer robots with more field of view for the laser. -->
<var coverType="2012" />
<!-- Cover color r g b -->
<var color="1 0 0" />
<!-- Only for G5 robots. Does the robot have a human machine interface (display, head) -->
<var hmi="false" />
</root>
When this file exists the driver automatically chooses the right config file for your robot. For each robot at least two files exist. The SCITOS-XYZ.xml
file (XYZ == your robots type) contains parameters for all modules. The SCITOS-XYZ-model.xml
file contains the model (for visualization and collision avoidance) of the robot. Additionally there may be a file describing the to be ignored intervals of the attached laser range finder for your robot (e.g. intervals where the range scanner "sees" the robot).
If you need to change the parameters of a module do not edit the provided configuration file. Please use the <parameter> tag in your configuration file to alter parameters of the robots modules. E.g. if you want to change the maximum speeds of the robot you need to add the following lines to your config file (assuming the SCITOS unit is in namespace robot):
<parameter name="/robot/Robot.RobotModelProvider.Model">
<MaxForwardVelocity>0.5</MaxForwardVelocity><!-- was 1.0 -->
<MaxBackwardVelocity>-0.2</MaxBackwardVelocity><!-- was -0.4 -->
<MaxRotVelocity>80</MaxRotVelocity><!-- was 120 -->
</parameter>
The next example shows how to alter a parameter of the module 'Localization':
<parameter name="/robot/Robot.Modules.Localization">
<RFIDReaderEnabled>true</RFIDReaderEnabled>
</parameter>
In the following sections each available module is described in more detail. For each module the name as used in the config file is given. To alter parameters of a module just replace 'Localization' in the above example by the name of the module you want to access.
A single configuration file is provided for starting up the robot. It can be started with
> miracenter /path/to/SCITOS/etc/SCITOSDriver.xml
The SCITOSManager
The SCITOSManager class of this domain manages (creates, observes and destroys) all SCITOSModules. The modules will be automatically discovered at runtime when they emit a CANOpen node status. Whenever the manager gets such a status packet from a not yet know node it will retrieve the CANOpen informations for that node. It then searches for a module plugin that matches the product id given in the node info. If such a plugin is found it will be created. Parameters for that plugin that were contained in the config file at startup will be used to initialize the module plugin. In the following sections the different modules will be described in detail.
The driver itself also provides a rigid model of the robot (e.g. for visualization or algorithms that need the footprint or bounding boxes of the robot) and also a drive model (e.g. for calculating movement or motion sampling)
Channels
Subscribed
Published
Services
Published
- IRobotModelProvider
- IRigidModelProvider
- mira::model::RigidModelPtr getRigidModel()
- Return the currently used rigid model
- void publishJoint(const std::string&, float, const Time& timestamp)
- Publishes state of a joint
Parameters
- RobotType (std::string)
The type of the robot (unused)
- RobotModelProvider (boost::shared_ptr<mira::robot::RobotModelProvider>)
The used robot model provider
- DeadModuleTimeout (mira::Duration) [property]
Timeout for removing dead modules
- Modules (list of mira::robot::SCITOSModule) [property]
Contains all discovered modules as child with their respective name
Transforms
The manager class creates all links and publishes all transforms that are contained in the used robot/rigid model.
List of modules
Auxiliary Charger for lead (Pb) gel battery powered robots
Additionally to the normal charger that allows the robot to be charged by cable, the auxiliary charger adds the ability to charge the robot via floor contacts.
Channels
Published
- charger/AuxChargerStatus (uint8)
The status flags of the auxiliary charger
- Bit 0: 1=connected, 0=unconnected
- Bit 1: 1=charging, 0=not charging
- Bit 2: 1=contacts up, 0=contacts not up
- Bit 3: 1=contacts down, 0=contacts not down
- Bit 4: unused
- Bit 5: unused
- Bit 6: unused
- Bit 7: unused
Parameters
Name in config file: AuxCharger
- ContactsDown (bool) [property]
Are charging contacts lowered. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsDown (uint16) [property]
How many steps for moving contacts down. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsUp (uint16) [property]
How many steps for moving contacts up. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsInitDown (uint16) [property]
How many steps for moving contacts down on initialization. (Only robots with charging contacts that can be lowered and raised. Use with care)
Charger for lead (Pb) gel battery powered robots
The charger module is responsible for charging the robots battery.
Channels
Subscribed
- charger/AuxChargerStatus (uint8)
The status flags of the auxiliary charger
Published
- charger/Battery (mira::robot::BatteryState)
The battery state of the robots battery
- charger/ChargerStatus (uint8)
The status flags of the charger
- Bit 0: 1=charging, 0=not charging
- Bit 1: 1=empty, 0=not empty
- Bit 2: 1=full, 0=not full
- Bit 3: 1=derating, 0=not derating (unused)
- Bit 4: 1=charging disabled, 0=charging enabled (only intern)
- Bit 5: 1=constant voltage mode active, 0=constant voltage mode inactive
- Bit 6: 1=constant current mode active, 0=constant current mode not active
- Bit 7: 1=internal error flag set, 0=internal error flag not set
Parameters
Name in config file: Charger
- ChargerPowerMode (uint8) [property]
The power mode of the charger (off=0, on=1, auto=2, standby=3, on_after_recharged=4)
- AutoPowerOn (uint16) [property]
Auto power-on time [min]. Can be used together with ChargerPowerMode=standby to automatically boot the robots PC after the specified time. E.g. AutoPowerOn=10 will start the PC again in 10 minutes (if its not already on)
Charger (second hardware generation)
The charger module is responsible for charging the robots battery and provides a persistent error memory for all hardware errors that occurred. It also includes the functionality of the auxiliary charger.
Channels
Subscribed
- charger/AuxChargerStatus (uint8)
The status flags of the auxiliary charger (Are provided by the charger itself because chargers of the second hardware generation include auxiliary charger functionality)
Published
- charger/AuxChargerStatus (uint8)
The status flags of the auxiliary charger (Are provided by the charger itself because chargers of the second hardware generation include auxiliary charger functionality)
- Bit 0: 1=connected, 0=unconnected
- Bit 1: 1=charging, 0=not charging
- Bit 2: 1=contacts up, 0=contacts not up
- Bit 3: 1=contacts down, 0=contacts not down
- Bit 4: temperature warning
- Bit 5: over temperature
- Bit 6: unused
- Bit 7: unused
- charger/Battery (mira::robot::BatteryState)
The battery state of the robots battery
- charger/ChargerStatus (uint8)
The status flags of the charger
- Bit 0: 1=charging, 0=not charging
- Bit 1: 1=empty, 0=not empty
- Bit 2: 1=full, 0=not full
- Bit 3: 1=derating, 0=not derating (unused)
- Bit 4: 1=charging disabled, 0=charging enabled (only intern)
- Bit 5: 1=constant voltage mode active, 0=constant voltage mode inactive
- Bit 6: 1=constant current mode active, 0=constant current mode not active
- Bit 7: 1=internal error flag set, 0=internal error flag not set
Services
Published
- void clearPersistentErrors()
- Clears persistent error memory (erases all messages)
- uint16 getPersistentErrorCount()
- Get number of persistent errors
- std::vector<mira::robot::PersistentError> getPersistentErrors()
- Get all persistent errors
- std::vector<mira::robot::PersistentError> getPersistentErrors(uint16, uint16)
- Get a range of persistent errors starting at the specified index (first param) with given size (second param)
- mira::robot::PersistentError getPersistentError(uint16)
- Get the persistent error at the specified index
- void savePersistentErrors(std::string)
- Save persistent errors to a file in JSON format
Parameters
Name in config file: Charger
- ChargerPowerMode (uint8) [property]
The power mode of the charger (off=0, on=1, auto=2, standby=3, on_after_recharged=4)
- AutoPowerOn (uint16) [property]
Auto power-on time [min]. Can be used together with ChargerPowerMode=standby to automatically boot the robots PC after the specified time. E.g. AutoPowerOn=10 will start the PC again in 10 minutes (if its not already on)
- ContactsDown (bool) [property]
Are charging contacts lowered. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsDown (uint16) [property]
How many steps for moving contacts down. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsUp (uint16) [property]
How many steps for moving contacts up. (Only robots with charging contacts that can be lowered and raised. Use with care)
- MotorStepsInitDown (uint16) [property]
How many steps for moving contacts down on initialization. (Only robots with charging contacts that can be lowered and raised. Use with care)
Drive (Motor controller)
Coming soon...
EBC
The EBC (External Bus Connector) module provides power for external devices that are attached to the robot. It also provides access to the robots CAN-bus. Each EBC module has two ports that are numbered as follows: 0=top port, 1=bottom port. There are three different voltages that a single EBC-Port can deliver (5V, 12V and 24V). One can also alter the maximum current for each voltage.
Parameters
Name in config file: EBC0..EBCX The name is derived from the slot the EBC module is plugged in counting from left to right.
- Port0_5V.MaxCurrent (float) [property]
Max current for port 0 5V [A]
- Port0_5V.Enabled (bool) [property]
Are 5V enabled at port 0
- Port0_12V.MaxCurrent (float) [property]
Max current for port 0 12V [A]
- Port0_12V.Enabled (bool) [property]
Are 12V enabled at port 0
- Port0_24V.MaxCurrent (float) [property]
Max current for port 0 24V [A]
- Port0_24V.Enabled (bool) [property]
Are 24V enabled at port 0
- Port1_5V.MaxCurrent (float) [property]
Max current for port 1 5V [A]
- Port1_5V.Enabled (bool) [property]
Are 5V enabled at port 1
- Port1_12V.MaxCurrent (float) [property]
Max current for port 1 12V [A]
- Port1_12V.Enabled (bool) [property]
Are 12V enabled at port 1
- Port1_24V.MaxCurrent (float) [property]
Max current for port 1 24V [A]
- Port1_24V.Enabled (bool) [property]
Are 24V enabled at port 1
Head module for SCITOS G3
Coming soon...
Head module for SCITOS G5
The head module provides access to the joints of the head (tilt, pan, eyes) and to the LED lighting system. The LED lighting system consists of 8 groups with 4 LEDs per group.
Parameters
Name in config file: Head
- HeadLightInterval (uint16) [property]
Interval for blinking or flashing mode
- LEDState0 (uint8) [property]
State of LED group 0 (off=0, on=1, blinking=2, fading=3)
- LEDState1 (uint8) [property]
State of LED group 1 (off=0, on=1, blinking=2, fading=3)
- LEDState2 (uint8) [property]
State of LED group 2 (off=0, on=1, blinking=2, fading=3)
- LEDState3 (uint8) [property]
State of LED group 3 (off=0, on=1, blinking=2, fading=3)
- LEDState4 (uint8) [property]
State of LED group 4 (off=0, on=1, blinking=2, fading=3)
- LEDState5 (uint8) [property]
State of LED group 5 (off=0, on=1, blinking=2, fading=3)
- LEDState6 (uint8) [property]
State of LED group 6 (off=0, on=1, blinking=2, fading=3)
- LEDState7 (uint8) [property]
State of LED group 7 (off=0, on=1, blinking=2, fading=3)
- LEDPhase0 (uint16) [property]
Phase shift of the sinus signal used for LED group 0
- LEDPhase1 (uint16) [property]
Phase shift of the sinus signal used for LED group 1
- LEDPhase2 (uint16) [property]
Phase shift of the sinus signal used for LED group 2
- LEDPhase3 (uint16) [property]
Phase shift of the sinus signal used for LED group 3
- LEDPhase4 (uint16) [property]
Phase shift of the sinus signal used for LED group 4
- LEDPhase5 (uint16) [property]
Phase shift of the sinus signal used for LED group 5
- LEDPhase6 (uint16) [property]
Phase shift of the sinus signal used for LED group 6
- LEDPhase7 (uint16) [property]
Phase shift of the sinus signal used for LED group 7
- LEDAmplitude0 (float) [property]
Amplitude of the sinus signal used for LED group 0
- LEDAmplitude1 (float) [property]
Amplitude of the sinus signal used for LED group 1
- LEDAmplitude2 (float) [property]
Amplitude of the sinus signal used for LED group 2
- LEDAmplitude3 (float) [property]
Amplitude the sinus signal used for LED group 3
- LEDAmplitude4 (float) [property]
Amplitude of the sinus signal used for LED group 4
- LEDAmplitude5 (float) [property]
Amplitude of the sinus signal used for LED group 5
- LEDAmplitude6 (float) [property]
Amplitude of the sinus signal used for LED group 6
- LEDAmplitude7 (float) [property]
Amplitude of the sinus signal used for LED group 7
Services
Published
- IHead
- void moveHeadUpDown(float)
- Move the head up/down [deg]
- void moveHeadLeftRight(float)
- Move head left/right [deg]
- void moveEyesLeftRight(float)
- Move eyes left/right [deg]
- void moveEyeLidUpDown(uint8, float)
- Set the opening of left(0), right(1) or both(2) eyes [%]
- void twinkle(uint8, uint32)
- Twinkle with left(0), right(1) or both(2) eyes leaving the eye closed for the given duration
Head module for SCITOS G6
Coming soon...
Localization
The localization module provides a RFID floor reader and a gravity sensor. The RFID reader can be used to localize the robot based on RFID tags in the floor.
Channels
Published
- RFIDFloorTag (uint64)
Tags seen by the RFID reader.
- GravitySensor (float)
Acceleration measurement of the gravity sensor.
Parameters
Name in config file: Localization
- RFIDReaderEnabled (bool) [property]
Is the RFID reader enabled
Main Control Unit
The main control unit combines motor controller, sonar driver, EBC ports and RFID reader in one module.
Channels
Published
- Odometry (mira::robot::Odometry2)
The odometry of the drive system. (Frame: RobotFrame)
- MotorStatus (uint8)
Status flag of the motor controller
- Bit 0: 1=normal mode, 0=not charging
- Bit 1: 1=motor stopped, 0=motor not stopped
- Bit 2: 1=free run enabled (breaks off), 0=breaks on
- Bit 3: 1=emergency button pressed, 0=emergency button not pressed
- Bit 4: 1=bumper pressed, 0=bumper not pressed
- Bit 5: 1=bus error, 0=bus ok
- Bit 6: 1=stall mode, 0=no stall
- Bit 7: 1=internal error flag set, 0=internal error flag not set
- Bumper (bool)
Is emergency stop active (bumper pressed)
- Mileage (float)
The total driven distance since start
- RFIDFloorTag (uint64)
Tags seen by the RFID floor reader. Only available with firmware >= 1.4.0
- sonar/USSensorXX (mira::robot::RangeScan)
XX=0..23 (old sonar system) or XX=0..13 (new sonar system) range scans. One for each sonar sensor.
Services
Published
- ISonar
- void enableSonar(bool)
- Enable/Disable sonar sensors
- IMotorController
- void enableMotors(bool)
- Enables/disable the motors.
- void resetMotorStop()
- Reset the motor stop flags (bumper, emergency stop flags, etc).
- void resetOdometry()
- Reset the odometry to zero.
- void emergencyStop()
- Performs an emergency stop and sets the motor emergency stop flag
Parameters
Name in config file: MainControlUnit
- OdometryInterval (uint16)
Odometry update interval [ms] (>= 50ms)
- ResetOdometryOnStart (bool)
Should odometry be reseted on start
- UseOdometryCorrection (bool) [property]
should odometry be corrected based on OdometryCorrection
- OdometryCorrection (mira::robot::OdometryCorrectionDifferentialDrive)
Parameters for odometry correction
- PersistentFreerun (bool) [property]
Is freerun persistently on (after sending a drive command, freerun will be re-enabled automatically)
- FreerunMode (bool) [property]
Is freerun enabled
- AccelerationUpdateInterval (uint8) [property]
This value defines how often the acceleration or deceleration of the robot can be updated. Small values result to a steeper ramp. Bigger values leads to a more flat ramp and a more soft movement. (Only available with firmware >= 1.8.4)
- WheelPIDKp (float) [property]
Kp part of the PID controller
- WheelPIDKi (float) [property]
Ki part of the PID controller
- WheelPIDKd (float) [property]
Kd part of the PID controller
- Force (int) [property]
The force that is applied to the motors. (Use with care)
- RFIDReaderEnabled (bool) [property]
Is RFID reader enabled (Only available with firmware >= 1.4.0)
- EBC_5V.MaxCurrent (float) [property]
Max current for EBC port 5V [A]
- EBC_5V.Enabled (bool) [property]
Are 5V enabled at EBC port
- EBC_12V.MaxCurrent (float) [property]
Max current for EBC port 12V [A]
- EBC_12V.Enabled (bool) [property]
Are 12V enabled at EBC port
- EBC_24V.MaxCurrent (float) [property]
Max current for EBC port 24V [A]
- EBC_24V.Enabled (bool) [property]
Are 24V enabled at EBC port
- FrontLaser.MaxCurrent (float) [property]
Max current for front laser power supply [A]
- FrontLaser.Enabled (bool) [property]
Is power for front laser enabled
- RearLaser.MaxCurrent (float) [property]
Max current for rear laser power supply [A]
- RearLaser.Enabled (bool) [property]
Is power for rear laser enabled
- USSensorModeSelection (int)
Which US sensor mode is active (none=0, old sensors=1, new sensors=2)
- SonarEnabled (bool) [property]
Is sonar system enabled
- USSensorHG1IgnoredSensors (std::set<int>) [property]
List of unused/masked sensors (0..23). (Only available if USSensorModeSelection=1)
- USSensorHG1LowNoiseMode (bool) [property]
Low or high voltage mode. (Only available if USSensorModeSelection=1)
- USSensorHG1MeasurementInterval (uint16) [property]
Measurement period of the sensors [ms] (>=50ms). (Only available if USSensorModeSelection=1)
- USSensorHG1SensorsPerMeasurement (int) [property]
Number of sensors per measurement (1x24=0, 2x12=1, 3x8=2, 4x6=3, 6x4=4, 8x3=5, 12x2=6, 24x1=7). (Only available if USSensorModeSelection=1)
- USSensorHG2Neighbours (uint32) [property]
Number of passive neighbour sensors (crosstalk). (Only available if USSensorModeSelection=2)
- USSensorHG2GroupMask (uint32) [property]
Mask for active sensors. (Only available if USSensorModeSelection=2)
- USSensorHG2Count (uint32) [property]
Number of active ultra sonic sensors (0..13). (Only available if USSensorModeSelection=2)
Transforms
- RobotFrame
- Is published frequently containing the current odometry values of the drive system
RFID
The RFID reader module provides the functionality to start the robots PC via an authenticated RFID tag.
Channels
Published
- RFIDUserTag (uint64)
Tags seen by the RFID reader.
Services
Published
- uint64 readPowerOnID(uint8)
- Read an id that allows to power on the PC from given index (0..15)
- void writePowerOnID(uint8, uint64)
- Store an id that allows to power on the PC in the given index (0..15)
Sonar
Coming soon...
Status display
The status display is mostly used in SCITOS-G5 robots. It provides fast access to different settings and shows odometry informations. It can be configured to display a user menu entry.
Channels
Published
- StatusDisplayUserMenuEvent (uint8)
Contains the index of the user menu entry that was selected via the dial knob (0..2)
Services
Published
- IStatusDisplay
- void enableUserMenu(bool)
- void setUserMenuName(const std::string&)
- Set the name of the user menu entry in the main menu of the status display
- void setUserMenuEntryName(uint32, const std::string&)
- Set the name of one of the 3 sub menu entries in the user menu of the status display
Parameters
Name in config file: StatusDisplay
- EnableUserMenu (bool) [property]
Is the user menu entry in the status display enabled
- UserMenuName (std::string) [property]
The name of the user menu entry in the main menu of the status display
- UserMenuEntryName1 (std::string) [property]
The name of the first sub menu entry in the user menu of the status display
- UserMenuEntryName2 (std::string) [property]
The name of the first sub menu entry in the user menu of the status display
- UserMenuEntryName3 (std::string) [property]
The name of the first sub menu entry in the user menu of the status display