Moveit与Gazebo联合仿真

上一篇博客已经将moveit!配置完毕，然而想要让moveit!控制gazebo中的机械臂，还需要进行一些接口的配置。现在我们有的功能包为sunday_description、sunday_moveit_config这两个功能包。且已经配置好xacro文件，本篇内容需要进行gazebo功能包的配置以及moveit功能包的文件修改。

gazebo_2">sunday_gazebo

创建sunday_gazebo功能包

catkin_create_pkg sunday_gazebo roscpp rospy std_msgs

创建launch、config、world、scripts等文件夹，其列表如下所示

.
├── CMakeLists.txt
├── config
├── include
├── launch
├── package.xml
├── scripts
├── src
└── world

配置关节轨迹控制器

创建sunday_gazebo/config/sunday_trajectory_control.yaml文件，用于配置关节轨迹控制器，代码如下：

sunday:
  arm_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6


    gains:
      joint_1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint_6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

对应创建sunday_gazebo/launch/sunday_trajectory_controller.launch文件，用于加载上述yaml文件代码如下：

<launch>

    <rosparam file="$(find sunday_gazebo)/config/sunday_trajectory_control.yaml" command="load"/>

    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/sunday" args="arm_joint_controller"/>

</launch>

配置关节状态控制器

创建sunday_gazebo/config/sunday_gazebo_joint_states.yaml文件，用于配置关节状态控制器，代码如下：

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

对应创建sunday_gazebo/launch/sunday_gazebo_states.launch文件，用于加载上述yaml文件代码如下：

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find sunday_gazebo)/config/sunday_gazebo_joint_states.yaml" command="load"/>

    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/sunday" args="joint_state_controller" />

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
        <remap from="/joint_states" to="/sunday/joint_states" />
    </node>

</launch>

gazebo_90">配置gazebo环境

创建sunday_gazebo/launch/sunday_gazebo_world.launch文件，用于加载gazebo环境，代码如下：

<launch>

  <!-- 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)"/>
    <arg name="world_name" value="$(find sunday_gazebo)/world/feeding_place.world"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find sunday_description)/urdf/sunday.xacro'" /> 


  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	args="-urdf -model sunday -param robot_description"/>
</launch>

配置moveit功能包文件

创建/修改sunday_moveit_config/config/controllers_gazebo.yaml文件，用于配置moveit!控制器接口，代码如下：

controller_manager_ns: controller_manager
controller_list:
  - name: sunday/arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6

修改sunday_moveit_config/launch/sunday_moveit_controller_manager.launch.xml，代码如下：

<launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find sunday_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>

创建/修改sunday_moveit_config/launch/moveit_planning_execution.launch，用于加载planning_group等moveit核心功能，代码如下：

<launch>
 <!-- # The planning and execution components of MoveIt! configured to 
 # publish the current configuration of the robot (simulated or real)
 # and the current state of the world as seen by the planner -->
 <include file="$(find sunday_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>

 <!-- # The visualization component of MoveIt! -->
 <include file="$(find sunday_moveit_config)/launch/moveit_rviz.launch">
  <arg name="config" value="true" />
 </include>

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/sunday/joint_states]</rosparam>
  </node>

</launch>

修改sunday_moveit_config/launch/moveit_rviz.launch，使用新版配置工具生成的moveit_rviz.launch文件有些许问题，代码如下：

<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" default="" />
  <arg     if="$(arg config)" name="command_args" default="-d $(find sunday_moveit_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 sunday_moveit_config)/config/kinematics.yaml"/>
  </node>

</launch>

配置总bringup.launch文件

配置sunday_gazebo/launch/sunday_moveit_bringup.launch，用于加载所有launch文件，代码如下：

<launch>
  
    <!-- Launch Gazebo  -->
    <include file="$(find sunday_gazebo)/launch/sunday_gazebo_world.launch" />

    <!-- ros_control arm launch file -->
    <include file="$(find sunday_gazebo)/launch/sunday_gazebo_states.launch" />   

    <!-- ros_control trajectory control dof arm launch file -->
    <include file="$(find sunday_gazebo)/launch/sunday_trajectory_controller.launch" />

    <!-- moveit launch file -->
    <include file="$(find sunday_moveit_config)/launch/moveit_planning_execution.launch">
    </include>
</launch>

至此gazebo与moveit功能包配置完毕，将功能包进行编译。sunday_gazebo列表如下

.
├── CMakeLists.txt
├── config
│   ├── sunday_gazebo_joint_states.yaml
│   └── sunday_trajectory_control.yaml
├── include
│   └── sunday_gazebo
├── launch
│   ├── sunday_bringup_moveit.launch
│   ├── sunday_gazebo_states.launch
│   ├── sunday_gazebo_world.launch
│   └── sunday_trajectory_controller.launch
├── package.xml
├── scripts
├── src
└── world
    └── feeding_place.world

world为配置好的抓取仿真场景。

联合仿真测试

运行代码roslaunch sunday_gazebo sunday_bringup_moveit.launch，可以看到同时加载moveit和gazebo场景，在Query栏，将Goal State设置为scan_food，在Commands栏点击Plan & Execute，可以看到gazebo中的机械臂运行到scan_food姿态。
在这里插入图片描述