Skip to content

XBotCore ROS integration

Jump to bottom
Luca Muratore edited this page Oct 14, 2019 · 3 revisions

Table of contents

Introduction

Joint space interface

ROS from the real-time layer

Introduction

The XBotCore framework mostly focuses on providing tools for controlling a generic robot inside a tight real-time loop. However, tasks that do not rely on high rate and delay-free feedback from the robot are probably more suitable for running inside non-real-time (NRT) processes. As a matter of fact, when writing code that can run inside a RT thread, only a small subset of operations that are commonly performed in normal c++ code are allowed.
Some rules of thumb that apply inside the RT domains are the following:

  • dynamic memory allocation should be avoided (i.e. those which cause malloc() or free() to be called)
  • writing to console or to a file must be avoided (except when using special tools like XbotInterface RT logger)
  • performing network communication must be avoided (e.g. ROS topics, services, actions, ...)
  • calling blocking functions should be avoided
  • ....

On the contrary, NRT processes are way more flexible, making the programmer's work a lot easier. For this reason, one of the main goals of XBotCore is to provide a seamless integration with NRT middlewares that are most commonly used in the robotics community. This page is about the integration between XbotCore and the Robot Operating System (ROS) middleware.

XBotCore provides two families of ROS interfaces:

  • a joint space interface, consisting in standard ROS topics that are advertised/subscribed to by XBotCore itself in order to publish the robot state (including sensors) and accept commands
  • tools for using a subset of ROS inter-process communication capabilities from the RT domain

In both cases, no magic is being used, and the previously stated rules of thumb still apply. The trick is to use ROS API from a NRT thread, that communicates with the XbotCore RT thread in a way that RT performance is not deteriorated. What XBotCore does is to wrap this machinery in a transparent and easy-to-use way.

Joint space interface

ROS-powered robots expose to their users an interface that is mainly based on topics. For instance, the robot joint state is usually published to a /joint_state topic through sensor_msgs/JointState messages. The same kind of message can be published by the user on a /command topic in order to control the robot.

With XBotCore we offer a very similar interface to the ROS middleware, the only difference lying in the message type being used. Broadly speaking, in XBotCore we use extended joint state messages that make it possible to perform more flexible control of the robot than it is allowed with standard ROS.

Some details are provided in the following sections.

The joint states topic

XBotCore will publish the robot joint state on a topic characterized as follows:

  • topic name: /xbotcore/joint_state
  • rate: 200 Hz (default value)
  • message type: xbot_msgs/JointState, whose definition is inside the xbot_msgs package

The definition of the message extends the standard JointState as follows:

std_msgs/Header header

# Joint names
string[] name

# Sensed data
float32[] link_position
float32[] motor_position
float32[] link_velocity
float32[] motor_velocity
float32[] effort
uint16[] temperature_motor
uint16[] temperature_driver

# Desired values
float32[] position_reference
float32[] velocity_reference
float32[] effort_reference
float32[] stiffness
float32[] damping

# Fault
uint16[] fault

# Auxiliary field (usually for motor current)
string aux_name
float32[] aux

Robot state publisher (i.e. ROS TF)

Inside the ROS ecosystem it is often the case that a special component (a ROS node) which is called the robot_state_publisher is run in order to publish transforms between all robot links. XBotCore runs such component internally, so that ROS nodes are able to read the robot transforms from the /tf topic. Moreover, the robot URDF is published to the ROS parameter server under the name /xbotcore/robot_description.

The command topic

XbotCore subscribes to a command topic, which is characterized as follows:

  • topic name: /xbotcore/command
  • rate: 200 Hz (default value)
  • message type: xbot_msgs/JointCommand, whose definition is inside the xbot_msgs package

The definition of the message extends the standard JointState as follows:

std_msgs/Header header

# Joint names
string[] name

# Desired values
float32[] position
float32[] velocity
float32[] effort
float32[] stiffness
float64[] damping
uint8[] ctrl_mode

# Auxiliary field
string aux_name
float32[] aux

Messages that are published on this topic are parsed by XBotCore as follows:

  • all fields are interpreted according to the joint serializaton established by the name field
  • any subset of the robot joints can be specified inside the name field
  • the number of values inside each field can be lower than the number of joints specified inside the name field

Direct joint-space control from ROS (example)

In order to perform direct joint-space control from ROS, a few steps are required. This is because XBotCore by default disables control from any NRT middleware in favour of real-time control through RT plugins. In order to enable control from the NRT layer, a special plugin XBotCommunicationPlugin must be enabled by

  • making sure that all RT plugins are stopped
  • enable the XBotCommunicationPlugin by calling the service /XBotCommunicationPlugin_switch with argument true

An example follows

user@pc-name:$ rosservice call /XBotCommunicationPlugin_switch 1
user@pc-name:$ rostopic pub /xbotcore/command xbot_msgs/JointCommand "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
name: ['torso_yaw']
position: [0.5]
velocity: [0]
effort: [0]
stiffness: [0]
damping: [0]
ctrl_mode: [1]
aux_name: ''
aux: [0]"

NOTE: messages should be continuously streamed to the command topic, rather than published once

Imu and force-torque sensors

Standard ROS messages for IMU and FT sensors are published to topics with name /xbotcore/imu/IMU_NAME and /xbotcore/ft/FT_NAME.

ROS from the real-time layer

Supported capabilities

  • publishing arbitrary ros messages from a RT plugin
  • subscribing to arbitrary ros topics from RT plugin
  • providing ros service servers from a RT plugin

Quick start

RT plugin header:

class MyPlugin : public XBot::XBotControlPlugin {

public:

    virtual bool init_control_plugin(XBot::Handle::Ptr handle);

    virtual bool close();

    virtual void on_start(double time);

    virtual void on_stop(double time);

    virtual void control_loop(double time, double period);

private:  

    bool srv_callback(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res)
    {
        _srv_flag.store(req.data);
        return true;
    }
        
    void callback(const std_msgs::Float64ConstPtr& msg)
    {
        _sub_value.store(msg->data);
    }
     
    std::atomic<bool> _srv_flag;
    std::atomic<float> _sub_value;
    XBot::RosUtils::PublisherWrapper::Ptr _pub_rt;
    XBot::RosUtils::SubscriberWrapper::Ptr _sub_rt;
    XBot::RosUtils::ServiceServerWrapper::Ptr _srv_rt;

};

RT plugin implementation:

bool MyPlugin::init_control_plugin(XBot::Handle::Ptr handle)
{
    _pub_rt = handle->getRosHandle()->advertise<geometry_msgs::Point>("/my_adv_topic", 10);
    _sub_rt = handle->getRosHandle()->subscribe<std_msgs::Float64>("/my_sub_topic", 1, &MyPlugin::callback, this);
    _srv_rt = handle->getRosHandle()->advertiseService("/my_service", &MyPlugin::srv_callback, this);

    return true;
}


void MyPlugin::control_loop(double time, double period)
{

    /* Create and fill the message... */
    geometry_msgs::Point msg;
    
    /* Push the message to a queue of to-be-published messages */
    _pub_rt->pushToQueue(msg);
    
    /* If our service was called with a true flag, print the latest float received on our subscribed topic */
    if(_srv_flag.load())
    {
        XBot::Logger::Info("Latest float received on /my_sub_topic was %f \n", _sub_value.load());
    }

}

NOTE: it is important to understand that callbacks are always called from another thread that is running in non-realtime mode (more specifically, the CommunicationHandler). For this reason, the user should make use of appropriate synchronization methods, like mutexes, atomic types, ...

Clone this wiki locally