webots模型联合ros仿真，实现ros自定义控制器

参考链接

1、官网，介绍了using ros控制器，分为官方标准控制器（ standard ROS controller）和自定义控制器（custom ROS controller）的使用，对于自定义控制器的官方解释如下：

2、官网2介绍了ros端的webots_ros包的安装，和自带ros节点的例子，webots_ros包：https://github.com/cyberbotics/webots_ros

3、博主zhangrelay对官方文档的翻译，进行了详细的介绍。
4、博主图契图卡在ros中实现了自定义c++节点
youtube教学视频，主要采用了官方模型与官方自带的控制器。

已有条件

ubuntu16.04，ros kinetic，webots2020a，webots机器人模型及环境。
首先我们要明确控制架构，webots主要是由两部分组成：控制器(controller)+仿真环境(world+model)，通过ros节点发送指令，机器人上面的ros控制器接受指令，控制机器人运动，因此两方面的工作需要做：ros节点和ros控制器。
下图是webots一个项目中的文件夹。

1. 仿真环境
   仿真环境中其自带的模型非常多，但有时候我们需要用自己的机器人，这时候可以通过将urdf文件转换为proto文件，从而加载进world，具体参考我另一篇博客。
2. 控制器
   webots功能主要集中在控制器，因为webots主要是用来实现规划算法，支持许多语言。
   

第一步，ros控制器

这里我已经搭载好一个简易的二自由度模型，配置好了电机、碰撞属性和物理属性：

然后可以配置控制器了，官方已经配置了ros控制器接口，在控制器选择中，左下角点击选择，然后选择ros，这表明我们已经在机器人上配置好了ros控制器接口。

配置好：

第二步，ros节点

1 官方的模型官方的节点

将模型配置ros控制器，可以利用官方自带的ros节点控制机器人了。只需要启动roscore，启动仿真，确保连接上ROS master，然后rosrun 包名 节点名即可，下图是运行的pioneer3at示意图：

2 自己的模型自己的节点

建立ros节点应当要建立工作空间

mkdir -p example_ws/src
cd example_ws/src
catkin_init_workspace

为了获取webots支持ros的依赖等配置文件，将webots安装路径（默认应该是在/usr/local/webots/projects/languages/ros，可以通过cd ${WEBOTS_HOME}/projects/languages/ros找到）的webots_ros文件夹拷贝至之前建立的工作空间下的src文件夹下，然后同理将相关通信格式文件拷贝到webots_ros下面。
也可以通过下列指令：

cp -r ${WEBOTS_HOME}/projects/languages/ros/webots_ros .
cp -r ${WEBOTS_HOME}/projects/default/controllers/ros/include/srv webots_ros/
cp -r ${WEBOTS_HOME}/projects/default/controllers/ros/include/msg webots_ros/

catkin_make一下，
但我需要编写自己的ros节点，控制这个两自由度机器人。因此创建自己的node（c++）文档，并在CMakelist中添加依赖和链接：

add_executable(example src/example.cpp)
add_dependencies(example webots_ros_generate_messages_cpp)
target_link_libraries(example
	${catkin_LIBRARIES}
)

source一下

source ~/example_ws/devel/setup.bash

rosrun即可

仿真结果：

视频：

webots与ros联合仿真-自定义模型与ros节点通行

报错情况：

若出现错误：Failed to call service set_velocity on motor 1。

call不成功
参考博客需要配置：

模型+自定义控制器+ros节点

参考官网对于自定义控制器的解释，需要建立一个包，处理来自 ROS 的“catkin_make”(CMakerlist)和来自 Webots 的“Makefile”，以便将生成的二进制文件链接到 Webots 的“libController”和“roscpp”库。此处示例

需要明确的是，控制器为一个可执行文件，即c++文件，通过编译、链接等一系列操作生成的，因此，整体思路便是，编写一个c++文件，整合ros与webots，通过共同的编译规则，生成一个可执行文件（控制器）即可。
对于c++编译规则不太懂，只感觉makefile比较复杂，因此，采用cmakelist的编译规则。

1. 第一步，建立ros工作空间，包，和节点，建立ros与webots的联系。这里我建立了一个listener节点用作控制器，引入webots相关头文件，这里只用了motor，用到什么引入什么。

   `#include <webots/Robot.hpp>
   #include <webots/Motor.hpp>`
   

2. 第二步，编辑ros功能包对应的cmakelist文件，添加依赖和链接库，特别注意链接库这块，ros必须依赖${catkin_LIBRARIES}，没有引用会报关于ros的未定义的引用的错，而webots依赖${LIBRARIES}，因为在ros节点中引用了webots，所以需要链接${LIBRARIES},不然会报错：关于webots的未定义的引用。

   add_dependencies(ros_talker_controller ros_controller2_generate_messages_cpp)
   target_link_libraries(ros_talker_controller
     ${catkin_LIBRARIES}
   )
   add_executable(ros_listener_controller
     src/ros_listener_controller.cpp
   )
   add_dependencies(ros_listener_controller ros_controller2_generate_messages_cpp)
   target_link_libraries(ros_listener_controller
     ${catkin_LIBRARIES}
     ${LIBRARIES}
   )
   
   

   另外，cmakelist是编译规则文件，因此，要将webots相关的编译、链接，结合到ros节点里面去，还需要在文档中添加链接路径等：

   if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
   endif()
   
   link_directories($ENV{WEBOTS_HOME}/lib/controller)
   
   set (LIBRARIES ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} 
                  ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX} 
   )
   
   set(EXECUTABLE_OUTPUT_PATH ../)
                  
   include_directories($ENV{WEBOTS_HOME}/include/controller/c $ENV{WEBOTS_HOME}/include/controller/cpp ./include /usr/local/include/eigen3)
   
   include_directories(
     include 
     "/usr/include/eigen3" 
     $ENV{WEBOTS_HOME}/include/controller/cpp
     $ENV{WEBOTS_HOME}/include/controller/c
   )
   

3. 第三步、编译、找到devel下的listenerl可执行文件，将其拷贝至控制器目录下，用作控制器。我是在回调函数中将接收数据，设置恒定速度控制。回调函数主题：

     while (robot->step(timeStep) != -1) 
     {
       m1->setPosition(INFINITY);
       m2->setPosition(INFINITY);
   
       m1->setVelocity(msg->x);
       m2->setVelocity(msg->y);
   
     }; 
      delete robot;
   }
   

   另外，main函数主体:

    ros::init(argc,argv,"listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("gps_info", 1,gpsCallback);
    ros::spin();
   

   搭配好控制器之后，应该显示控制器加载完成：
   

4. 第四步，roscore，source，rosrun启动自定义ros控制节点，我这里是建立的话题发布者talker，发布一系列值。控制器接收到指令，开始以恒定速度控制电机转动。到这里我们就实现了自定义模型+自定义控制器+自定义ros节点控制，有了这些基础（ros通信与自定义控制器），接下来就可以设计、封装控制器，尝试更多的控制算法了！！！
   
   视频：

   ros+webot联合仿真，自定义控制器与ros节点