ROS1-moveit-gazebo 仿真配置
隔几年没用ros1来做东西了,这几天想通过moveit和gazebo来做一个大模型智能作业仿真,就搭配一个越疆的ME6协助机械臂gazebo仿真环境。在利用mvoeit assistant setup来生成一些配置文件后,在配置几个控制器过程碰到一个问题,网上搜好多人都碰到,就算加载不到控制器,提示如下:
由于moveit助理程序生成的配置文件里面好多都是重复的,搞了大半天才梳理清楚,这里记录下,主要从整个流程框架去记录。
一、moveit和ros cotrollers,gazebo的关系
先上一张图:
核心就是ros cotroller这个功能包提供的三个controller:
负责仿真环境里面机器人的arm_controller和joint_state_controller,即在上图右边绿色框的两部分对应的两个控制器,负责轨迹插补和执行轴的运动控制和各轴状态的反馈,控制器的定义写成yaml格式
me6_robot:
arm_joint_controller:
type: position_controllers/JointTrajectoryController
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
gains:
joint1: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
joint2: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
joint3: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
joint4: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
joint5: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
joint6: {p: 1000.0, d: 0.0, i: 0.1, i_clamp: 0.0}
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
joint1: {trajectory: 0.1, goal: 0.1}
joint2: {trajectory: 0.1, goal: 0.1}
joint3: {trajectory: 0.1, goal: 0.1}
joint4: {trajectory: 0.1, goal: 0.1}
joint5: {trajectory: 0.1, goal: 0.1}
joint6: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
顶层met_robot主要为命名控制,区分是那个机器人,上面的type一定要和urdf文件里面的transmission里的硬件接口配置类型一样。
me6_robot:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
以及一个负责moveit和ros之间的Fellow joint cotroller,即中线左边的socket框,复杂封装打包moveit完成轨迹规划的路径点,通过socket发送到绿色框的arm_controller:
controller_list:
- name: /me6_robot/arm_joint_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: True
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
二、配置控制器和接口的关键
碰到的问题核心主要是moveit_manage_controllers这个参数的设置,主要有三种参数可以设置:fake, simple, ros_control,用gazebo仿真大部分都是用simple的,注意这里的三个参数值只是决定了最终在ros 服务器里面参数值的参数:
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
如果是ros_control 的话应该是:
<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
fake的话:
<param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
确定用那种类型后,就是要导入前面提到的三个控制器中moveit这一侧的定义 yaml文件。
其实要配置的东西不多,之所以折腾半天,主要是mvoeit助理插件生成的配置文件和launch文件是在过于冗余,把所有可能类型的配置和launch文件都帮你生成了,而且launch文件是一个include一个,且控制器配置还不是正确的所以如果你不熟悉整个原理,真的很容易乱,比如以下就是moveit助理插件生成的文件:
很多lauch文件实际不需要的,它只是先帮你生成,你用那个改那个,这里以moveit gazebo仿真为例,说明整个launch的链接关系。
1.首先你要把gazebo里面的机器人模型、场景等启动的launch文件写好;
2.新建一个moveit和gazebo 的bring_up.launch文件,在这个文件里面先把第1步写好的gazebo的仿真环境include 进来,再把前面配置好的gazebo这端的arm_controller和joint_state_controller 两个控制器yaml文件以rosparam的方式load进来,再启动一个ros控制器管理器节点(pkg=“controller_manager“),记得配置对应arm_controller和joint_state_controller的两个节点arg参数给这个节点。再remap robot_state_publisher 和joint_state_publisher的topic,主要指定命名空间。
以上这步的launch内容如下:
<launch>
<!-- Load the gazebo model -->
<include file="$(find me6_gazebo)/launch/me6_robot_gazebo.launch" />
<!-- Load the controllers rosparam -->
<rosparam command="load" file="$(find me6_gazebo)/config/me6_gazebo_joint_state_controller.yaml"/>
<rosparam command="load" file="$(find me6_gazebo)/config/me6_gazebo_arm_joint_controller.yaml"/>
<!-- Load the controllers manager -->
<node name="controller_spawner" pkg="controller_manager" type="spawner"
respawn="false" output="screen" ns="/me6_robot"
args="joint_state_controller
arm_joint_controller
--timeout 60 " />
<!--remap robot_state_publisher suscripte topic to me6_robot/joint_states,for rize and rqt -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from ="/joint_states" to="/me6_robot/joint_states"/>
</node>
<!-- Load the joint_state_publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
<remap from ="/joint_states" to="/me6_robot/joint_states"/>
3.在这个bring_up.launch文件里继续增加moveit着一块的配置,这个配置就涉及到一堆的串联配置launch文件了。主要内容如下:
<!-- Load the moveit configuration -->
<include file="$(find me6_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="false" />
</include>
<include file="$(find me6_moveit_config)/launch/move_group.launch">
<arg name="publish_monitored_planning_scene" value="true" />
</include>
<include file="$(find me6_moveit_config)/launch/moveit_rviz.launch">
<arg name="rviz_config" value="$(find me6_moveit_config)/launch/moveit.rviz" />
</include>
</launch>
上面三个launch文件 助理插件都帮你生成好了,其中两个planning_context.launch 和 moveit_rviz.launch都不用改,出错主要是在中间的move_group.launch文件里,而核心是把最开始将的moveit_manage_controllers这个参数的设置确定对就行了:
move_group.launch里的配置主要是用什么规划器之类的,这些都不用改,最核心的就是它include了trajectory_execution.launch.xml ,而这个文件又include了最终配置moveit_manage_controllers这个ros服务器里的参数的launch文件,我上面所有launch文件里的simple_moveit_controller_manager.launch.xml文件,这个simple_moveit_controller_manager.launch.xml配置了最终的moveit_manage_controllers 参数,并把上面提到的三个控制器中的moveit这边的控制器定义配置yaml文件加载进来。所以就像俄罗斯套娃一样一个套一个,最终核心的就是x_moveit_controller_manager.launch.xml(x代表了你include那个类型的moveit_controller_manager启动文件)。当然这些都是在moveit助理插件生成的launch文件。其实完全可以直接写一个没那么多套娃的launch文件。
还有一个导致我一开始混乱的是前面提到的x_moveit_controller_manager.launch.xml中的x在这些launch文件里面有重复设置,通过arg来设置,moveit_group.launch文件里面有,在trajectory_execution.launch.xml 里面也有,如果你都不注释掉,真正决定的是moveit_group.launch里面设置的这个参数:
我把关键串联的代码放下面,
1.moveit_group.launch
<!-- move_group settings -->
<arg name="pipeline" default="ompl" />
<arg name="allow_trajectory_execution" default="true"/>
<arg name="moveit_controller_manager" default="simple" />
<arg name="fake_execution_type" default="interpolate"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="publish_monitored_planning_scene" default="true"/>
………………
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<!-- <arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" /> -->
<arg name="fake_execution_type" value="$(arg fake_execution_type)" />
</include>
2.trajectory_execution.launch.xml文件
<!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" default="simple" />
<arg name="fake_execution_type" default="interpolate" />
………………
<!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
<include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
3.x_moveit_controller_manager.launch.xml, x的值取决于moveit_controller_manager这个arg参数
<launch>
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<!-- Load controller list to the parameter server -->
<rosparam file="$(find me6_moveit_config)/config/simple_moveit_controllers.yaml" />
<!-- <rosparam file="$(find me6_moveit_config)/config/ros_controllers.yaml" /> -->
</launch>
这里的simple_moveit_controllers.yaml文件里面就算前面提到的三个控制器中的第三个,即moveit这边的这个控制器的配置。
所以其实没有多少要修改的,值是助手插件生成的文件实在太冗余了,当然如果报错可能跟你的控制器的命名控制不一致有关。