学习完gazebo如何控制机器人后转为学习webots。
主要参考了CSDN：全网最全的Webots资料在这里
https://blog.csdn.net/weixin_43455581/article/details/108711611
https://blog.csdn.net/crp997576280/category_9855084.html
官网的很多例题，并给出了简介，需要什么功能照着简介找对应的就可以了
具体内建控制器怎么写，建模怎么弄，可以参考webots软件自带的例程webtos/projects/samples

名词介绍，这里的世界文件为.wbt，而不是.world。且devices包含以前的执行器和传感器等。

新建工程目录，不要直接直接在原来的webots目录下，这样由于webots保护，无法创建一些文件。
wizard->new project directory。然后再进行下面的步骤

自带的机器人都被封装了，需要右键转换成基本节点才能见到内部的构成

如何创建一个机器人

• 类似gazebo内带的构建机器人方法一样，可以用可视化的模型树建模。当然比较简单了。
• 使用solidworks导出三维模型 模型.wrl .stl等，可以在webot中file->import。然后进行webots中的零部件装配。gazebo让人诟病的就是零部件装配全靠手动写xacro等建模文件，否则很不精确。
• 使用urdf2webots功能包，将urdf转为proto(VRML语言)格式，proto类似gazebo使用的sdf(xml语言)格式一样是仿真软件自己指定的仿真描述文件。详情github搜urdf2webots就可以下载，然后进入包输入pip install -r requiredments.txt 安装，输入 phthon demo.py --input=xxx.urdf。得到当前终端目录下有一个叫xxx.pr中to的文件。然后在webots中按加号插入模型，在PROTO nodes(Current Project)中导入当前工程路径下的protos文件夹下。
  image.png
  重新打开该窗口就可以发现有一个模型
  image.png
  只做到这这步，下次再打开模型就消失了。需要转换其为基本节点：点击模型->covert to base node(s)

新建世界
file->new world。会出现一片漆黑，然后左边场景树。一个是worldinfo一个是viewpoint。点击场景树中的选项，其下方有个参数编辑区，截图修改场景树中模型或属性的数值。其中worldinfo就类似gazebo中的world，定义了一堆世界信息比如重力加速度、仿真步长等。

image.png

• 地面：PROTO nodes(Webots Projects)->objects -> floors
• 光源：点光源、平行光源、锥形光源。分别存在 Base nodes -> pointLight/DIrctionLight/SpotLight

创建环境物体
先插入solid节点，然后点击场景树中solid，选中其children，添加shape。appearance是gazebo中的纹理textures，而geometry描述实体形状。点击geometry添加一个sphere球(也可以添加mesh网格)。此时geometry 后出现Sphere，然后点开可以修改半径和细分度。然后为appearance添加纹理，双击textures，添加imageTextures。在纹理图片中的url属性中选择纹理图片路径

image.png

。
现在只是有可视形状(对应gazebo中vision)。还没有物理属性，所以无法对仿真做出动作响应。点击physics，添加physics节点，然后可以设置 质量、密度(mass density二选一，其中一个必须为-1)。这样他就有了物理属性，但是有物理属性后，他还没有碰撞实体，现在只有shape这个现实外形。因此对boundingObject(类似collision)进行设置。可以直接复制vision也就是children中的shape属性给他使用

image.png

控制器代码， wizard-> new robot_controller 创建对应语言的控制器文件。具体查看博客内容作为控制代码模板。然后在机器人的controller中选择对应控制器。 其中控制器参数是用来传入程序的入口参数？在webot_ros中用来指定机器人的unique name。

与gazebo和ros通信有gazebo_ros_control一样。webot有webots_ros，其实也是机器人控制器或插件内实现了服务器、数据、话题等并发布到ros网络上。可通过apt安装。但是建议通过clone https://github.com/cyberbotics/webots_ros 然后重点查看src中的complete_test.cpp和robot_infomation_parser.cpp 前者是api例子，后者是程序模板。而webot.launch 教我们如何通过launch启动webot，其实是运行一个叫webot的python脚本(需要设置成可执行文件权限)。而phthon脚本通过终端命令来启动webots。然后可以向这个默认的webot.launch文件指定.wbt(世界文件)，然后包含这个默认webots.launch文件来编写用户自己的文件来启动webot。

image.png

image.png

image.png

启动后输入rosservice list 可以发现好多服务，可以用来获取各种信息，包括传感器，关节等。这点和gazebo_ros_control 一致。

