moveit与gazebo联合仿真

准备工作：

1.moveit_config已经配置好，各种插件以及软件包装全。

2.创建myrobot_gazebo功能包，需要的依赖为：gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control

3.应用下列步骤时将ROBOT改为自己机器人的名字（写文时我是将我的机器人robot改为ROBOT，可能会有疏忽没改过来的）ps：不建议把自己的机器人命名为和robot有关字样，太容易混淆。

1.myrobot_gazebo/launch/ROBOT_world.launch用于将urdf文件在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)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" textfile="$(find ROBOT_package)/urdf/ROBOT.urdf" /> 


  <!-- 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 ROBOT -param robot_description"/> 


</launch>

2.关节状态控制器为可选插件，主要用于发布机器人关节状态和tf状态，否则在rviz的fixed frame的下拉列表中看不到坐标系选项，只能手动输入。

2.1 launch文件：myrobot_gazebo/launch/ROBOT_gazebo_states.launch

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

    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/ROBOT" 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="/ROBOT/joint_states" />
    </node>

</launch>

2.2launch中的yaml文件：myrobot_gazebo/config/ROBOT_gazebo_joint_states.yaml

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

3.关节轨迹控制器

3.1launch文件：myrobot_gazebo/launch/ROBOT_trajectory_controller.launch

<launch>

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

    <node name="robot_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/ROBOT" args="ROBOT_joint_controller ROBOT_gripper_controller "/>

</launch>

3.2launch中的yaml文件：

myrobot_gazebo/config/trajectory_control.yaml

ROBOT:
  ROBOT_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - j1
      - j2
      - j3
      - j4
      - j5
      - j6

    gains:
      j1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      j2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      j3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      j4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      j5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      j6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
#需要末端执行器的在后面添加
  ROBOT_gripper_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - finger_j1
      - finger_j2

    gains:
      finger_j1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      finger_j2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
     

4.moveit控制器的启动文件：myrobot_moveit_config)/launch/moveit_planning_execution.launch

<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 myrobot_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>
 # The visualization component of MoveIt!
 <include file="$(find myrobot_moveit_config)/launch/moveit_rviz.launch"/>

  <!-- 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">[/ROBOT/joint_states]</rosparam>
  </node>

</launch>

4.1配置一：添加文件myrobot_moveit_config/config/controllers_gazebo.yaml

controller_manager_ns: controller_manager
controller_list:
  - name: ROBOT/ROBOT_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - j1
      - j2
      - j3
      - j4
      - j5
      - j6
  - name: ROBOT/ROBOT_gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_j1
      - finger_j2

4.2配置二：更改myrobot_moveit_config/launch/robot_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 myrobot_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>

将读取到参数服务器中的控制文件改为controllers_gazebo.yaml

5.将上述四个launch集成：myrobot_gazebo/launch/robot_bringup_moveit.launch

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

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

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

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

</launch>

踩坑：

1.Unable to connect to move_group action server 'move_group' within allotted time (30s)

 解决：在rviz里面点击reset