Translations of this page?:

Q.bo Overview

Q.bo layout

Q.bo layout contents

  • 1. Side switch
  • 2. Charger connector
  • 3. Eyes
  • 4. SRF
  • 5. Microphones
  • 6. Proximity sensor
  • 7. Speakers
  • 8. Wi-Fi antenna
  • 9. Back cover
  • 10. Back security switch
  • 11. Rear panel connectors
  • 12. LCD Screen

XTION plug-in instructions

Attach XTION to Adapter

Attach Adapter to Q.bo

Remove screws from head.

Place the Adapter on top of the head and screw the Adapter’s specials screws.

Finally plug-in the XTION USB to rear panel connectors.

ArduQbo reference guide

ArduQbo is the driver implemented to communicate the Q.Board1 and Q.Board2 with the PC. It is prepared to work over ROS (www.ros.org) so it is needed to have ROS installed in your ubuntu distro to make it function properly. It is recommended to follow the ROS tutorials to understand the ArduQbo guide.

Package Summary

This package is the ROS serial driver for the controller boards of QBO. It is needed to control the movement and the sensors of Q.bo. It is recommended to use a configuration file to set the different parameters of the node.

Qbo_arduqbo ROS Node

This node serves as the ROS driver for the controller boards of Q.bo. It is modular, it means than the systems that are going to be active can be configured with some ROS parameters. The list of the controllers that can be activated is the following:

  • Base controller
  • Battery controller
  • IMU controller
  • Joint controller
  • LCD controller
  • Mics controller
  • Mouth controller
  • Nose controller
  • Distance sensors controller

Each controller subscribes and/or publishes to different topics.

Base Controller

The base controller activates the movement of the base of the robot and the positioning system.

  • Published Topics
    • ~odom (nav_msgs::Odometry): Topic where the robot position in the world is published. The topic name can be changed with a ROS parameter
  • Subscribed Topics
    • ~cmd_vel (geometry_msgs::Twist): Topic where the Twist messages must be send to move the robot base. The topic name can be changed with a ROS parameter

Battery Controller

The battery controller activates the sending of the battery level in a ROS topic.

  • Published Topics
    • ~battery_state (qbo_arduqbo::BatteryLevel): Provides the battery level and the charging status. The topic name can be changed with a ROS parameter.

IMU Controller

The IMU controller activates the sending of the IMU sensor readings in a ROS topic. The IMU sensor must be connected to the Q.board1

  • Published Topics
    • ~imu_state (sensor_msgs::Imu): Provides the inertial movement sensors readings of Q.bo to the ROS system. The topic name can be changed with a ROS parameter.
    • ~imu_state/is_calibrated (std_msgs::Bool): Indicates if the inertial sensors are calibrated or not.
  • Services
    • ~imu_state/calibrate (std_srvs::Empty): This service calibrates the inertial sensors of Q.bo.

Joint Controller

The joint controller activates the movement of the servos of the robot. It means the neck and eyes servos in Q.bo. The sensors are configured with a pair of ROS parameter described in the parameters section of this node.

  • Subscribed Topics
    • ~cmd_joints (sensor_msgs::JointState): Topic where the sensor_msgs::JointState messages must be send to move the servos of the robot. The topic name can be changed with a ROS parameter

LCD Controller

The LCD controller enables the sending of messages from the PC to the LCD screen of Q.bo.

  • Subscribed Topics
    • ~cmd_lcd (qbo_arduqbo::LCD): The messages sent to this topic are forwarded to the LCD screen of Q.bo. Only the first two lines of the LCD can be changed by the PC.

Microphone Controller

The microphone controller activates the sending of the mics levels in a ROS topic.

  • Published Topics
    • ~mics_state (qbo_arduqbo::NoiseLevels): Provides the microhpones’ levels. The topic name can be changed with a ROS parameter

Mouth Controller

The Mouth controller enables the sending of mouth images from the PC to the mouth of Q.bo.

  • Subscribed Topics
    • ~cmd_mouth (qbo_arduqbo::Mouth): Topic where the qbo_arduqbo::Mouth messages must be send to set the mouth of the robot. The topic name can be changed with a ROS parameter.

Nose Controller

The Nose controller enables the sending of the nose color from the PC to the nose of Q.bo.

  • Subscribed Topics
    • ~cmd_nose (qbo_arduqbo::Nose): Topic where the qbo_arduqbo::Nose messages must be send to set the nose color of the robot. The topic name can be changed with a ROS parameter

Distance Sensors Controller

The distance sensors controller publishes a set of sensor_msgs::PointCloud messages to the ROS system. It publishes one message for each activated sensor. The rate, frame and topic of the published messages is configured with the ROS parameters of the sensor controller.

ROS Messages

BatteryLevel

