Developer’s Manual


Gazebo Simulation

Example for a launch file: This launch file would load and set also some start parameters for the gazebo simulation, for example it would start it in a not paused stated (“paused” set to “false”).

        <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find roboy_simulation)/worlds/"/>
        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="true"/>
        <arg name="gui" value="true"/>
        <arg name="headless" value="false"/>
        <arg name="debug" value="false"/>
        <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>

Example for a world ( file: In this case the world file contains a ground plane, the legs with upper body roboy model and a light source, a sun.

<world name="default">

<!-- A ground plane -->
<!--PabiRoboy -->
<!--Sun -->

<!-- Focus camera on tall pendulum -->
<gui fullscreen='0'>
    <camera name='user_camera'>
        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>

Model Configuration

If you want to see a camera feed from a gazebo simulation you need to have a camera sensor that captures images and publishes them via messages over a ros bridge. Those messages a standard sensor messages. You can refer to a gazebo plugin that has already been implemented. It is re- commended to attach this sensor to a position close to the models head because you want to its POV to maximize the POV experience. To implement such a thing, just open the model.sdf of the specific model you want to have in the simulation and add the following section.

<sensor type="camera" name="camera">
      <camera name="head">
        <pose>0 1.25 0 -1.5707963267948966 -1.5707963267948966 0</pose>
          <!-- Noise is sampled independently per pixel on each frame.
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1]. -->
      <plugin name="camera_controller" filename="">

The pose determines where the camera will be looking at and which perspective it will be publishing messages from. In order to publish images the camera sensor needs a plugin attached to it, in this case its a standard plugin-in, the ros camera from the gazebo library. The width and height tag determine the resolution of the published images, the update rates is crucial to how many images are sent in one second (25 means, 25 updates per second).

In order to control Roboy in the simulation via ROS messages, the respective model needs to have the suiting plugin.

#include "roboy_simulation/BeRoboyPlugin.hpp"
#include <math.h>

using namespace std;
using namespace gazebo;


BeRoboyPlugin::BeRoboyPlugin() : ModelPlugin() {}


void BeRoboyPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
        // get the model
        model = _parent;
        // bind the gazebo update function to OnUpdate
        updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&BeRoboyPlugin::OnUpdate, this, _1));
        // get all joints and the initial pose
        physics::Joint_V jointVector = model->GetJoints();
        initPose = model->GetWorldPose();

        // Init ros if it is has not been initialized
                int argc = 0;
                char **argv = NULL;
                ros::init(argc, argv, "BeRoboy");

        // Create ros node
        nh = ros::NodeHandlePtr(new ros::NodeHandle("BeRoboy"));
        spinner = boost::shared_ptr<ros::AsyncSpinner>(new ros::AsyncSpinner(1));

        jointCommand_sub = nh->subscribe("/roboy/middleware/JointCommand", 1, &BeRoboyPlugin::JointCommand, this);
        setPosition_sub = nh->subscribe("/roboy/middleware/Position", 1, &BeRoboyPlugin::SetPosition, this);
        pose_pub = nh->advertise<roboy_communication_middleware::Pose>("/roboy/simulation/"+ _parent->GetName() +"_pose", 1);
        hip_sub = nh->subscribe("/roboy/middleware/DarkRoom/sensor_location", 1, &BeRoboyPlugin::DarkRoomSensor, this);
        for(auto joint = jointVector.begin(); joint != jointVector.end(); joint++)
                // Test if joint type is revolute
                if((*joint)->GetType() != 576)
                // replace whitespace with underscore in the names
                string _modelName = model->GetName();
                string jointName = (*joint)->GetName();
                string _jointName = jointName;
                boost::algorithm::replace_all(_modelName, " ", "_");
                boost::algorithm::replace_all(_jointName, " ", "_");
                jointAngles[jointName] = (*joint)->GetAngle(0).Radian();

