Create Ned’s digital twin with Webots and ROS

Create Ned's digital twin with Webots and ROS

Note

This tutorial is working from:
The version v3.0.0 of the ned_ros_stack
The version v3.0.0 of Niryo Studio

Introduction

  • On July 2021, Ned was added on Webots. You can have more information on Cyberbotics’ website.

  • By downloading Webots and running Ned’s sample World, you will be able to control its joints and its gripper with your keyboard, and also run a demo. It is highly recommanded to follow our previous tutorial about Webots simulation before reading this one.

What is Webots from Cyberbotics ?

“Webots is an open source and multi-platform desktop application used to simulate robots. It provides a complete development environment to model, program and simulate robots. It has been designed for a professional use, and it is widely used in industry, education and research. Cyberbotics Ltd. maintains Webots as its main product continuously since 1998.”

You can download Webots for free on their website.

Note

To do this tutorial we used “Ubuntu 18.04” and “Webots R2021a”. You will need it for this tutorial.

You can find a sample of Ned on their website.

Goal of the tutorial

In the first part of the tutorial you will learn how to use the digital twin we created to control a real Ned with a simulated one on the robotics simulator Webots. By controlling Ned on Webots, you will be able to send the same commands on the real robot, in realtime.

In the second part of the tutorial, you will learn from scratch how to implement a ROS controller for Ned on Webots using the webots_ros package and how to develop a digital twin between Webots and Ned.

By finishing this tutorial, you will be able to modify the ROS package and do your own projects with Ned, Webots and ROS. You can find the ROS package on our github.

How to use the digital twin

Installing ROS and “webots_ros” Package

The first part of this tutorial consists in installing ROS and the webots_ros package on your computer. To do that you will have to open a new terminal and write the following commands:

cd
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update
sudo apt-get install ros-melodic-desktop-full (if not already installed)
sudo apt-get install python-rosdep
sudo apt-get install python-rospkg
pip3 install rospkg==1.3.0
cd /etc/ros/rosdep//sources.list.d/
sudo rm 20-default.list
sudo rosdep init
rosdep update
sudo apt-get install ros-melodic-webots-ros

Ned is working with Ubuntu 18.04 and ROS melodic. Following these commands you will install ROS melodic (if it is not already done), update your rosdep and install ros-webots dependencies on your computer.

Note

If you want more information about the installation of ROS and the webots_ros package, you can take a look at Webots’ documentation.

Setting the appropriate environment

Now that you have the ros-webots dependencies installed on your computer, you will have to set the appropriate environment.

One thing you can already do is to source the ROS environment properly. To do that, open a new terminal:

cd
source /opt/ros/melodic/setup.bash

Create your ROS workspace

Now, we need to create a new catkin workspace. It is on this workspace that we will put a new ROS package to communicate between Webots and Ned via ROS.

To create your catkin workspace, you just have to add the following lines:

mkdir -p ~/catkin_ned_webots/src
cd ~/catkin_ned_webots/
catkin_make
source devel/setup.bash

Your new catkin workspace is now ready!

Check if the ROS_PACKAGE_PATH environment is properly set up

Before creating a new webots ROS package, you have to make sure that the ROS_PACKAGE_PATH environment is properly set up.

To check that, on the same terminal you created the workspace, you can add this command line:

printenv | grep ROS

With this command you can see all the environment starting with “ROS”. In our case we need to set the ROS_PACKAGE_PATH environment so we can use the following command:

echo $ROS_PACKAGE_PATH

It should return “/home/youruser/catkin_ws/src:/opt/ros/melodic/share”.

If not, use:

export ROS_PACKAGE_PATH=/home/youruser/catkin_ws/src:/opt/ros/melodic/share

Set the WEBOTS_HOME environment

If you have installed Webots in your root repository, your WEBOTS_HOME path should be “/usr/local/webots” and you don’t have to do anything else about it. If your WEBOTS_HOME path is different, you need to write this command in the terminal:

export WEBOTS_HOME=/usr/local/webots

Fill your .bashrc

If you don’t want to have to repeat all the exports and settings each time, you can fill the details in your .bashrc by using the following command in the terminal:

nano ~/.bashrc

Now you can put all the exports at the end of the .bashrc and you won’t need to do it again when you open another terminal.

Run a first node from Webots

Now that your workspace is created and you have set the appropriate environment, you are ready to launch your first webots node.