The ROS message BatteryLevel, is defined in the BatteryLevel.msg file located in the msg folder. It is published by qbo_arduqbo, indicated the battery level in volts and the charging status of the robot. The content of BatteryLevel.msg is:

  • Header header
  • float32 level
  • uint8 status

LCD

The ROS message LCD, is defined in the LCD.msg file located in the msg folder. qbo_arduqbo subscribes to it. It provides the possibility to set different messages in the LCD screen of Q.bo. The content of LCD.msg is:

  • Header header
  • string msg

The string msg must contain the message that is going to be set in the LCD screen.

NoiseLevels

The ROS message NoiseLevels, is defined in the NoiseLevels.msg file located in the msg folder. It is published by qbo_arduqbo, indicated the noise levels detected in the mics inputs of the Q.board2. Its values are the 10bit ADC readings of this noise levels. The content of NoiseLevels.msg is:

  • Header header
  • uint16 m0
  • uint16 m1
  • uint16 m2

Mouth

The ROS message Mouth, is defined in the Mouth.msg file located in the msg folder. qbo_arduqbo subscribes to it. It provides the possibility to change the mouth shape of Q.bo. The content of Mouth.msg is:

  • Header header
  • bool[20] mouthImage

Each element of the mouthImage array corresponds to a led of the mouth. If the value is True the led turns on and if the value is false the led turns off. The following table shows the led matrix and the element index of each led. It is assumed in the table that you see the led matrix from the front.

0 - 1 - 2 - 3 - 4 5 - 6 - 7 - 8 - 9 10 - 11 - 12 - 13 - 14 15 - 16 - 17 - 18 - 19

Nose

The ROS message Nose, is defined in the Nose.msg file located in the msg folder. qbo_arduqbo subscribes to it. It provides the possibility to change the color of the Q.bo's nose. It only accept 2 different colors. The content of Nose.msg is:

  • Header header
  • uint8 color

A value of 0 turns off the nose, a value of 2 puts a blue light on the nose and a value of 4 puts a green light on the nose.

ROS Services

The qbo_arduqbo package provides a service to test the different sensors and actuators available in Q.bo.

Test

The content of the Test.srv file is:

  • int8 SRFcount
  • int16[] SRFAddress
  • bool Gyroscope
  • bool Accelerometer
  • bool LCD
  • bool Qboard3
  • bool Qboard1
  • bool Qboard2

The test service checks the existence of the distance sensors controllers, of the IMU system, of the LCD screen and the boards Q.board1, Q.board2 and Q.board3.

ROS Parameters

The node accepts the following parameters:

  • ~port1 (string, default: ”/dev/USB0”): This is the string that indicates the serial port where the first controller board is connected.
  • ~port2 (string, default: ”/dev/USB1”): This is the string that indicates the serial port where the second controller board is connected.
  • ~dmxPort (string, default: ”/dev/USB2”): This is the string that indicates the serial port where the Dynamixel controller is connected.
  • ~timeout1 (float, default: 0.01): This is the maximum time in seconds that the driver waits for the Q.Board connected to the ~port1 to answer a command.
  • ~timeout2 (float, default: 0.01): This is the maximum time in seconds that the driver waits for the Q.Board connected to the ~port2 to answer a command.
  • ~baud1 (integer, default: 115200): This is the baudrate of the serial communication in the port1.
  • ~baud2 (integer, default: 115200): This is the baudrate of the serial communication in the port2.
  • ~rate (float, default: 15.0): This parameter sets the publication rate of the joint states message.
  • ~controlledservos (dictionary, default: {}): This a dictionary that sets the configuration of the servos whose speed is needed to be controlled
  • ~uncontrolledservos (dictionary, default: {}): This a dictionary that sets the configuration of the servos whose speed is not needed to be controlled
  • ~dynamixelservo (dictionary, default: {}): This a dictionary that sets the configuration of the Dynamixel servos
  • ~joint_states_topic (string, default: “joint_states”): This parameters sets the name of the topic where the joint states messages are going to be sent.
  • ~controllers (dictionary, default: {}): This is a dictionary that sets the controllers that are going to be active in the system and its configuration.

Servos dictionary

The elements of the parameters ~controlledservos, ~uncontrolledservos and ~dynamixelservo must be dictionaries. The element name must be the same as the joint name of the servo in the urdf model of the robot (if it exists). Following an example:

uncontrolledservos: {
   left_eyelid_joint: {id: 3, invert: true, max_angle_degrees: 180.0,
    min_angle_degrees: -180.0, range: 360.0, neutral: 1500 },
    right_eyelid_joint: {id: 4, max_angle_degrees: 180.0,
min_angle_degrees: -180.0, range: 360.0, neutral: 1500},
}
dynamixelservo: {
   head_pan_joint: {id: 1, invert: false, max_angle_degrees: 66.0, min_angle_degrees: -66.0, range: 300.0, ticks: 1024, neutral: 570},
   head_tilt_joint: {id: 2, invert: false, max_angle_degrees: 20.0, min_angle_degrees: -37.8, range: 300.0, ticks: 1024, neutral: 512},
}

