ROS2学习日志：CV开发之关联Gazebo/Webots/ROS2

1.关于模拟器

         (1)安装位置：分别安装在/usr/bin/gazebo和usr/local/bin/webots，其中gazebo由/usr/bin/gzclient(负责前端界面显示与交互)和/usr/bin/gzserver(负责后端数据管理与通信)组成。

         (2)启动说明：启动命令分别是gazebo [options] [worldfile]和webots [options] [worldfile]，详细参数选项可通过gazebo -h或webots -h查看。当世界文件为空时，gazebo启动空模拟器，webots启动空模拟器或上次启动的世界文件。

2.关于控制器

         (1)如何设置：在模型文件或世界文件中指定控制器，gazebo在xml中添加<plugin>...</plugin>来指定和配置控制器，webots在wbt文件中由controller和controllerArgs来指定和配置控制器。

         (2)如何编码：参见官方样例。需要说明的是，gazebo的控制器被编译成的是动态库文件，webots的控制器被编译成的是可执行文件。

3.ROS控制器

         (1)关联ROS：将仿真器与外界的交互通过ROS的订阅发布机制来实现，从而摆脱复杂的API编程，扮演此中间角色的可形象的称之为ROS控制器或ROS驱动。

         (2)启动脚本：分别是gazebo.launch.py和robot_launch.py，其中gazebo.launch.py由gzclient.launch.py和gzserver.launch.py组成。

4.直接启动与脚本启动的关系

         (1)核心关系：脚本启动也是调用直接启动，只是将直接启动的参数形式重向为脚本的参数形式，使得启动操作更方便，然而本质上直接启动也能完成脚本启动的所有功能，只是稍显麻烦而已。

         (2)gazebo.launch.py对直接启动的重点扩展：启动时默认init:=true和factory:=true，这将加载位于/opt/ros/foxy/lib/中libgazebo_ros_init.so和libgazebo_ros_factory.so(这两个文件的作用是...)，相当于设置了参数-s /opt/ros/foxy/lib/libgazebo_ros_init.so -s /opt/ros/foxy/lib/libgazebo_ros_factory.so的直接启动(注意这两个文件也可以放到模型文件中加载而无需手动加载)。

         (3)robot_launch.py对直接启动的重点扩展：启动时默认executable=webots_node，这将加载位于/opt/ros/foxy/lib/webots_ros2_core中webots_node(这是所有WebotsROS控制器的基类且它加载了所有内置传感器的驱动)，相当于wbt文件中设置controller为/opt/ros/foxy/lib/webots_ros2_core/webots_node后的直接启动(注意webots要使用外部控制器必须将wbt文件中的controller设置为<extern>)。

         (4)gazebo.launch.py对比robot_launch.py：Gazebo需要加载单独加载libgazebo_ros_init.so和libgazebo_ros_factory.so及每个传感器的驱动，Webots只能加载一个驱动且只是webots_node或webots_node子类，但webots_node已完成了所有内置传感器驱动的加载。

         另外，脚本启动可能还包含附加启动其它程序，如机器人状态发布器，然而这些操作都是可以手动操作的。

         总结而言，脚本启动一次完成了直接启动和相关附加程序的启动，但这些启动都可以手动一步一步操作。

5.直接启动与脚本启动的样例

         Gazebo脚本启动不能指定控制器(只能在模型文件中指定)，但直接启动可以，脚本启动可设置是否启动ROS基本功能。

         (1)无ROS控制器的Gazebo模型：/usr/share/gazebo-11/world包含很多模型，其中everything.world攘括了所有模型。

　　　　加载环境：source /opt/ros/foxy/setup.bash  && source /usr/share/gazebo/setup.sh

　　　　直接启动且模型无ROS控制器-->能使无任何ROS功能：gazebo --verbose /usr/share/gazebo-11/worlds/everything.world

　　　　直接启动且模型无ROS控制器-->能使有基础ROS功能：gazebo --verbose -s /opt/ros/foxy/lib/libgazebo_ros_init.so -s /opt/ros/foxy/lib/libgazebo_ros_factory.so /usr/share/gazebo-11/worlds/everything.world