Indeed, when you installed the ros-webots dependencies, you downloaded at the same time the webots nodes examples.

To launch it, go in your workspace, source and launch it. To do that, write the following commands:

cd catkin_ned_webots
source devel.setup.bash
roslaunch webots_ros e_puck_line.launch

Here, webots_ros is the package and e_puck_line.launch is the launcher file that calls the node e_puck_line.cpp.

Webots should now open and you should have an e_puck robot following a line like this screenshot:

Webots e_puck robot following a line

You can see the different topics of e_puck if you open a new terminal and run this command:

rostopic list

You should have this in your terminal:

Ned's topics list

Put the ned_webots_package from Niryo in your workspace

It is now time to add the niryo_robot_webots package made by Niryo in your package to be able to run the digital twin.

To do that, you will have to “git clone” the package from our Github, put it in your /src file of the workspace you created before and catkin_make again.

You can use the following commands:

cd catkin_ned_webots/src
git clone https://github.com/NiryoRobotics/ned_applications.git ned_applications
mv ned_applications/tutorials/ROS_Webots_Digital_Twin .
rm -rf ned_applications
cd ..
catkin_make
source devel/setup.bash

Connect via multi-machines to Ned and run the digital twin

Before being able to control Ned on Webots and have a digital twin on the real robot, there are still few steps to follow. To run your project, you will have to connect to Ned from your computer via multi-machines (Ned’s Raspberry Pi cannot launch Webots).

Note

You can follow our tutorial about the connection via multi-machines from your computer to Ned if you need help for this setup.

To connect via multi-machines to Ned from your computer, you can open a first terminal, connect via ssh and write the next commands:

Note

If you need help for the SSH connection, please follow our tutorial: Connect to Ned/Ned2 via SSH.

ssh niryo@IP_OF_NED (password: robotics)
cd catkin_ws
sudo service niryo_robot_ros stop (wait a bit)
export ROS_IP=IP_OF_NED
export ROS_MASTER_URI=http://IP_OF_NED::11311
roslaunch niryo_robot_bringup niryo_ned_robot.launch

Then open a second terminal and write the following commands:

cd catkin_ned_webots
export ROS_IP=IP_OF_YOUR_COMPUTER
export ROS_MASTER_URI=http://IP_OF_NED::11311

Warning

It is important to connect Ned to Niryo Studio and launch a calibration. After that, click on the button “BACK TO HOME”. By doing that you make sure that both real Ned and simulated Ned have the same position at the beggining.

Launch the digital-twin

It’s now time to launch the digital-twin. Select the second terminal (the one connected to your computer) and write the following commands to launch the launcher ned_webots.launch:

cd catkin_ned_webots
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch

By doing that, Webots will be launched with Ned’s environment and it will be connected via ROS to the real robot.

If you move the robot on Webots via your keyboard, the real robot should reproduce the exact same movement.

How to create the digital twin step by step

If you want to go further and learn more about how to implement your own digital twin between Webots and Ned, we highly recommand you to follow this section of the tutorial.

Note

