Moveit与Gazebo联合仿真

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

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_6gains: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/JointStateControllerpublish_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环境

创建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_controlleraction_ns: follow_joint_trajectorytype: FollowJointTrajectorydefault: truejoints:- 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姿态。

小结

至此moveit!与gazebo联合仿真配置完毕，配置原理部分不展开介绍，各位可以去古月居学习这部分的原理内容。接下去将讲解如何用yolo训练自己的数据集。

参考资料

1.古月居机械臂课程