Gazebo tutorial学习-----ros_control+Gazebo

本文将使用ros_control和gazebo插件gazebo_ros_control实现模拟控制器，并驱动机器人joint运动。

1 gazebo和ros_control数据流

2 实验前置条件

请确保已经安装ros_control, ros_controllers。

3 客制化URDF文件

本文将继续使用gazebo中RRBot的URDF，并在其基础上做客制化修改。

3.1 Add transmission elements to a URDF

为了在机器人RRBot中使用ros_control，我们需要在URDF中增加一些相应的elements（如 <transmission> element），这种element被用来连接机器人的actuators和 joints, 详细定义请看<transmission> spec 。

对于gazebo_ros_control插件来说，transmission中重要的信息tag如下：

<joint name=""> - name必须于URDF中其它地方的joint对应
<type> - transmission类型，目前仅仅实现了"transmission_interface/SimpleTransmission"，当然你可以自己根据需求添加
<hardwareInterface> - 放置在<actuator> 和<joint> tags之内,用来告知gazebo_ros_control插件哪些硬件接口要加载进来(position, velocity or effort interfaces)，目前仅effort接口被实现，根据需要可自行添加其它接口

其它剩下的names and elements目前被忽略处理。

3.2 Add the gazebo_ros_control plugin

除了transmission tags, Gazebo插件也需要添加到URDF文件，该插件作用是解析transmission tags，加载hardware interfaces和controller manager。缺省的情况下插件gazebo_ros_control很简单，然而基于其插件架构特定，用户可以随便扩展定制自己需要的硬件接口。

下面缺省的插件XML定义应该加入到你的URDF文件，

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
  </plugin>
</gazebo>

gazebo_ros_control <plugin> tag有下面几个可选属性：

<robotNamespace>: 插件实例的ROS命名空间，在URDF/SDF文件缺省是机器人名字
<controlPeriod>: controller更新周期update (in seconds), 缺省是Gazebo的period
<robotParam>: 参数服务器robot_description (URDF)的位置，缺省是'/robot_description'
<robotSimType>: robot sim interface插件名字 (详细见下面), 缺省是 'DefaultRobotHWSim'

缺省的gazebo_ros_control的行为：

在缺省情况下，没有 <robotSimType> tag, gazebo_ros_control会尝试获取它需要的所有接口以实现与基于ros_control的controller通信。

在缺省情况下，它的行为提供如下ros_control接口:

hardware_interface::JointStateInterface
hardware_interface::EffortJointInterface
hardware_interface::VelocityJointInterface - not fully implemented

高级gazebo_ros_control的行为（定制gazebo_ros_control插件）：

gazebo_ros_control插件也提供了基于pluginlib的接口，这样方便客制化Gazebo和ros_control之间的硬件接口，以仿真更加复杂的机构（如非线性弹簧，连接等等）。

客制化的新插件必须继承gazebo_ros_control::RobotHWSim，它是仿真的hardware_interface::RobotHW。 RobotHWSim提供的API可以实现对Gazebo模拟器中joint的读和写。

RobotHWSim的扩展类在URDF model中指定，当机器人model被加载时候也被一起加载。如下面的XML将加载缺省插件 (行为同没有使用<robotSimType> tag情况):

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

3.3 RRBot例子

我们添加 <transmission> 块为每个joint。注意：<hardwareInterface>必须包含在<joint>和<actuator> tags(seeros_control issue here)之间。打开你的rrbot.xacro文件，在文件底部你可以看到：

 <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

此外，在文件rrbot.gazebo你会看到插件gazebo_ros_control，该插件会读取<transmission> tags中内容：

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/rrbot</robotNamespace>
  </plugin>
</gazebo>

4 创建ros_control package

接下来我们为ros_control已经支持的controller创建配置文件和启动文件。

4.1 创建新package

mkdir ~/catkin_ws
cd ~/catkin_ws
catkin_create_pkg MYROBOT_control ros_control ros_controllers
cd MYROBOT_control
mkdir config
mkdir launch

4.2 创建.yaml配置文件

PID gains和controller设置需要保存到yaml 文件，该文件被roslaunch调用，其内容加载到了参数服务器。在config文件夹，将如下文件内容应用到你robot中。MYROBOT_control/config/rrbot_control.yaml:

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

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

4.3 创建roslaunch文件

创建roslaunch文件启动controller。在launch文件夹创建MYROBOT_control/launch/MYROBOT_control.launch文件，并使用下面内容：

<launch>

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

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/rrbot/joint_states" />
  </node>

</launch>

以上代码解读如下：

1）上面第一行"rosparam", 加载yaml文件中controller的配置到参数服务器。