On our Github, you can find all the required files to directly launch the digital twin without creating the code (just follow the previous section of this tutorial. This part will help you if you already have some knowledge with Webots, C++ and ROS, and want to learn more by doing your own application on Ned.

Create a ROS package to launch Ned on Webots and connect it to ROS

In this part, you are going to create your webots_ros package and modify it to launch Ned on Webots and connect it to ROS.

First, to create the niryo_robot_webots package, download the webots_ros package from Cyberbotics’ Github.

To do that, use these commands:

cd catkin_ned_webots/src
git clone https://github.com/cyberbotics/webots_ros.git

Modify the name of the package

Modify the name of the package as you wish. We used “niryo_robot_webots” to respect the format of the other ROS packages from Ned but you can name it as you want.

Warning

When you change the name of a ROS package, you should also change it in the launcher, package.xml and the CMakeList.txt.

Delete the files you do not need

Then delete the files that you do not need in your project. In the folder “/launch” delete every launchers files but “webots.launch”.

Delete the nodes

First, you should delete all the C++ nodes in the “/src” folder.

In the “/scripts” folder, just keep the ros_python.py and webots_launcher.py.

Warning

When you delete a node, pay attention to delete everything that is related to this node in the CMakelist.txt.

Then, you can build your package and, if you correctly modified the related files, your should not have any error:

cd catkin_ned_webots
catkin_make

You are now ready to create your own files to launch Ned on Webots and connect it to ROS!

Add the files you need to simulate Ned on Webots in your package.

To launch Webots with Ned and connect it to ROS, you also need to add a few folders/files in your package. If you have followed our previous Webots tutorial, you should be familiar with these files.

Note

You can find all the files of Ned on Webots on their website or in our Github.

Add the proto files

To do that, in your package, create a new folder called “protos” and add the proto file Ned.proto in it.

Add the world files

In your package, create a new folder called “worlds” and add the world file ned.wbt and the ned.wbproj in it.

Create the launcher to load Ned’s worlds on Webots and connect the robot on ROS

Now, you can create the launcher to load Ned’s world on Webots and connect the robot to ROS.

You should go back to your launcher folder and create a launcher called “ned_webots.launch”. Then copy these lines in it:

<launch>
<!-- start Webots -->

<arg name="no_gui" default="false," doc="Start Webots with minimal GUI"/>
<include file="$(find niryo_robot_webots)/launch/webots.launch">
    <arg name="mode" value="realtime"/>
    <arg name="no_gui" value="$(arg no_gui)"/>
    <arg name="world" value="$(find niryo_robot_webots)/worlds/ned.wbt"/>
</include>

<arg name="duration" default="20" doc="Duration in seconds"/>
<arg name="auto_close" default="false" doc="Startup mode"/>
</launch>

You can now “catkin_make” and launch the launcher:

cd catkin_ned_webots
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch

When the Webots screen appears, select Ned and change its controller by “ros” instead of “ned”.

Niryo ROS controller in webots

Run the last command again and you should have this:

Ned simulation on Webots

Ned should now be connected to ROS on Webots.

Code a ROS node to control Ned on Webots with the keyboard

Now that Ned is connected to ROS on Webots. You can code a ROS node to control it. For our digital twin, we used C++ this time as the major part of Webots’s nodes are written in C++ for ROS.

Note

To do this controller, we got inspired by an already existing node created by Webots and called keyboard_teleop.cpp.

The first step consists in creating a “/include/niryo_robot_webots” folder, creating a webots_core.hpp file, then copy/paste the following code in it:

/*
webots_node.hpp
Copyright (C) 2020 Niryo
All rights reserved.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef WEBOTSCORE_HPP
#define WEBOTSCORE_HPP

// ros
#include "ros/ros.h"
#include <std_msgs/String.h>

// webots
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/robot_get_device_list.h>


namespace webots {

class WebotsCore
{
public:
    WebotsCore();
    virtual ~WebotsCore();
    int loop();

private:
    int initController();
    void initServices();
    void initPublishers();

    // define methodes
    void controllerNameCallback(const std_msgs::String::ConstPtr &name);
    void quit(int sig);

    // Callbacks
    void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value);
    std::string castDoubleToString(double joint_to_cast);

private:
    ros::NodeHandle _nh;

    static constexpr int TIME_STEP = 32;

    // define services
    ros::ServiceClient _joint_1Client;
    ros::ServiceClient _joint_2Client;
    ros::ServiceClient _joint_3Client;
    ros::ServiceClient _joint_4Client;
    ros::ServiceClient _joint_5Client;
    ros::ServiceClient _joint_6Client;
    ros::ServiceClient _jaw_1Client;
    ros::ServiceClient _jaw_2Client;

    ros::ServiceClient _timeStepClient;
    webots_ros::set_int _timeStepSrv;

    ros::ServiceClient _enableKeyboardClient;
    webots_ros::set_int _enableKeyboardSrv;

    // Publishers
    ros::Publisher _joint_1Pub;
    ros::Publisher _joint_2Pub;
    ros::Publisher _joint_3Pub;
    ros::Publisher _joint_4Pub;
    ros::Publisher _joint_5Pub;
    ros::Publisher _joint_6Pub;
    ros::Publisher _jaw_1Pub;
    ros::Publisher _jaw_2Pub;

    // define attributes
    int _controllerCount;
    std::vector<std::string> _controllerList;
    std::string _controllerName;

    _Float64 _joint_1 {0};
    _Float64 _joint_2 {-0.5};
    _Float64 _joint_3 {1.25};
    _Float64 _joint_4 {0};
    _Float64 _joint_5 {0};
    _Float64 _joint_6 {0};
    _Float64 _jaw_1 {0};
    _Float64 _jaw_2 {0};
};

}

#endif // WEBOTSCORE_HPP

The second step consists in creating a “webots_core.cpp” node in the “/src” files and copy/paste the following code in it:

// Copyright 1996-2020 Cyberbotics Ltd.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Includes
#include "ros/ros.h"

#include <webots_ros/Int32Stamped.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/robot_get_device_list.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <signal.h>
#include <stdio.h>
#include <iostream>
#include <string.h>
#include <sstream>


#include "niryo_robot_webots/webots_core.hpp"

namespace webots {

constexpr int TIME_STEP = 32;

WebotsCore::WebotsCore()
{
    initController();
    initServices();
    initPublishers();
}

WebotsCore::~WebotsCore()
{

}

int WebotsCore::initController()
{
    // subscribe to the topic model_name to get the list of availables controllers
    ros::Subscriber nameSub = _nh.subscribe("model_name", 100, &WebotsCore::controllerNameCallback, this);
    while (_controllerCount == 0 || _controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
        ros::Duration(0.5).sleep();
    }
    ros::spinOnce();

    // if there is more than one controller available, let the user choose
    if (_controllerCount == 1)
        _controllerName = _controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= _controllerCount)
        _controllerName = _controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }

    // leave topic once it's not necessary anymore
    nameSub.shutdown();

    return 0;
}
/**
        * @brief initServices
*/
void WebotsCore::initServices()
{
    // Get the joints of Ned on Webots
    _joint_1Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_1/set_position");
    _joint_2Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_2/set_position");
    _joint_3Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_3/set_position");
    _joint_4Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_4/set_position");
    _joint_5Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_5/set_position");
    _joint_6Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_6/set_position");
    _jaw_1Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_base_to_jaw_1/set_position");
    _jaw_2Client = _nh.serviceClient<webots_ros::set_float>(_controllerName + "/joint_base_to_jaw_2/set_position");
}

/**
        * @brief initPublishers
*/
void WebotsCore::initPublishers()
{
    // Publisher Joints Ned
    _joint_1Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_1", 1000);
    _joint_2Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_2", 1000);
    _joint_3Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_3", 1000);
    _joint_4Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_4", 1000);
    _joint_5Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_5", 1000);
    _joint_6Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/joint_6", 1000);
    _jaw_1Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/jaw_1", 1000);
    _jaw_2Pub = _nh.advertise<std_msgs::String>("niryo_robot_webots/jaw_2", 1000);
}

/**
        * @brief controllerNameCallback
        * @param std_msgs::String::ConstPtr &name
        * @return controllerList
*/
void WebotsCore::controllerNameCallback(const std_msgs::String::ConstPtr &name)
{
    // catch names of the controllers availables on ROS network
    _controllerCount++;
    _controllerList.push_back(name->data);
    ROS_INFO("Controller #%d: %s.", _controllerCount, _controllerList.back().c_str());
}

/**
    * @brief quit
    * @param sig
*/
void WebotsCore::quit(int sig)
{
    // shut down webots with the ESCAPE keyboard key
    _enableKeyboardSrv.request.value = 0;
    _enableKeyboardClient.call(_enableKeyboardSrv);
    _timeStepSrv.request.value = 0;
    _timeStepClient.call(_timeStepSrv);
    ROS_INFO("User stopped the 'webots_node' node.");
    ros::shutdown();
    exit(0);
}

/**
    * @brief keyboardCallback
    * @param webots_ros::Int32Stamped::ConstPtr &value
*/
void WebotsCore::keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    // Move Ned according to the keyboard key pressed
    int key = value->data;
    int send = 0;

    switch (key)
    {
    case 'A':
        if (_joint_1 > -2.78)
        {
        _joint_1 += -0.05;
        send = 1;
        }
        break;
    case 'Z':
        if (_joint_1 < 2.78)
        {
        _joint_1 += 0.05;
        send = 1;
        }
        break;
    case 'S':
        if (_joint_2 > -0.38)
        {
        _joint_2 += -0.05;
        send = 1;
        }
        break;
    case 'Q':
        if (_joint_2 < 0.48)
        {
        _joint_2 += 0.05;
        send = 1;
        }
        break;
    case 'X':
        if (_joint_3 > -1.28)
        {
        _joint_3 += -0.05;
        send = 1;
        }
        break;
    case 'W':
        if (_joint_3 < 1.18)
        {
        _joint_3 += 0.05;
        send = 1;
        }
        break;
    case 'U':
        if (_joint_4 > -1.98)
        {
        _joint_4 += -0.05;
        send = 1;
        }
        break;
    case 'Y':
        if (_joint_4 < 1.98)
        {
        _joint_4 += 0.05;
        send = 1;
        }
        break;
    case 'H':
        if (_joint_5 > -1.48)
        {
        _joint_5 += -0.05;
        send = 1;
        }
        break;
    case 'J':
        if (_joint_5 < 0.88)
        {
        _joint_5 += 0.05;
        send = 1;
        }
        break;
    case 'N':
        if (_joint_6 > -2.48)
        {
        _joint_6 += -0.05;
        send = 1;
        }
        break;
    case 'B':
        if (_joint_6 < 2.48)
        {
        _joint_6 += 0.05;
        send = 1;
        }
        break;
    case 'L':
        if (_jaw_1 < 0.009)
        {
        _jaw_1 += 0.001;
        _jaw_2 += 0.001;
        send = 1;
        }
        send = 1;
        break;
    case 'M':
        if (_jaw_1 > -0.009)
        {
        _jaw_1 += -0.001;
        _jaw_2 += -0.001;
        send = 1;
        }
        break;
    case 27:
        quit(-1);
        break;
    default:
        send = 0;
        break;
    }

    webots_ros::set_float joint_1Srv;
    webots_ros::set_float joint_2Srv;
    webots_ros::set_float joint_3Srv;
    webots_ros::set_float joint_4Srv;
    webots_ros::set_float joint_5Srv;
    webots_ros::set_float joint_6Srv;
    webots_ros::set_float jaw_1Srv;
    webots_ros::set_float jaw_2Srv;
    joint_1Srv.request.value = _joint_1;
    joint_2Srv.request.value = _joint_2;
    joint_3Srv.request.value = _joint_3;
    joint_4Srv.request.value = _joint_4;
    joint_5Srv.request.value = _joint_5;
    joint_6Srv.request.value = _joint_6;
    jaw_1Srv.request.value = _jaw_1;
    jaw_2Srv.request.value = _jaw_2;

    // Check if one of the Joint_XSrv was a success or not
    if (send)
    {
    if (!_joint_1Client.call(joint_1Srv) ||
        !_joint_2Client.call(joint_2Srv) ||
        !_joint_3Client.call(joint_3Srv) ||
        !_joint_4Client.call(joint_4Srv) ||
        !_joint_5Client.call(joint_5Srv) ||
        !_joint_6Client.call(joint_6Srv) ||
        !_jaw_1Client.call(jaw_1Srv) ||
        !_jaw_2Client.call(jaw_2Srv) ||
        !joint_1Srv.response.success ||
        !joint_2Srv.response.success ||
        !joint_3Srv.response.success ||
        !joint_4Srv.response.success ||
        !joint_5Srv.response.success ||
        !joint_6Srv.response.success ||
        !jaw_1Srv.response.success ||
        !jaw_2Srv.response.success)
        ROS_ERROR("Failed to send new position commands to the robot.");
    }
    return;
}

