#################################################################
Create Ned's digital twin with Webots and ROS 
#################################################################

.. image:: /images/digital-twin-webots/digital_twin_webots.gif
                     :alt: Create Ned's digital twin with Webots and ROS
                     :width: 100%
                     :align: center

.. 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 <https://cyberbotics.com/>`_.
- 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 :doc:`our previous tutorial about Webots simulation </source/tutorials/simulation_ned_webots>` 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 <https://cyberbotics.com/>`_.

.. 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 <https://www.cyberbotics.com/doc/guide/ned?version=omichel:master>`_.

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 <https://github.com/cyberbotics/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 <https://github.com/NiryoRobotics/ned_applications/tree/master/tutorials/ROS_Webots_Digital_Twin>`_.


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 <https://github.com/cyberbotics/webots_ros>`_ package on your computer. To do that you will have to open a new terminal 
and write the following commands: 

.. code-block:: bash

    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 <https://github.com/cyberbotics/webots_ros>`_ package, you can take a look at `Webots' documentation <https://cyberbotics.com/doc/guide/tutorial-8-using-ros?tab-ros=melodic>`_.



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:

.. code-block:: bash

    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: 

.. code-block:: bash

    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: 

.. code-block:: bash

    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: 

.. code-block:: bash

    echo $ROS_PACKAGE_PATH  

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

If not, use: 

.. code-block:: bash
    
    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: 

.. code-block:: bash
    
    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:

.. code-block:: bash
    
    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: 

.. code-block:: bash
    
    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: 

          .. image:: /images/digital-twin-webots/e_puck_webots.png
                      :alt: Webots e_puck robot following a line
                      :width: 1000px
                      :align: center

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

.. code-block:: bash
    
    rostopic list

You should have this in your terminal:

            .. image:: /images/digital-twin-webots/e_puck_topics.png
                        :alt: Ned's topics list
                        :width: 500px
                        :align: center


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:

.. code-block:: bash

    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 :doc:`the connection via multi-machines from your computer to Ned <./setup_ned_ros_multi_machines>` 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: :doc:`./setup_connect_ned_ssh`.

.. code-block:: bash

    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:

.. code-block:: bash

    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:

.. code-block:: bash

    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 :ref:`previous section of this tutorial <source/tutorials/digital_twin_ned_webots_using_ros:How to use the digital twin>`.
    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 <https://github.com/cyberbotics/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 <https://github.com/cyberbotics/webots_ros>`_ package from Cyberbotics' Github. 

To do that, use these commands: 

.. code-block:: bash
    
    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:

.. code-block:: bash

    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 :doc:`previous Webots tutorial <./simulation_ned_webots>`, 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 <https://github.com/NiryoRobotics/ned_applications/tree/master/tutorials/Webots_simulation>`_.

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:

.. code-block:: xml

    <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:

.. code-block:: bash

    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".

        .. image:: /images/digital-twin-webots/ros_controller.png
                :alt: Niryo ROS controller in webots
                :width: 1000px
                :align: center

Run the last command again and you should have this:

        .. image:: /images/digital-twin-webots/ned_ros.png
                :alt: Ned simulation on Webots
                :width: 1000px
                :align: center

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 <https://github.com/cyberbotics/webots_ros/blob/master/src/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: 

.. code-block:: cpp

    /*
    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:

.. code-block:: cpp

    // 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:

.. code-block:: cpp

    /*
        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:

.. code-block:: xml

    <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:

.. code-block:: bash

    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:

.. code-block:: bash

    rostopic list

.. image:: /images/digital-twin-webots/topics_ned.png
    :alt: Ned's topics
    :width: 350px
    :align: center


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:

.. code-block:: python

    #!/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:

.. code-block:: xml

    <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 <https://github.com/NiryoRobotics/ned_applications/tree/master/tutorials/ROS_Webots_Digital_Twin>`_.

**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 :ref:`first section <source/tutorials/digital_twin_ned_webots_using_ros:How to use the digital twin>` of this tutorial.