　　　　脚本启动且模型无ROS控制器-->能使有基础ROS功能：ros2 launch gazebo_ros gazebo.launch.py verbose:=true world:=/usr/share/gazebo-11/worlds/everything.world

　　　　脚本启动且模型无ROS控制器-->能使无基础ROS功能：ros2 launch gazebo_ros gazebo.launch.py verbose:=true world:=/usr/share/gazebo-11/worlds/everything.world init:=false factory:=false

         (2)有ROS控制器的Gazebo模型：/opt/ros/foxy/share/gazebo_plugins/worlds包含很多模型，下以gazebo_ros_video_demo.world模型作为测试。

　　　　加载环境：source /opt/ros/foxy/setup.bash && source /usr/share/gazebo/setup.sh

　　　　直接启动且模型有ROS控制器-->能使无基础但有DIYROS功能：gazebo --verbose /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_video_demo.world

　　　　直接启动且模型有ROS控制器-->能使有基础且有DIYROS功能：gazebo --verbose -s /opt/ros/foxy/lib/libgazebo_ros_init.so -s /opt/ros/foxy/lib/libgazebo_ros_factory.so /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_video_demo.world

　　　　脚本启动且模型有ROS控制器-->能使有基础且有DIYROS功能：ros2 launch gazebo_ros gazebo.launch.py verbose:=true /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_video_demo.world

　　　　脚本启动且模型有ROS控制器-->能使无基础且有DIYROS功能：ros2 launch gazebo_ros gazebo.launch.py verbose:=true /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_video_demo.world init:=false factory:=false

         Webots直接启动不能指定DIY控制器(只能在模型文件中指定)，但脚本启动可以，脚本启动必须将controller设置<extern>。

         (1)无ROS控制器的Webots模型：/usr/local/webots/projects包含很多模型且大多附带控制器，以下测试需要提前将测试模型的controller设为<extern>。

　　　　加载环境：source /opt/ros/foxy/setup.bash

　　　　直接启动且模型无ROS控制器-->能使无任何ROS功能：webots --mode=realtime /usr/local/webots/projects/robots/dji/mavic/worlds/mavic_2_pro.wbt

　　　　直接启动且模型无ROS控制器-->能使有基础ROS功能：正在尝试

　　　　脚本启动且模型无ROS控制器-->能使有基础ROS功能：ros2 launch webots_ros2_core robot_launch.py world:=/usr/local/webots/projects/robots/dji/mavic/worlds/mavic_2_pro.wbt

　　　　脚本启动且模型无ROS控制器-->能使无基础ROS功能：无法做到，因为要么使用默认的webots_ros2_core->webots_node要么指定一个ROS控制器，否则无法启动。

         (2)有ROS控制器的Webots模型：/opt/ros/foxy/share包含几个典型的模型且都带ROS控制器，由于使用外部控制器(ROS控制器是外部控制器)，所以这些模型的controller都已设为<extern>。

　　　　加载环境：source /opt/ros/foxy/setup.bash

　　　　直接启动且模型有ROS控制器-->能使无基础且无DIYROS功能：webots --mode=realtime /opt/ros/foxy/share/webots_ros2_epuck/worlds/epuck_world.wbt

　　　　直接启动且模型有ROS控制器-->能使有基础且无DIYROS功能：正在尝试将controller设置为webots_node

　　　　直接启动且模型有ROS控制器-->能使有基础且有DIYROS功能：正在尝试将controller设置为xxxxxx_robot_controller

　　　　脚本启动且模型有ROS控制器-->能使有基础且有DIYROS功能：ros2 launch webots_ros2_core robot_launch.py world:=/opt/ros/foxy/share/webots_ros2_epuck/worlds/epuck_world.wbt package:=webots_ros2_epuck executable:=driver

　  速控：ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

　　　　脚本启动且模型有ROS控制器-->能使有基础且无DIYROS功能：ros2 launch webots_ros2_core robot_launch.py world:=/opt/ros/foxy/share/webots_ros2_epuck/worlds/epuck_world.wbt