/**
    * @brief castDoubleToString
    * @param double joint_to_cast
    * @return string s
*/
std::string WebotsCore::castDoubleToString(double joint_to_cast)
{
    // function to cast a double into string
    std::string s;
    std::ostringstream oss;
    oss << joint_to_cast;
    s = oss.str();
    return s;
}

int WebotsCore::loop()
{
    // enable the keyboard
    _enableKeyboardClient = _nh.serviceClient<webots_ros::set_int>(_controllerName + "/keyboard/enable");
    _enableKeyboardSrv.request.value = TIME_STEP;
    if (_enableKeyboardClient.call(_enableKeyboardSrv) && _enableKeyboardSrv.response.success) {
    ros::Subscriber sub_keyboard;
    sub_keyboard = _nh.subscribe(_controllerName + "/keyboard/key", 1, &WebotsCore::keyboardCallback, this);
    while (sub_keyboard.getNumPublishers() == 0) {
    }
    ROS_INFO("Keyboard enabled.");
    ROS_INFO("Use the keyboard keys in Webots window to move the robot.");
    ROS_INFO("Press the ESC key to stop the node.");

    // publish joints values
    while (ros::ok()) {

        std_msgs::String joint_str;
        joint_str.data = castDoubleToString(_joint_1);
        _joint_1Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_joint_2);
        _joint_2Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_joint_3);
        _joint_3Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_joint_4);
        _joint_4Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_joint_5);
        _joint_5Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_joint_6);
        _joint_6Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_jaw_1);
        _jaw_1Pub.publish(joint_str);

        joint_str.data = castDoubleToString(_jaw_2);
        _jaw_2Pub.publish(joint_str);

        _timeStepClient = _nh.serviceClient<webots_ros::set_int>(_controllerName + "/robot/time_step");
        _timeStepSrv.request.value = TIME_STEP;
        ros::spinOnce();
        if (!_timeStepClient.call(_timeStepSrv) || !_timeStepSrv.response.success)
        ROS_ERROR("Failed to call service time_step for next step.");
    }
    }
    // error with the keyboard
    else
    ROS_ERROR("Could not enable keyboard, success = %d.", _enableKeyboardSrv.response.success);

    // keyboard failed to be enabled
    _enableKeyboardSrv.request.value = 0;
    if (!_enableKeyboardClient.call(_enableKeyboardSrv) ||
        !_enableKeyboardSrv.response.success)
    ROS_ERROR("Could not disable keyboard, success = %d.", _enableKeyboardSrv.response.success);
    _timeStepSrv.request.value = 0;
    _timeStepClient.call(_timeStepSrv);
    ros::shutdown();
    return (0);
}

} // namespace webots

