MoveIt和Gazebo的roslaunch文件以及通信

研究roslaunch文件中启动了什么以及相互通信使用的话题和配置

arg/param/rosparam的介绍在这里

• 主题
  
  • robot model
    
    • robot description
    • joint state publisher
    • use_gui
    • robot state publisher
  • gazebo
    
    • gazebo empty world
    • spawn model
    • controller
  • moveit
  • rviz

robot model

robot description

分urdf和xacro两种

<param name="robot_description" textfile="$(find mastering_ros_robot_description_pkg)/urdf/pan_tilt.urdf" />

xacro需要调用xacro.py转换一下

<param name="robot_description" command="$(find xacro)/xacro.py $(find mastering_ros_robot_description_pkg)/urdf/seven_dof_arm.xacro" />

xacro除了建立机器人几何模型，还可以添加一系列传动接口、控制接口甚至可以添加虚拟摄像机读取gazebo中的机器人图像点云等等，urdf中允许配置传感器话题，一旦urdf被加载到gazebo，就会开始发布话题。

joint state publisher

默认发送关节值到/joint_states这个话题，数据来源是Joint State Interfaces，这个接口会从编码器中读取机器人的关节值，由joint_state_controller来控制发布的速率。remap可以改变话题名

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <remap from="/joint_states" to="/seven_dof_arm/joint_states"/>
</node>

如果没有真实机器人或者gazebo，可以在node中添加

 <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>

让moveit监听fake_controller发回的关节角

use_gui

设置为true，就可以跳出一个GUI框，关节角运动范围是从URDF中读取到的，拖动进度条可以改变机器人的关节值，让机器人运动。

<param name="use_gui" value="true"/>

如果在roslaunch文件中，则对全局有效，上面这句代码也可以放置在节点内，比如

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <param name="use_gui" value="true"/>
</node>

那么该参数仅在节点内有效，参数服务器中显示的是/node_name/param_name，本例中是/joint_state_publisher/use_gui

robot state publisher

读取/joint_states的数据然后发布到/tf和/tf_static

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

gazebo

gazebo empty world

调用gazebo_ros生成一个空的世界模型，如果跑的时候出错了，可能是安装的时候模型不全，去下一个完整的模型包放到主目录的.gazebo目录下models文件夹里就好了

<!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

spawn model

把模型加载到gazebo，arg中-urdf是说要解析的模型是urdf类型的，-model后面跟的是加载的模型的名字，-param是文件来源，来自参数服务器的robot_description参数

<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model seven_dof_arm -param robot_description"/> 

controller

要使gazebo机器人能够运动，要在urdf中添加相应的虚拟硬件接口，hardwareInterface除了position还有velocity、effort等。

<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>

为了控制机器人，需要添加控制接口

<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/seven_dof_arm</robotNamespace>
</plugin>
</gazebo>

控制器的参数一般设在yaml文件中，第一句是命名空间，joint state controller控制关节值发布速率，joint position controller是位置控制，同时设置了PID。

seven_dof_arm:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: position_controllers/JointPositionController
    joint: shoulder_pan_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: position_controllers/JointPositionController
    joint: shoulder_pitch_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint3_position_controller:
    type: position_controllers/JointPositionController
    joint: elbow_roll_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint4_position_controller:
    type: position_controllers/JointPositionController
    joint: elbow_pitch_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint5_position_controller:
    type: position_controllers/JointPositionController
    joint: wrist_roll_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint6_position_controller:
    type: position_controllers/JointPositionController
    joint: wrist_pitch_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint7_position_controller:
    type: position_controllers/JointPositionController
    joint: gripper_roll_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}

yaml文件需要先加载到参数服务器才能使用

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find seven_dof_arm_gazebo)/config/seven_dof_arm_gazebo_control.yaml" command="load"/>

生成控制器才能控制gazebo中机器人

 <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/seven_dof_arm" args="joint_state_controller
                      joint1_position_controller
                      joint2_position_controller
                      joint3_position_controller
                      joint4_position_controller
                      joint5_position_controller
                      joint6_position_controller
                      joint7_position_controller"/>

加载控制器以后，rostopic中会出现这个话题，接收命令

/seven_dof_arm/joint1_position_controller/command

通过向该话题发布命令，gazebo中的机器人就可以运动了

rostopic pub /seven_dof_arm/joint4_position_controller/command std_msgs/Float64 1.0

moveit

moveit通过FollowJointTrajectoryAction与机器人控制器通信，move_node会初始化一个client，server是跑在机器人上或者Gazebo上的。

setup assistant可以创建moveit配置包

roslaunch moveit_setup_assistant setup_assistant.launch

planning scene monitor负责机器人自身位姿（从/joint_states话题读取）和各种传感器输入

首先要启动planning context

 <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find seven_dof_arm_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

下面是planning_context.launch，加载机器人和环境