　　　　直接启动且模型有ROS控制器-->能使无基础且无DIYROS功能：无法做到，因为要么使用默认的webots_ros2_core->webots_node要么指定一个ROS控制器，否则无法启动。

6.Gazebo间接脚本与Webots直接脚本样例

         Webots脚本样例在WebotsROS2官网上有很多可供参考，其中最通用的是webots_ros2_core包中的robot_launch.py，其它样例都是其缩减版的扩展或直接调用它后的扩展，以下是robot_launch.py的精简版(调用之前先source /opt/ros/foxy/setup.bash)：

         Gazebo脚本样例可参见turtlebot3_bringup包和nav2_bringup包中的启动脚本，其中最通用的是gazebo_ros包中的gzserver.launch.py和gzclient.launch.py及这两者的合并版gazebo.launch.py，通常是直接调用gazebo.launch.py后再扩展。

         需要说明是Python启动脚本的参数是可传递的且可覆盖的，即gazebo.launch.py参数包含gzserver.launch.py和gzclient.launch.py的参数，若内层脚本参数与外层脚本参数重名，则内层参数被覆盖，以下是调用gazebo.launch.py的启动脚本样例(调用之前先source /opt/ros/foxy/setup.bash && source /usr/share/gazebo/setup.sh)：

import os, sys, shutil, argparse
from ament_index_python.packages import (get_package_prefix, get_packages_with_prefixes, get_package_share_directory)
import launch
from launch import (Action, LaunchDescription)#InheritFrom LaunchDescriptionEntity
from launch.actions import (TimerAction, ExecuteProcess, DeclareLaunchArgument, IncludeLaunchDescription)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch.events import TimerEvent
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode
def create_argument(args, varname, default, description = 'no description'):
    cfg = LaunchConfiguration(varname, default = default)
    args.append(DeclareLaunchArgument(varname, default_value = default, description=description))
    return cfg
def handler_for_shutdown_launch_when_action_exit(action):
    return launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(target_action=action,
            on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())]))

def generate_launch_description():
    """
    This script for taking out some usual arguments from gzclient.launch.py and gzserver.launch.py but hiden by gazebo.launch.py

    """
    user_arguments = []

    #1.RobotSimulator
    world = create_argument(user_arguments, 'world',  get_package_share_directory('gazebo_plugins') + '/worlds/gazebo_ros_video_demo.world')
    gui = create_argument(user_arguments, 'gui', 'true')
    init = create_argument(user_arguments, 'init', 'true')
    factory = create_argument(user_arguments, 'factory', 'true')
    verbose = create_argument(user_arguments, 'verbose', 'true')
    robot_simulator = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory('gazebo_ros'), '/launch/gazebo.launch.py']),
        launch_arguments =
        {
            'world': world,
            'gui': gui,
            'init': init,
            'factory': factory,
            'verbose': verbose,
            'client_required': 'true',
            'server_required': 'true',
        }.items())

    #2.SimulatorClock
    use_sim_time = create_argument(user_arguments, 'use_sim_time', 'true')
    clock_simulator = ExecuteProcess(cmd=['ros2', 'param', 'set', '/gazebo', 'use_sim_time', use_sim_time], output='screen')

    #3.EntitySpawner
    entity_name = create_argument(user_arguments, 'entity_name', 'ambulance', 'diretory name coming from /root/.gazebo/models and downloaded from https://github.com/osrf/gazebo_models or http://models.gazebosim.org/')
    entity_x = create_argument(user_arguments, 'entity_x', '-10')
    entity_y = create_argument(user_arguments, 'entity_y', '10')
    entity_z = create_argument(user_arguments, 'entity_z', '0')
    entity_spawner = Node(package='gazebo_ros',
                        executable='spawn_entity.py',
                        arguments=['-entity', entity_name, '-database', entity_name, '-x', entity_x, '-y', entity_y, '-z', entity_z],
                        output='screen')

    #4.LaunchAllEntities
    return LaunchDescription(user_arguments + [
        robot_simulator,
        clock_simulator,
        entity_spawner])