Now your can create another file called “webots_node.cpp” and copy/paste the following code:

/*
    webots_node.hpp
    Copyright (C) 2020 Niryo
    All rights reserved.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

// ros
#include <ros/ros.h>
#include "niryo_robot_webots/webots_core.hpp"


// main webots_core.cpp
int main(int argc, char **argv) {

// create a node named 'webots_node' on ROS network
ros::init(argc, argv, "webots_node");
ros::NodeHandle _nh;

ros::AsyncSpinner spinner(4);
spinner.start();

auto myWebots_shared_ptr = std::make_shared<webots::WebotsCore>();
myWebots_shared_ptr->loop();

ros::waitForShutdown();

ROS_INFO("Webots_node - Shutdown node");

}

Note

Don’t forget that when you add a new node in your package, you should update the CMakeList.txt file.

The last step: add the previous node in the launcher ned_webots.launch, which should now look like this:

<launch>
<!-- start Webots -->

<arg name="no_gui" default="false," doc="Start Webots with minimal GUI"/>
<include file="$(find niryo_robot_webots)/launch/webots.launch">
    <arg name="mode" value="realtime"/>
    <arg name="no_gui" value="$(arg no_gui)"/>
    <arg name="world" value="$(find niryo_robot_webots)/worlds/ned.wbt"/>
</include>

<arg name="duration" default="20" doc="Duration in seconds"/>
<arg name="auto_close" default="false" doc="Startup mode"/>
<node name="webots_node" pkg="niryo_robot_webots" type="webots_node" args="$(arg duration)" required="$(arg auto_close)" output="screen"/>
</launch>

Run the node to control Ned on Webots with the keyboard and ROS

You are now ready to run the node previously created with the following commands:

cd catkin_ned_webots
catkin_make
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch

The Webots application should appear and you should be able to control Ned with your keyboard.

Note

Don’t forget to click on the simulation screen before controlling the robot with your keyboard.

After that, you should have more topics by doing a:

rostopic list
Ned's topics

Subscribe to the joints values of Ned on Webots and publish it on the real robot

Finally, you can subscribe to the joint values of Webots’ Ned and send it to the real robot. Indeed, with the new topics we just created, we are able to get the joints states of Ned on Webots.

The goal is to publish these states on the real robot to make it reproduce the movements (digital twin).

Create a new node in the “/scripts” folder, call it send_webots_command.py and copy/paste this code:

#!/usr/bin/env python
"""
Node to send a command to Ned when we move it on Webots
"""

import rospy

from std_msgs.msg import String
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


class SendWebotsCommand:
    def __init__(self):
        rospy.init_node('send_webots_command')

        # Set rate in Hz
        rate = 15
        r = rospy.Rate(rate)

        self._webots_joint1 = None
        self._webots_joint2 = None
        self._webots_joint3 = None
        self._webots_joint4 = None
        self._webots_joint5 = None
        self._webots_joint6 = None

        # Subscribers
        rospy.wait_for_message('/niryo_robot_webots/joint_1', String)
        rospy.Subscriber('/niryo_robot_webots/joint_1', String,
                        self.__callback_webots_joint_1)

        rospy.wait_for_message('/niryo_robot_webots/joint_2', String)
        rospy.Subscriber('/niryo_robot_webots/joint_2', String,
                        self.__callback_webots_joint_2)

        rospy.wait_for_message('/niryo_robot_webots/joint_3', String)
        rospy.Subscriber('/niryo_robot_webots/joint_3', String,
                        self.__callback_webots_joint_3)

        rospy.wait_for_message('/niryo_robot_webots/joint_4', String)
        rospy.Subscriber('/niryo_robot_webots/joint_4', String,
                        self.__callback_webots_joint_4)

        rospy.wait_for_message('/niryo_robot_webots/joint_5', String)
        rospy.Subscriber('/niryo_robot_webots/joint_5', String,
                        self.__callback_webots_joint_5)

        rospy.wait_for_message('/niryo_robot_webots/joint_6', String)
        rospy.Subscriber('/niryo_robot_webots/joint_6', String,
                        self.__callback_webots_joint_6)

        self._joint_states = None
        rospy.Subscriber('/joint_states', JointState,
                        self.__callback_joint_states)

        # Publishers
        self._joint_trajectory_publisher = rospy.Publisher('/niryo_robot_follow_joint_trajectory_controller/command',
                                                        JointTrajectory, queue_size=1)

        while not rospy.is_shutdown():

            self.joint_target_tuple = [self._webots_joint1, -self._webots_joint2, -self._webots_joint3, self._webots_joint4, self._webots_joint5, self._webots_joint6]
            self.joint_target = list(self.joint_target_tuple)
            # send command to Ned
            self.publish_joint_trajectory(self.joint_target)
            r.sleep()

    # Callbacks
    def __callback_joint_states(self, _joint_states_msg):
        self._joint_states = _joint_states_msg.position[:6]

    def __callback_webots_joint_1(self, _joint_1):
        self._webots_joint1 = float(_joint_1.data)

    def __callback_webots_joint_2(self, _joint_2):
        self._webots_joint2 = float(_joint_2.data)

    def __callback_webots_joint_3(self, _joint_3):
        self._webots_joint3 = float(_joint_3.data)

    def __callback_webots_joint_4(self, _joint_4):
        self._webots_joint4 = float(_joint_4.data)

    def __callback_webots_joint_5(self, _joint_5):
        self._webots_joint5 = float(_joint_5.data)

    def __callback_webots_joint_6(self, _joint_6):
        self._webots_joint6 = float(_joint_6.data)

    def publish_joint_trajectory(self, joint_target):

        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.Time.now()
        joint_trajectory.header.frame_id = "arm"
        joint_trajectory.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']

        joint_trajectory_points = JointTrajectoryPoint()
        joint_trajectory_points.positions = self.joint_target
        joint_trajectory_points.time_from_start = rospy.Duration(0.1)

        joint_trajectory.points = [joint_trajectory_points]
        self._joint_trajectory_publisher.publish(joint_trajectory)
        rospy.loginfo("joints command : " + str(self.joint_target))


if __name__ == '__main__':
    try:
        SendWebotsCommand()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

Add this node in your launcher

Your launcher ned_webots.launch should look like that:

 <launch>

     <!-- start Webots -->
     <arg name="no_gui" default="false," doc="Start Webots with minimal GUI"/>
     <include file="$(find niryo_robot_webots)/launch/webots.launch">
     <arg name="mode" value="realtime"/>
     <arg name="no_gui" value="$(arg no_gui)"/>
     <arg name="world" value="$(find niryo_robot_webots)/worlds/ned.wbt"/>
     </include>

     <arg name="duration" default="20" doc="Duration in seconds"/>
     <arg name="auto_close" default="false" doc="Startup mode"/>
     <node name="webots_node" pkg="niryo_robot_webots" type="webots_node" args="$(arg duration)" required="$(arg auto_close)" output="screen"/>

     <node name="send_webots_command" pkg="niryo_robot_webots" type="send_webots_command.py" output="screen"></node>

</launch>

Note

If you want to assure that everything is right, do not hesitate to compare with our original niryo_robot_webots package on our Github.

You just managed to make a digital-twin between a simulated Ned on Webots and a real Ned!

To launch the digital twin, you can follow the first section of this tutorial.