Create Ned’s digital twin with Webots and ROS
Note
- This tutorial is working from:
- The version v3.0.0 of the ned_ros_stackThe 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:
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:
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”.
Run the last command again and you should have this:
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
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.