Two servos are defined in the parameter ~uncontrolledservos and two in the parameter ~dynamixelservo. Its elements are:

  • id (integer, default: -1): Identification number for the servo in the Q.board2 or identification number of the Dynamixel servo in the Dynamixel bus
  • invert (boolean, default: false): Changes the rotation direction of the servo
  • max_angle_degrees (float, default: 180.0): Maximum angle that is going to be sent to the servo in degrees
  • min_angle_degrees (float, default: -180.0): Minimum angle that is going to be sent to the servo in degrees
  • range (float, default: 180.0): Angle range of the servo
  • tics (integer, default: 1800): Number of possible positions of the servo in its range
  • neutral (integer, default: 1500): This value is used to set the 0 degree value of the servo

The ~uncontrolledservos parameter can be changed to ~controlledservos. In its case, a software estimation of the servo position is performed.

Controllers dictionary

The elements of the parameter ~controllers must be dictionaries. The element name is used as a representative name for the controller. Each controller must include the element type. Only a fixed number of types are allowed in the driver. Each of them has its own specific parameters to set some options of the controller. An example of the ~controllers parameter is shown below:

controllers: {
   "j_con": {
       type: joint_controller,
       rate: 15.0,
       topic: /cmd_joints
   },
   "b_con": {
       type: base_controller,
       rate: 15.0,
       topic: /cmd_vel,
       odom_topic: /odom,
       tf_odom_broadcast: true
   }
}

In the example two controllers are configured, the base controller and the joint controller. Following the different controller types and their specific parameters:

  • Base controller
    • type (string, must be: base_controller)
    • rate (float, default: 15.0): Rate at which the odom message is going to be published
    • topic (string, default: cmd_vel): Topic name where the controller subscribes to receive the Twist messages that command the robot movement
    • odom_topic (string, default: odom): Topic name where the odom message is going to be published
    • tf_odom_broadcast (boolean, default: true): If true, the tf transform from odom to base_link is publish
  • Battery controller
    • type (string, must be: battery_controller)
    • rate (float, default: 15.0): Rate at which the battery message is going to be published
    • topic (string, default: battery_state): Topic name where the battery message is going to be published
  • IMU controller
    • type (string, must be: imu_controller)
    • rate (float, default: 15.0): Rate at which the imu message is going to be published
    • topic (string, default: imu_state): Topic name where the imu message is going to be published
  • Joint controller
    • type (string, must be: joint_controller)
    • rate (float, default: 15.0): Rate at which the joint message is going to be published
    • topic (string, default: battery_state): Topic name where the joint message is going to be published
  • LCD controller
    • type (string, must be: lcd_controller)
    • topic (string, default: cmd_lcd): Topic name where the controller subscribes to receive the LCD message that are sent to the LCD screen
  • Mics controller
    • type (string, must be: mics_controller)
    • rate (float, default: 15.0): Rate at which the noise level message is going to be published
    • topic(string, default: cmd_mics): Topic name where the controller subscribes to receive the mic message that that change the microphone source of the Q.board2
    • mics_topic (string, default: mics_state): Topic name where the noise levels message is going to be published
  • Mouth controller
    • type (string, must be: mouth_controller)
    • topic (string, default: cmd_mouth): Topic name where the controller subscribes to receive the mouth message that changes the mouth leds of Q.bo
  • Nose controller
    • type (string, must be: nose_controller)
    • topic (string, default: cmd_nose): Topic name where the controller subscribes to receive the nose message that change the nose color of Q.bo
  • Distance sensor controller
    • type (string, must be: sensors_controller)
    • rate (float, default: 15.0): Rate at which the point cloud message with the sensor readings is going to be published
    • topic (string, default: battery_state): Topic name where the point cloud message with the sensor readings is going to be published
    • sensors (dictionary, default: {}): This element is where the distance sensors are configured. Following is explained with more detail the element contents.

Sensors dictionary

controllers: {
   "sens_con": {
       type: sensors_controller,
       rate: 5.0,
       topic: /distance_sensors_state,
       sensors: {
         front: {
           front_left_srf10: { type: srf10, address: 230, frame_id: front_left_addon },

    front_right_srf10: { type: srf10, address: 226,                            frame_id: front_right_addon }
         },
    floor: {
           floor_sensor: {type: gp2d12, address: 8, frame_id: front_addon },
         }
       }
   }
}
qbo/userguide.txt · Last modified: 2012/12/03 07:33 by sermelo