<launch>
  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
  <arg name="load_robot_description" default="false"/>

  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description" default="robot_description"/>

  <!-- Load universal robot description format (URDF) -->
  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro.py '$(find mastering_ros_robot_description_pkg)/urdf/seven_dof_arm.xacro'"/>

  <!-- The semantic description that corresponds to the URDF -->
  <param name="$(arg robot_description)_semantic" textfile="$(find seven_dof_arm_config)/config/seven_dof_arm.srdf" />

  <!-- Load updated joint limits (override information from URDF) -->
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find seven_dof_arm_config)/config/joint_limits.yaml"/>
  </group>

  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find seven_dof_arm_config)/config/kinematics.yaml"/>
  </group>

</launch>

然后启动move group

 <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find seven_dof_arm_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

move_group.launch文件，默认使用OMPL库进行规划

<launch>

  <include file="$(find seven_dof_arm_config)/launch/planning_context.launch" />

  <!-- GDB Debug Option -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix"
       value="gdb -x $(find seven_dof_arm_config)/launch/gdb_settings.gdb --ex run --args" />

  <!-- Verbose Mode Option -->
  <arg name="info" default="$(arg debug)" />
  <arg unless="$(arg info)" name="command_args" value="" />
  <arg     if="$(arg info)" name="command_args" value="--debug" />

  <!-- move_group settings -->
  <arg name="allow_trajectory_execution" default="true"/>
  <arg name="fake_execution" default="false"/>
  <arg name="max_safe_path_cost" default="1"/>
  <arg name="jiggle_fraction" default="0.05" />
  <arg name="publish_monitored_planning_scene" default="true"/>

  <!-- Planning Functionality -->
  <include ns="move_group" file="$(find seven_dof_arm_config)/launch/planning_pipeline.launch.xml">
    <arg name="pipeline" value="ompl" />
  </include>

  <!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(find seven_dof_arm_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="seven_dof_arm" unless="$(arg fake_execution)"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

  <!-- Sensors Functionality -->
  <include ns="move_group" file="$(find seven_dof_arm_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_sensor_manager" value="seven_dof_arm" />
  </include>

  <!-- Start the actual move_group node/action server -->
  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />

    <!-- load these non-default MoveGroup capabilities -->
    <!--
    <param name="capabilities" value="
                  a_package/AwsomeMotionPlanningCapability
                  another_package/GraspPlanningPipeline
                  " />
    -->

    <!-- inhibit these default MoveGroup capabilities -->
    <!--
    <param name="disable_capabilities" value="
                  move_group/MoveGroupKinematicsService
                  move_group/ClearOctomapService
                  " />
    -->

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
  </node>

</launch>

其中，下面这边是生成控制器

<!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(find seven_dof_arm_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="seven_dof_arm" unless="$(arg fake_execution)"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

trajectory_execution.launch.xml如下

<launch>

  <!-- This file makes it easy to include the settings for trajectory execution  -->  

  <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
  <arg name="moveit_manage_controllers" default="true"/>
  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>

  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->

  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
  <arg name="moveit_controller_manager" default="seven_dof_arm" />
  <include file="$(find seven_dof_arm_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

</launch>

根据fake execution会选择不同的配置文件
fake moveit controller manage如下

<launch>

  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>

  <!-- The rest of the params are specific to this plugin -->
  <rosparam file="$(find seven_dof_arm_config)/config/fake_controllers.yaml"/>

</launch>

其中fake_controllers.yaml如下，不需要配置控制类型等

controller_list:
  - name: fake_arm_controller
    joints:
      - shoulder_pan_joint
      - shoulder_pitch_joint
      - elbow_roll_joint
      - elbow_pitch_joint
      - wrist_roll_joint
      - wrist_pitch_joint
      - gripper_roll_joint
  - name: fake_gripper_controller
    joints:
      - finger_joint1

如果不是fake execution，加载robot moveit controller manager

<launch>
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

    <!-- load controller_list -->
    <arg name="use_controller_manager" default="true" />
    <param name="use_controller_manager" value="$(arg use_controller_manager)" />

    <!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find seven_dof_arm_config)/config/controllers.yaml"/>
</launch>

controllers.yaml如下

controller_manager_ns: controller_manager
controller_list:
  - name: seven_dof_arm/seven_dof_arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_pan_joint
      - shoulder_pitch_joint
      - elbow_roll_joint
      - elbow_pitch_joint
      - wrist_roll_joint
      - wrist_pitch_joint
      - gripper_roll_joint
  - name: seven_dof_arm/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1
      - finger_joint2

最后启动rviz

 <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find seven_dof_arm_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

moveit_rviz.launch的介绍在下面rviz中。

rviz

启动rviz，-d参数是display config file load，加载一些显示参数，required参数不知道是干啥的，回头再研究。

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mastering_ros_robot_description_pkg)/urdf.rviz" required="true" />

-f可以指定fixed frame

rosrun rviz rviz -f /rgbd_camera_optical_frame

moveit_rviz.launch文件
anon是取一个匿名的节点名，config = true则选择自定义的moveit.rviz作为参数设置

<launch>

  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="config" default="false" />
  <arg unless="$(arg config)" name="command_args" value="" />
  <arg     if="$(arg config)" name="command_args" value="-d $(find seven_dof_arm_config)/launch/moveit.rviz" />

  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
    args="$(arg command_args)" output="screen">
    <rosparam command="load" file="$(find seven_dof_arm_config)/config/kinematics.yaml"/>
  </node>

</launch>