void BeRoboyPlugin::publishPose()
        roboy_communication_middleware::Pose msg;
        for(auto link:model->GetLinks()){
                math::Pose p = link->GetWorldPose();

void BeRoboyPlugin::JointCommand(const roboy_communication_middleware::JointCommandConstPtr &msg){
        for(uint i=0;i<msg->link_name.size();i++){
                jointAngles[msg->link_name[i]] = msg->angle[i];

void BeRoboyPlugin::SetPosition(const roboy_communication_middleware::PositionConstPtr &msg){
        math::Vector3 pos(msg->x, msg->y, msg->z);
                gazebo::math::Pose p(pos, initPose.rot);
        initPose = p;

void BeRoboyPlugin::DarkRoomSensor(const roboy_communication_middleware::DarkRoomSensorConstPtr &msg)
        int hipIDPos = -1;
        for(int i = 0; i < msg->ids.size(); i++)
        // hip id of the sensor should be 4
        if(msg->ids[i] == hipID)
                hipIDPos = msg->ids[i];
        if(hipIDPos == -1)

        // move the position of the model
        math::Quaternion modelRot = model->GetWorldPose().rot;
        math::Vector3 modelPos = math::Vector3(msg->position[hipIDPos].x, msg->position[hipIDPos].y, msg->position[hipIDPos].z);
        initPose = math::Pose(math::Pose(modelPos, modelRot));

void BeRoboyPlugin::OnUpdate(const common::UpdateInfo &_info)
        // make the model stationary
        // set velocity and force to zero and force for every saved joint and set angle to saved value
        for(auto it = joints.begin(); it != joints.end(); it++)
                model->GetJoint(*it)->SetVelocity(0, 0);
                model->GetJoint(*it)->SetForce(0, 0);
                model->GetJoint(*it)->SetPosition(0, jointAngles[*it]);
        initPose = model->GetWorldPose();


Unity Scene

In Unity you need to establish a Rosbridge in order to be able to communicate with the various types of Roboy, e.g. the simulation one or the real one. Both of them are sending their camera feed as Image messages of the type sensor_msgs/Image. Therefore you need also a suiting subscriber in Unity to be able to receive the messages correctly and parse them afterwards in the right manner.

Image message in Unity

namespace ROSBridgeLib
        namespace sensor_msgs
                public class ImageMsg : ROSBridgeMsg

                        public ImageMsg(JSONNode msg){...}

                        public ImageMsg(HeaderMsg header, byte[] data){...}

                        public byte[] GetImage(){...}

                        public static string GetMessageType(){...}

                        public override string ToString(){...}
                        public override string ToYAMLString(){...}

Image Subscriber in Unity

namespace ROSBridgeLib
        public class RoboyCameraSubscriber : ROSBridgeSubscriber
                public new static string GetMessageTopic()
                        return either "/roboy/camera/image_raw" or "/zed/rgb/image_raw_color"

                public new static string GetMessageType()
                        return "sensor_msgs/Image";

                public new static ROSBridgeMsg ParseMessage(JSONNode msg)
                        //ImageMsg from sensor messages lib
                        return new ImageMsg(msg);

                public new static void CallBack(ROSBridgeMsg msg)
                        ImageMsg image = (ImageMsg)msg;
                        //ReceiveMessage respectively either for the simulation or zed image


After getting the ros bridge connection right and being able to receive image messages as well as reading them correctly the camera feeds should be displayed and rendered at at suited position. For this purpose this unity scene uses a canvas in camera space. Attached to this canvas are various image planes (unity ui images) that can wrap up the received messages.

There is also a View Selection Manager embedded to the BeRoboy™ scene, it is used to fluently switch from one view to another. This manager is responsible for the procedures after a button on the 3D selection menu is pressed. When a certain button is invoked by onClick() the state of various different game objects needs to manipulated (mostly enabling or disabling them). A View Selection Manager always needs the desired references in order to set them, if they not already come preconfigured.

Selection menu in 3D

After clicking on one of the buttons, the View Selection Manager takes the necessary steps to change to the respective view.

Receiving Images Info

Depending on what images you want to receive, you need to set the size of the color arrays in the BeRoboyManager class. m_colorArraySample = new Color [width*height]

In addition you also need to set the texture size in Awake() respectively m_texSample = new Texture2D(width, height)