剩下的怎么写就可以看博客的 ros通信下。包括cmakelist 源码等。代码例程也可以看webots_ros包下的实例代码`robot_infomation_parser.cpp。
比如

CMakeLists中的find_package 加入webots_ros

webots_ros发布的topic/service的名字格式为：robot_unique_name(webots模型中name字段内容+ 进程ID号 + device_name)/ service或topic_name。
而修改controllerArgs字段为 --name=myrobot_name，则可以设置robot_unique_name为一个固定的值，否则程序每次启动进程ID不同影响程序编写通用性。

以下模板不是webots 控制器的代码模板，而是webots+ros中 ros的使用代码。
模板一

#include <signal.h>
#include <locale.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf/transform_broadcaster.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>

#define TIME_STEP 32
#define NMOTORS 4
#define MAX_SPEED 4

ros::NodeHandle *n;

static int controllerCount;
static std::vector<std::string> controllerList;

ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;

static const char *motorNames[NMOTORS] = {"wheel1", "wheel2", "wheel3", "wheel4"};

// 更新速度
void updateSpeed() {
  double speeds[NMOTORS];

    speeds[0] = MAX_SPEED;
    speeds[1] = MAX_SPEED;
    speeds[2] = MAX_SPEED;
    speeds[3] = MAX_SPEED;

  for (int i = 0; i < NMOTORS; ++i) {
    ros::ServiceClient set_velocity_client;
    webots_ros::set_float set_velocity_srv;
    set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
                                                                  std::string("/set_velocity"));
    set_velocity_srv.request.value = speeds[i];
    set_velocity_client.call(set_velocity_srv);
  }
}

// 获取可用控制器的名称
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

//退出函数
void quit(int sig) {
  ROS_INFO("终止节点运行.");
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ros::shutdown();
  exit(0);
}

int main(int argc, char **argv) {
  setlocale(LC_CTYPE,"zh_CN.utf8");

  ros::init(argc, argv, "my_robot", ros::init_options::AnonymousName);
  n = new ros::NodeHandle;

  signal(SIGINT, quit);

  std::string controllerName;


/**********************************************   当作模板即可   ****************************************************/
  // 订阅主题model_name以获得可用控制器列表
  ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();

// 设置基本仿真步长
  timeStepClient = n->serviceClient<webots_ros::set_int>("my_robot/robot/time_step");
  timeStepSrv.request.value = TIME_STEP;

  // 多控制器时可选
  if (controllerCount == 1){
  controllerName = controllerList[0];
  }
  else {
    int wantedController = 0;
    std::cout << "选择要使用的控制器的编号 :\n";
    std::cin >> wantedController;
    if (1 <= wantedController && wantedController <= controllerCount)
      controllerName = controllerList[wantedController - 1];
    else {
      ROS_ERROR("无效的控制器编号.");
      return 1;
    }
  }
  ROS_INFO("Using controller: '%s'", controllerName.c_str());
  nameSub.shutdown();


/**********************************************  电机模式设置   ****************************************************/
  for (int i = 0; i < NMOTORS; ++i) {
    ros::ServiceClient set_position_client;
    webots_ros::set_float set_position_srv;
    set_position_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
                                                                  std::string("/set_position"));
    //设置速度模式
    set_position_srv.request.value = INFINITY;
    if (set_position_client.call(set_position_srv) && set_position_srv.response.success)
      ROS_INFO("motor %s 工作模式为速度模式.", motorNames[i]);
    else
      ROS_ERROR("无法调用motor %s的set_position服务 .", motorNames[i]);

    // 创建速度client
    ros::ServiceClient set_velocity_client;
    set_velocity_client = n->serviceClient<webots_ros::set_float>(std::string("my_robot/") + std::string(motorNames[i]) +
                                                                  std::string("/set_velocity"));
    // 创建服务
    webots_ros::set_float set_velocity_srv;
    set_velocity_srv.request.value = 0.0;

    if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
      ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);
    else
      ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]);
  }

  /**********************************************   传感器使能   ****************************************************/
  // 使能 camera
  ros::ServiceClient set_camera_client;
  webots_ros::set_int camera_srv;
  ros::Subscriber sub_camera;
  set_camera_client = n->serviceClient<webots_ros::set_int>("my_robot/kinect_color/enable");
  camera_srv.request.value = 64;
  set_camera_client.call(camera_srv);

/******************************************** 主循环 ****************************************************** */
  while (ros::ok()) {
    updateSpeed(); // 更新小车速度

    if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
      ROS_ERROR("Failed to call service time_step for next step.");
      break;
    }
    ros::spinOnce();
  }
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);

  ros::shutdown();
  return 0;
}

模板二

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

#include <signal.h>
#include "ros/ros.h"

// include files to use services like 'robot_get_time'.
// srv files needed to use webots service can be found in the /srv folder where you found this example.
// for more info on how to create and use services with ROS refer to their website: http://wiki.ros.org/
// here 'webots_ros' is the name of the package used for this node. Replace it by your own package.
#include <webots_ros/robot_get_device_list.h>

#include <webots_ros/get_int.h>
#include <webots_ros/get_string.h>

// include files to use standard message types in topic
// Webots only use basic messages type defined in ROS library
#include <std_msgs/String.h>

#define TIME_STEP 32

static int controllerCount;
static std::vector<std::string> controllerList;

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

void quit(int sig) {
  ROS_INFO("User stopped the 'robot_information_parser' node.");
  ros::shutdown();
  exit(0);
}

int main(int argc, char **argv) {
  std::string controllerName;
  std::vector<std::string> deviceList;
  // create a node named 'robot_information_parser' on ROS network
  ros::init(argc, argv, "robot_information_parser", ros::init_options::AnonymousName);
  ros::NodeHandle n;

  signal(SIGINT, quit);

  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();

  // if there is more than one controller available, it 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 is not necessary anymore
  nameSub.shutdown();

  // call get_type and get_model services to get more general information about the robot
  ros::ServiceClient getTypeClient = n.serviceClient<webots_ros::get_int>(controllerName + "/robot/get_type");
  webots_ros::get_int getTypeSrv;
  ros::ServiceClient getModelClient = n.serviceClient<webots_ros::get_string>(controllerName + "/robot/get_model");
  webots_ros::get_string getModelSrv;

  getTypeClient.call(getTypeSrv);
  if (getTypeSrv.response.value == 40)
    ROS_INFO("This controller is on a basic robot.");
  else if (getTypeSrv.response.value == 41)
    ROS_INFO("This controller is on a supervisor robot.");
  else
    ROS_INFO("This controller is on a differential wheels robot.");

  if (getModelClient.call(getModelSrv)) {
    if (!getModelSrv.response.value.empty())
      ROS_INFO("The model of this robot is %s.", getModelSrv.response.value.c_str());
    else
      ROS_ERROR("The robot doesn't seems to have a model.");
  } else
    ROS_ERROR("Could not get the model of this robot.");

  // call deviceList service to get the list of the name of the devices available on the controller and print it
  // the deviceListSrv object contains 2 members: request and response. Their fields are described in the corresponding .srv
  // file
  ros::ServiceClient deviceListClient =
    n.serviceClient<webots_ros::robot_get_device_list>(controllerName + "/robot/get_device_list");
  webots_ros::robot_get_device_list deviceListSrv;

  if (deviceListClient.call(deviceListSrv)) {
    deviceList = deviceListSrv.response.list;
    ROS_INFO("The controller has %lu devices availables:", deviceList.size());
    for (unsigned int i = 0; i < deviceList.size(); i++)
      ROS_INFO("Device [%d]: %s.", i, deviceList[i].c_str());
  } else
    ROS_ERROR("Failed to call service deviceList.");
}

webots 控制器代码

#include <webots/robot.h>

// Added a new include file
#include <webots/motor.h>

#define TIME_STEP 64

int main(int argc, char **argv) {
 wb_robot_init();

 // get the motor devices
 WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");
 WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");
 // set the target position of the motors
 wb_motor_set_position(left_motor, 10.0);
 wb_motor_set_position(right_motor, 10.0);

 while (wb_robot_step(TIME_STEP) != -1);

 wb_robot_cleanup();

 return 0;
}

webots 速度控制

#include <webots/robot.h>

// Added a new include file
#include <webots/motor.h>

#define TIME_STEP 64

#define MAX_SPEED 6.28

int main(int argc, char **argv) {
  wb_robot_init();

  // get a handler to the motors and set target position to infinity (speed control)
  WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");
  WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");
  wb_motor_set_position(left_motor, INFINITY);
  wb_motor_set_position(right_motor, INFINITY);

  // set up the motor speeds at 10% of the MAX_SPEED.
  wb_motor_set_velocity(left_motor, 0.1 * MAX_SPEED);
  wb_motor_set_velocity(right_motor, 0.1 * MAX_SPEED);

  while (wb_robot_step(TIME_STEP) != -1) {
  }

  wb_robot_cleanup();

  return 0;
}

webots控制器 详细教程

https://www.cyberbotics.com/doc/guide/tutorial-4-more-about-controllers?tab-language=c++

如何搭建一个机器人教程
https://www.cyberbotics.com/doc/guide/tutorial-6-4-wheels-robot?tab-language=c

image.png

Cmakelist

cmake_minimum_required(VERSION 3.0.2)
project(webots_controller)

find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  rospy
  sensor_msgs
  std_msgs
  tf
  webots_ros
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES webots_controller
  CATKIN_DEPENDS message_runtime message_generation roscpp rospy sensor_msgs std_msgs tf webots_ros
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(first_webots_ros_simulation src/first_webots_ros_simulation.cpp)
target_link_libraries(first_webots_ros_simulation
    ${catkin_LIBRARIES}

package.xml

<?xml version="1.0"?>
<package>
  <name>webots_controller</name>
  <version>0.0.0</version>
  <description>The webots_controller package</description>
  <maintainer email="robert.h.x.s@foxmail.com">robert</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>webots_ros</build_depend>

  <run_depend>webots_ros</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>sensor_msgs</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>tf</run_depend>
  <run_depend>message_generation</run_depend>

</package>