Réaliser un jumeau numérique entre Ned et Webots avec ROS
Note
- Ce tutoriel est fonctionnel à partir de :
- La version v3.0.0 de la ned_ros_stackLa version v3.0.0 de Niryo Studio
Introduction
En Juillet 2021, Ned a été ajouté sur Webots. Vous pouvez avoir plus d’informations sur le site de Cyberbotics.
En téléchargeant Webots et en exécutant le sample Webots de Ned, vous pourrez contrôler ses articulations et son gripper avec votre clavier, et aussi exécuter une démo. Il est fortement recommandé de suivre le tutoriel sur la simulation de Ned dans Webots avant de suivre ce tutoriel.
Qu’est ce que Webots, de Cyberbotics ?
« Webots est un simulateur robotique open-source et multiplateformes. Il a été conçu pour un usage professionnel, et est largement utilisé dans l’industrie, l’éducation et la recherche. Il fournit un environnement de développement complet pour modéliser, programmer et simuler des robots. Cyberbotics Ltd. maintient Webots comme son produit principal sans interruption depuis 1998. » (source).
Vous pouvez télécharger Webots gratuitement sur leur site.
Note
Pour réaliser ce tutoriel nous avons utilisé « Ubuntu 18.04 » et « Webots R2021a ». Vous en aurez besoin pour ce tutoriel.
Vous pouvez trouver un démonstrateur de Ned sur le site de Cyberbotics.
Objectifs de ce tutoriel
Dans la première partie du tutoriel, vous apprendrez à utiliser le jumeau numérique que nous avons créé pour contrôler un vrai Ned avec un Ned simulé sur Webots. En contrôlant Ned sur Webots, vous serez en mesure d’envoyer les mêmes commandes sur le vrai robot, en temps réel.
Dans la deuxième partie du tutoriel, vous apprendrez à partir de zéro comment mettre en œuvre un contrôleur ROS pour Ned sur Webots en utilisant le paquet webots_ros et comment développer un jumeau numérique entre Webots et Ned.
À la fin de ce tutoriel, vous serez en mesure de modifier le paquet ROS et de réaliser vos propres projets avec Ned, Webots et ROS. Vous pouvez trouver le paquet ROS sur notre Github.
Lancement du jumeau numérique
Installation de ROS et du paquet « webots_ros »
La première partie de ce tutoriel consiste à installer ROS et le paquet webots_ros. Pour ce faire, ouvrez un nouveau terminal et utilisez les commandes suivantes :
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 fonctionne sous Ubuntu 18.04 et ROS melodic. En suivant ces commandes, vous allez installer ROS melodic (si ce n’est pas déjà fait), mettre à jour votre rosdep et installer les dépendances ROS de webots sur votre ordinateur.
Note
Si vous voulez plus d’informations sur la façon d’installer ROS et le paquet webots_ros, vous pouvez consulter la documentation de Webots à ce propos.
Configuration de l’environnement de travail
Maintenant que vous avez installé les dépendances de ros-webots sur votre ordinateur, il est nécessaire de configurer le bon environnement de travail.
Nous allons commencer par sourcer correctement l’environnement ROS. Pour ce faire, ouvrez un nouveau terminal et écrivez :
cd
source /opt/ros/melodic/setup.bash
Création de votre espace de travail ROS
Maintenant, nous devons créer un nouvel espace de travail catkin. C’est dans ce nouvel espace de travail que nous allons créer un nouveau paquet ROS pour communiquer entre Webots et Ned via ROS.
Pour créer votre espace de travail catkin, il vous suffit d’ajouter les lignes suivantes :
mkdir -p ~/catkin_ned_webots/src
cd ~/catkin_ned_webots/
catkin_make
source devel/setup.bash
Votre espace de travail est maintenant prêt !
Vérification de la configuration de l’environnement ROS_PACKAGE_PATH
Avant de créer un nouveau paquet ROS de webots, vous devez vous assurer que l’environnement ROS_PACKAGE_PATH est correctement configuré.
Pour ce faire, dans le même terminal que celui où vous avez créé l’espace de travail, vous pouvez ajouter cette commande :
printenv | grep ROS
Avec cette commande, vous pouvez voir tous les environnements commençant par « ROS ». Dans notre cas, nous devons définir l’environnement ROS_PACKAGE_PATH, nous pouvons donc utiliser cette commande :
echo $ROS_PACKAGE_PATH
Cela devrait retourner : « /home/youruser/catkin_ws/src:/opt/ros/melodic/share ».
Sinon, vous pouvez utiliser :
export ROS_PACKAGE_PATH=/home/youruser/catkin_ws/src:/opt/ros/melodic/share
Configuration l’environnement WEBOTS_HOME
Si vous avez installé Webots dans votre dossier racine (root), votre chemin WEBOTS_HOME devrait être « /usr/local/webots » et vous n’avez alors rien de plus à faire à ce propos. Si votre chemin est différent, utilisez cette commande dans le terminal :
export WEBOTS_HOME=/usr/local/webots
Modification de votre .bashrc
Si vous ne souhaitez pas reproduire les précédentes manipulations pour vos prochaines utilisations de ce jumeau numérique (digital twin), vous pouvez modifier votre fichier .bashrc via la commande suivante :
nano ~/.bashrc
Vous pouvez alors placer l’ensemble des exports à la fin du fichier. Vous n’aurez ainsi plus à effectuer ces étapes de configuration par la suite.
Lancement d’un premier node depuis Webots
Maintenant que votre espace de travail est créé et que vous avez défini l’environnement approprié, vous êtes prêt à lancer votre premier nœud webots.
En effet, lorsque vous avez installé les dépendances de ros-webots, vous avez téléchargé en même temps les exemples de nœuds webots.
Pour le lancer, allez sur votre espace de travail, sourcez, puis lancez-le. Pour ce faire, écrivez les commandes suivantes :
cd catkin_ned_webots
source devel.setup.bash
roslaunch webots_ros e_puck_line.launch
Ici, webots_ros est le paquet et e_puck_line.launch est le launcher qui appelle le nœud e_puck_line.cpp.
Après avoir fait cela, Webots devrait s’ouvrir et vous devriez avoir un robot e_puck suivant une ligne comme sur cette capture d’écran :
Vous pouvez voir les différents topics du robot e_puck si vous ouvrez un nouveau terminal et lancez cette commande :
rostopic list
Vous devriez avoir ceci dans votre terminal :
Ajout de ned_webots_package dans l’espace de travail
Il est maintenant temps d’ajouter notre paquet niryo_robot_webots dans votre paquet pour permettre au jumeau numérique de fonctionner.
Pour ce faire, vous devez cloner le paquet depuis notre Github, le mettre dans votre fichier « /src » de l’espace de travail que vous avez créé auparavant et catkin_make à nouveau.
Pour ce faire, écrivez les commandes suivantes dans un terminal :
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
Connexion à Ned en multi-machines et lancement du jumeau numérique
Avant de pouvoir contrôler Ned dans Webots et de créer une relation de jumeau numérique avec le robot réel, il reste encore quelques étapes à suivre. Pour lancer le projet, vous allez devoir connecter Ned à votre ordinateur en multi-machines (la Raspberry Pi du Ned ne peut pas lancer Webots).
Note
Vous pouvez suivre notre tutoriel permettant d’effectuer la connexion en multi-machines depuis votre ordinateur vers Ned si vous avez besoin d’aide pour cette étape.
Pour vous connecter via multi-machines à Ned depuis votre ordinateur, vous devez ouvrir un premier terminal, vous connecter via ssh et écrire les commandes suivantes :
Note
Si vous avez besoin d’aide pour la connexion SSH, vous pouvez suivre notre tutoriel : Se connecter à Ned/Ned2 en 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
Ensuite, ouvrez un terminal et écrivez les lignes suivantes :
cd catkin_ned_webots
export ROS_IP=IP_OF_YOUR_COMPUTER
export ROS_MASTER_URI=http://IP_OF_NED::11311
Avertissement
Il est important de connecter Ned à Niryo Studio et de lancer une calibration. Après cela, cliquez sur le bouton « BACK_TO_HOME ». En faisant cela, vous vous assurez que le Ned simulé et le Ned réel ont tous deux la même position de départ.
Lancement du jumeau numérique
Il est maintenant temps de lancer le jumeau numérique (digital twin). Pour ce faire, sélectionnez le deuxième terminal (celui qui est connecté à votre ordinateur) et utilisez les commandes suivantes pour lancer le lanceur ned_webots.launch :
cd catkin_ned_webots
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch
En faisant cela, Webots sera lancé avec l’environnement de Ned et il sera connecté via ROS au robot physique.
Si vous déplacez le robot sur Webots à l’aide de votre clavier, le robot réel devrait reproduire exactement le même mouvement.
Création d’un jumeau numérique, étape par étape
Si vous souhaitez aller plus loin et en savoir plus sur la manière dont vous pouvez créer un jumeau numérique entre Webots et Ned, nous vous recommandons vivement de suivre cette partie du tutoriel.
Note
Sur notre Github, vous pouvez déjà trouver tous les fichiers et codes dont vous aurez besoin si vous voulez directement lancer le jumeau numérique sans créer les fichiers vous-même avec l’aide de la première partie de ce tutoriel si besoin. La section ci-dessous est adressée aux personnes ayant déjà quelques bases avec Webots ainsi qu’en C++ et ROS, souhaitant en apprendre davantage avec une nouvelle application sur Ned.
Création d’un paquet ROS pour lancer Ned sur Webots et le connecter à ROS
Dans cette partie, vous allez créer votre paquet webots_ros et le modifier afin de pouvoir lancer Ned sur Webots et le connecter à ROS.
Tout d’abord, pour créer le paquet niryo_robot_webots, téléchargez le paquet webots_ros depuis le Github de Cyberbotics.
Pour ce faire, utilisez les commandes suivantes :
cd catkin_ned_webots/src
git clone https://github.com/cyberbotics/webots_ros.git
Modification du nom du package
Modifiez le nom du paquet comme vous le souhaitez. Nous avons utilisé « niryo_robot_webots » pour respecter le format des autres paquets ROS de Ned mais vous pouvez l’appeler comme vous le souhaitez.
Avertissement
Lorsque vous changez le nom d’un paquet ROS, vous devez également changer celui-ci dans le launcher mais aussi dans les fichiers package.xml et CMakeList.txt.
Suppression des fichiers dont vous n’avez pas besoin
Supprimez les fichiers dont vous n’avez pas besoin pour votre projet. Dans le dossier « /launch », supprimez tous les launchers à l’exception de webots.launch.
Suppession des nœuds inutiles
Supprimez tous les nœuds C++ du dossier « /src ».
Dans le dossier « /scripts », gardez uniquement les fichiers ros_python.py et webots_launcher.py.
Avertissement
Lorsque vous supprimez un nœud, faites attention à supprimer tout ce qui est lié à celui-ci dans le fichier CMakelist.txt.
Maintenant, vous pouvez créer votre package avec la commande suivante. Obtenir une erreur à cette étape signifie qu’il reste peut être des références aux fichiers supprimés au sein du projet.
cd catkin_ned_webots
catkin_make
Vous êtes maintenant prêt à créer vos propres fichiers pour lancer Ned sur Webots et le connecter à ROS !
Ajoutez les fichiers dont vous avez besoin pour simuler Ned sur Webots dans votre paquet.
Pour lancer Webots avec Ned et le connecter à ROS, vous allez encore avoir quelques dossiers/fichiers à ajouter dans votre paquet. Si vous avez suivi notre précédent tutoriel concernant la simulation avec Webots, ces fichiers vous seront familiers.
Note
Vous pouvez trouver tous les fichiers de Ned sur Webots sur leur site web ou sur notre Github.
Ajout du fichier PROTO
Dans votre paquet, créez un nouveau dossier appelé « /protos » et insérez-y le fichier proto Ned.proto.
Ajout des fichiers « world »
Dans votre paquet, créez un nouveau dossier appelé « /worlds » et ajoutez à ce dossier les fichiers world ned.wbt et le fichier ned.wbproj.
Création d’un launcher et connexion du robot à ROS
Il est maintenant temps de créer le launcher pour charger le monde dans Webots, et connecter le robot à ROS.
Allez dans votre dossier launcher et créez un launcher appelé « ned_webots.launch ». Puis copiez ces lignes dessous :
<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>
Vous pouvez maintenant « catkin_make » et lancer le launcher :
cd catkin_ned_webots
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch
Lorsque l’application Webots apparaît, sélectionnez Ned et changez son contrôleur par « ros » au lieu de « ned ».
Lancez la dernière commande de nouveau. Vous devriez obtenir ceci :
Maintenant, Ned est connecté à ROS sur Webots.
Création d’un nœud ROS pour contrôler Ned sur Webots avec le clavier
Maintenant que Ned est connecté à ROS sur Webots, nous allons créer un nœud ROS pour le contrôler. Nous allons utiliser le langage C++ cette fois-ci car la majeure partie des nœuds de Webots sont écrits dans ce langage.
Note
Pour réaliser ce contrôleur, nous allons nous inspirer du nœud déjà réalisé par Webots appelé keyboard_teleop.cpp.
La première étape consiste en la création d’un dossier « /include/niryo_robot_webots » puis d’un fichier webots_core.hpp en faisant un copier-coller du code suivant :
/*
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
La deuxième étape concerne la création du nœud « webots_core.cpp » dans les fichiers « /src » en faisant un copier/coller du code suivant :
// 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
Troisième tâche, vous devez créer un autre nœud appelé « webots_node.cpp » et copier/coller le code suivant:
/*
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
N’oubliez pas que, lorsque vous ajoutez un nouveau nœud à votre paquet, vous devez mettre à jour le fichier CMakeList.txt.
Enfin, vous pouvez ajouter le nœud dans le launcher ned_webots.launch, qui devrait maintenant ressembler à ceci :
<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>
Lancement du nœud et contrôle de Ned au clavier
Vous êtes maintenant prêt à exécuter votre nœud, pour cela écrivez les commandes suivantes :
cd catkin_ned_webots
catkin_make
source devel/setup.bash
roslaunch niryo_robot_webots ned_webots.launch
Webots devrait s’ouvrir et vous devriez être en mesure de contrôler Ned avec votre clavier.
Note
N’oubliez pas de cliquer sur l’écran de simulation avant de contrôler le robot avec votre clavier.
Maintenant, vous devriez avoir les topics suivants :
rostopic list
Ecoute des valeurs articulaires de Ned sur Webots pour les publier sur le robot réel
La dernière étape consiste à écouter les valeurs des articulations de Ned sur Webots et à les envoyer au robot physique. En effet, avec les nouveaux topics créés, nous avons à présent la possibilité d’écouter les valeurs articulaires de Ned sur Webots.
Le but est alors de publier ces valeurs sur le robot réel afin qu’il reproduise les mouvements avec une relation de jumeau numérique (digital twin).
Pour réaliser cela, il suffit de créer un nouveau nœud dans le dossier /scripts, de l’appeler send_webots_command.py et de copier/coller ce 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
Ajout du nœud au launcher
Votre launcher ned_webots.launch devrait maintenant ressembler à ceci :
<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
Si vous voulez vous assurer que tout est correctement configuré, n’hésitez pas à faire une comparaison avec notre paquet « niryo_robot_webots » sur notre Github.
Vous venez de créer un jumeau numérique entre un Ned simulé sur Webots et un Ned réel !
Pour lancer le jumeau numérique, vous pouvez suivre la première partie de ce tutoriel.