2）controller_spawner node调用python脚本启动2个位置controller，该脚本创建service call到controller manager，告诉controller manager需要启动哪些controller。

该脚本也加载第三个controller，该controller用来发布所有关节的状态，并且广播主题/joint_states。 spawner只是roslaunch使用的辅助脚本。

3）最后一行启动一个robot_state_publisher node，用来监听来自joint_state_controller的消息/joint_states，然后发布转换到/tf主题。这样在Rviz中你可以看到仿真的robot并做一些其它任务。

5 用roslaunch启动controller

5.1 启动RRBot simulation:

roslaunch rrbot_gazebo rrbot_world.launch

5.2 加载2个joint的controller

roslaunch rrbot_control rrbot_control.launch

或者可以手动使用service calls完成controller加载，启动，停止：

5.3 首先加载rrbot_control.yaml到参数服务器中

5.4 加载controller:

rosservice call /rrbot/controller_manager/load_controller "name: 'joint1_position_controller'"
rosservice call /rrbot/controller_manager/load_controller "name: 'joint2_position_controller'"

5.5 启动controller:

rosservice call /rrbot/controller_manager/switch_controller \
"{start_controllers: ['joint1_position_controller','joint2_position_controller'], stop_controllers: [], strictness: 2}"

5.6 停止controller:

rosservice call /rrbot/controller_manager/switch_controller \
"{start_controllers: [], stop_controllers: ['joint1_position_controller','joint2_position_controller'], strictness: 2}"

6 发送command

6.1 手动发送

rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

6.2 Use RQT To Send Commands

在这一部分中我们会使用tools来可视化的看controller的性能并对controller参数和增益进行优化，尤其是PID增益。我们会使用RQT，基于ROS中插件的接口。

（1）启动RQT

rosrun rqt_gui rqt_gui

（2）添加command发布器

在RQT的'Plugins' menu，添加 'Topics->Message Publisher' 插件，然后从下拉框选择topic，可以将command送到你想控制的controller。

对于RRBot, 添加如下:

/rrbot/joint1_position_controller/command

然后按界面右上方绿色“＋”符号。通过topic name左侧的check box使能topic发布器。设置rate column到100 (这是发布频率)。

接下来，扩展topic你可以看到"data"行。在expression column, data row, 你可以尝试不同弧度值（在joint1的joint limits中都行），不过本例中由于joints是连续的所以没有限制，意味着任何值都可以work。

再往下，在同一个expression box中我们使用正玄波可以实现自动改变值，

sin(i/100)

对于更高级的控制，你可以配置发布正玄波给机器人明确joint limits：

sin(i/rate*speed)*diff + offset

变量说明:

i - the RQT variable for time
rate - the frequency that this expression is evaluated. This should be the same number as in the rate column of the topic publisher. Recommended value is 100.
speed - how quick you want the join to actuate. Start off with just 1 for a slow speed
upper_limit and lower_limits - the joint limits of the hardware being controlled by this controller
diff = (upper_limit - lower_limit)/2
offset = upper_limit-diff

（3）可视化controller性能

添加一个Plot插件到RQT并且添加上面发布器主题：

/rrbot/joint1_position_controller/command/data

点击绿色add按钮，你就可以看到正玄波。

添加另一个topic到Plot插件来跟踪命令的位置和执行器实际位置的误差：

/rrbot/joint1_position_controller/state/error

注意： RQT plot plugin可能有bug（超过1分钟之后画图开始奇怪）。目前解决方案是按压位于插件右上方的蓝色刷新按钮。

（4）优化PID增益

最后我们将使用dynamic reconfigure 来调节优化PID controller的比例，微分，积分增益。

添加'Dynamic Reconfigure' 插件到RQT，然后点击'Expand All'可以看子选项。如果你的controller使用了PID你应该使用 "pid"选项。点击之后会出现5个滑块可以调节controller，你可以通过调节它们直到获得你期望的controller性能为止。

（5）使用roslaunch保存RQT perspective

roslaunch rrbot_control rrbot_rqt.launch

7 连接Rviz和Gazebo仿真

现在你可以使用ros_control来发送命令到模拟机器人，你也可以用ros_control jointstatecontroller来读取gazebo中机器人状态。

如果你已经用前面的rosparam和roslaunch启动了joint_state_controller，下面你可以启动Rviz：

rosrun rviz rviz

修改"Global Options"里面的"Fixed Frame" 为"world"，添加"RobotModel"display type到Rviz之后你就可以在Rviz中看到gazebo中的仿真机器人了。

8 仿真结果显示

9 参考代码

https://github.com/ros-simulation/gazebo_ros_demos

来源：CSDN

作者：倔强不倒翁

链接：https://blog.csdn.net/x_r_su/article/details/53304034

标签

ros

gazebo