ROS学习记录12【SLAM】仿真学习1——ROS Control
零.前言
学完了ROS的基础应用，我们就可以开始向更复杂的应用开始学习了，比如我们的SLAM。但是昂贵的底盘与激光雷达是学习这一部分的阻碍，为了弥补经费的不足，我们可以从0开始，创建一个自己的仿真小车，然后实现SLAM过程。最后真的有幸用到激光雷达和机器人底盘，那么直接移植写好的内容就可以了.
本次实验的平台为ubuntu 20.04 + ros/noetic，无特殊说明，后面的教程都为这个。
后面会把整个项目打包上传至github上，地址待定，写完再改。
一.什么是ros_control
该ros_control包是一个重写pr2_mechanism包，使控制器通用的并不仅止于PR2所有机器人。
它是介于你的机器人硬件代码以及各种定位导航的一个中间件。
你可以自己编写节点将下位机的数据传输至上位机并自己处理数据并进行控制。当然也可以交给ros_control去处理。当然ros_control除了将数据处理成我们定位导航包的需求以外，同时还会传递至Gazebo仿真环境中。这里我就尝试边学边做一个简单的Demo，去实现一下简单的ros_control。
你可以理解为：
它的结构是这样的：
其官网地址：https://wiki.ros.org/ros_control#Examples
二.创建工作空间与包
先安装ros_control：
sudo apt-get install ros-noetic-ros-control ros-noetic-ros-controllers ros-noetic-velocity-controllers ros-noetic-view-controller-msgs
在工作空间下创建一个SLAM的模型包：
假设先拥有这些包，后面不够的话再添加：std_msgs roscpp rospy controller_manager joint_state_controller robot_state_publisher
catkin_create_pkg slam_model std_msgs roscpp rospy controller_manager joint_state_controller robot_state_publisher
然后编译，修改环境变量：
source /home/kanna/ros_ws/devel/setup.bash
在slam_model下创建urdf和launch文件夹，最后整个文件夹结构看起来因该是这样：
三.创建一个能旋转和能扭动的小机构
在slam_model下创建一个nbmod.urdf
写入：
<robot name="nbmod">
<link name="base_footprint">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
geometry>
visual>
link>
<joint name="base_footprint_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<parent link="base_footprint"/>
<child link="base_link"/>
joint>
<link name="base_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<geometry>
<box size="1 1 1"/>
geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 0 0.8 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
geometry>
<inertial>
<mass value="20"/>
<inertia ixx="0.00533333333333" ixy="0" ixz="0" iyy="0.00833333333333" iyz="0" izz="0.0666666666667"/>
inertial>
collision>
link>
<joint name="joint_base2lidar" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="lidar"/>
<origin rpy="0 0 0" xyz="0 0 0.75"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
joint>
<link name="lidar">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<geometry>
<cylinder length="0.5" radius="0.25"/>
geometry>
<material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.5" radius="0.25"/>
geometry>
<inertial>
<mass value="1"/>
<inertia ixx="0.000379166666667" ixy="0" ixz="0" iyy="0.000379166666667" iyz="0" izz="0.000625"/>
inertial>
collision>
link>
<joint name="axle_lidar2barrel" type="revolute">
<parent link="lidar"/>
<child link="barrel"/>
<origin rpy="0 -1.57 0" xyz="-0.25 0 0.25"/>
<axis xyz="0 1 0"/>
<limit effort="2.5" lower="0" upper="0.785" velocity="0.1"/>
joint>
<link name="barrel">
<inertial>
<origin xyz="0 0 0.25"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<geometry>
<cylinder length="0.5" radius="0.01"/>
geometry>
<material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<geometry>
<cylinder length="0.5" radius="0.01"/>
geometry>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.000379166666667" ixy="0" ixz="0" iyy="0.000379166666667" iyz="0" izz="0.000625"/>
inertial>
collision>
link>
<gazebo reference="base_link">
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="lidar">
<material>Gazebo/Bluematerial>
gazebo>
<gazebo reference="barrel">
<material>Gazebo/Redmaterial>
gazebo>
robot>
这样就做好了一个简易的坦克。现在编写launch在rviz和gazebo里看看效果
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">include>
<param name="robot_description" textfile="$(find slam_model)/urdf/nbmod.urdf" />
<param name="use_gui" value="true"/>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_model)/urdf/config.rviz" required="true">node>
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find slam_model)/urdf/nbmod.urdf -urdf -model nbmod" output="screen"/>
launch>
在配置好rviz的可视化参数后，我们可以移动某些关节，在rviz里看到效果，可是gazebo纹丝不动。
这时候我们的话题关系是这样的：
那么我们就要请出ros_control插件进行指令的转义了，将joint_states等话题发布给gazebo
四.ROS Control
4.1 认识Gazebo ROS Control Plugin
Gazebo ROS Control Plugin这个继承于gazebo::ModelPlugin类，也是我们仿真时用的插件。最主要的还是编写ROS Controller。其源码地址：https://github.com/ros-simulation/gazebo_ros_pkgs
我们最主要的是控制某些关节的位置，那么就会用到这个JointPositionController或JointGroupPositionController二者的区别就是：注册一个控制器，是控制一个joint还是多个joint。
以及，我们所有关节的tf关系是由/namespace/joint_states这个话题所发布的。
4.2 在URDF里注册插件
首先在我们nbmod.urdf里面添加我们的transmission，官方说明地址：http://wiki.ros.org/urdf/XML/Transmission。
添加两个transmission
<transmission name="tran_lidar">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="joint_base2lidar">
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<transmission name="tran_barrel">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="axle_lidar2barrel">
<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>
joint>
<actuator name="motor2">
<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<robotNamespace>/nbmodrobotNamespace>
plugin>
gazebo>
我们需要修改的：
transmission命名urdf里的关节名1
然后还需要的是注册我们的插件：
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<robotNamespace>/nbmodrobotNamespace>
plugin>
gazebo>
唯一要改的就是为该机器人创建个命名空间，也就是等会儿我们的话题等，前面会有一个/nbmod的前缀。
4.2 为control配置参数
首先，我们在slam_model里创建一个config文件夹，然后创建一个nbmod_control.yaml并写入
nbmod:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
axle_lidar2barrel_position_controller:
type: position_controllers/JointPositionController
joint: axle_lidar2barrel
pid:
p: 100.0
i: 0.1
d: 10.0
joint_base2lidar_velocity_controller:
type: velocity_controllers/JointVelocityController
joint: joint_base2lidar
pid: {p: 100, i: 0.1, d: 10.0}
按照：
命名空间
- 控制器
-- 控制器参数
这样的格式写好配置，具体参数的配置在官网可以查到https://wiki.ros.org/robot_mechanism_controllers，比如PositionController的例子是：
Subscribed Topics
command (std_msgs/Float64)
The position to command
Parameters
joint (string, default: Required)
The joint to control
pid/p (double, default: Required)
The proportional gain relating efforts to the error in position
pid/d (double, default: 0.0)
Derivative gain, relating efforts to velocity (derivative of position)
pid/i (double, default: 0.0)
Integral gain
pid/i_clamp (double, default: 0.0)
Bounds enforced on the integral windup
4.3 创建launch文件
不变的部分
<include file="$(find gazebo_ros)/launch/empty_world.launch">include>
<param name="robot_description" textfile="$(find slam_model)/urdf/nbmod.urdf" />
<param name="use_gui" value="true"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_model)/urdf/config.rviz" required="true">node>
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find slam_model)/urdf/nbmod.urdf -urdf -model nbmod" output="screen"/>
这里是启动rviz和gazebo，删除了能在rviz的joint发布器和控制器。
新加的：
<rosparam file="$(find slam_model)/config/nbmod_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/nbmod" args="joint_state_controller axle_lidar2barrel_position_controller joint_base2lidar_velocity_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
<remap from="/joint_states" to="/nbmod/joint_states" />
node>
- 第一行是将我们的控制器的参数配置加入进来
- 第二行是我们创建的
controller共三个：joint_state_controller、axle_lidar2barrel_position_controller、joint_base2lidar_velocity_controller - 第三行是
rviz需要的姿态信息话题，但是我们控制gazebo的时候是在/nbmod/joint_states发布，所以要将rviz上的/joint_states重映射到/nbmod/joint_states上，这样就可以实现对rviz上的控制了。
4.4 使用
首先我们把刚才的launch启动：
然后这个红字可忽略，我们看到controller已经成功加载了
现在有以下话题：
然后我们对这两个controller发布消息：
刚才也提到过：
Subscribed Topics
command (std_msgs/Float64)
The position to command
所以我们这样发送：
rostopic pub -1 /nbmod/joint_base2lidar_velocity_controller/command std_msgs/Float64 "data: 1.2"
rostopic pub -1 /nbmod/axle_lidar2barrel_position_controller/command std_msgs/Float64 "data: 0.2"
此时我们的这个坦克就转起来了：
五.踩坑日记
之前死活：Could not load controller 'joint_base2lidar_velocity_controller' because controller type 'velocity_controllers/JointVelocityController' does not exist.
然后提示：
Use 'rosservice call controller_manager/list_controller_types' to get the available types
然
我就去输出，结果就只有四个controller，没有其他的。我第一反映是下包，然后各种尝试，乱拼乱凑，结果不行。甚至试了：install robot_mechanism_controllers
然后我顺着搜索，就找到了：
https://answers.ros.org/question/254084/gazebo-could-not-load-controller-jointtrajectorycontroller-does-not-exist-mastering-ros-chapter-10/
这个帖子也遇到了这个情况，然后提示说：
sudo apt-get install ros*controller*
然后下面又出现了：The following NEW packages will be installed: ros-kinetic-bhand-controller ros-kinetic-diff-drive-controller ros-kinetic-dynamixel-controllers ros-kinetic-dynamixel-driver ros-kinetic-dynamixel-msgs ros-kinetic-dynamixel-sdk ros-kinetic-dynamixel-workbench-controllers ros-kinetic-dynamixel-workbench-msgs ros-kinetic-dynamixel-workbench-toolbox ros-kinetic-force-torque-sensor-controller ros-kinetic-gripper-action-controller ros-kinetic-imu-sensor-controller ros-kinetic-joint-trajectory-controller ros-kinetic-kobuki-controller-tutorial ros-kinetic-md49-base-controller ros-kinetic-md49-messages ros-kinetic-md49-serialport ros-kinetic-nav-pcontroller ros-kinetic-robot-controllers ros-kinetic-robot-controllers-interface ros-kinetic-robot-controllers-msgs ros-kinetic-robotis-controller-msgs ros-kinetic-ros-controllers ros-kinetic-rqt-controller-manager ros-kinetic-rqt-joint-trajectory-controller ros-kinetic-velocity-controllers ros-kinetic-view-controller-msgs ros-kinetic-yocs-diff-drive-pose-controller ros-kinetic-yocs-safety-controller
我一Ctrl+F，就定位到了我们要的东西：
ros-noetic-velocity-controllers!!!
基本上国内很多教程使用到了这个，但是都没有安装过程，都只是说用了官网的一段文字：sudo apt-get install ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control
但这个是不全的，所以得加上我们的这个即可，问题解决！
六.全部代码
6.1 nbmod_control.yaml
ros_ws/src/slam_model/config/nbmod_control.yaml：
nbmod:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
axle_lidar2barrel_position_controller:
type: position_controllers/JointPositionController
joint: axle_lidar2barrel
pid:
p: 100.0
i: 0.1
d: 10.0
joint_base2lidar_velocity_controller:
type: velocity_controllers/JointVelocityController
joint: joint_base2lidar
pid: {p: 100, i: 0.1, d: 10.0}
6.2 nbmod_show.launch
ros_ws/src/slam_model/launch/nbmod_show.launch
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">include>
<param name="robot_description" textfile="$(find slam_model)/urdf/nbmod.urdf" />
<param name="use_gui" value="true"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
<remap from="/joint_states" to="/nbmod/joint_states" />
node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_model)/urdf/config.rviz" required="true">node>
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find slam_model)/urdf/nbmod.urdf -urdf -model nbmod" output="screen"/>
<rosparam file="$(find slam_model)/config/nbmod_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/nbmod" args="joint_state_controller axle_lidar2barrel_position_controller joint_base2lidar_velocity_controller"/>
launch>
6.3 nbmod.urdf
ros_ws/src/slam_model/urdf/nbmod.urdf
<robot name="nbmod">
<link name="base_footprint">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
geometry>
visual>
link>
<joint name="base_footprint_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.5"/>
<parent link="base_footprint"/>
<child link="base_link"/>
joint>
<link name="base_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<geometry>
<box size="1 1 1"/>
geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue">
<color rgba="0 0 0.8 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
geometry>
<inertial>
<mass value="20"/>
<inertia ixx="0.00533333333333" ixy="0" ixz="0" iyy="0.00833333333333" iyz="0" izz="0.0666666666667"/>
inertial>
collision>
link>
<joint name="joint_base2lidar" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="lidar"/>
<origin rpy="0 0 0" xyz="0 0 0.75"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
joint>
<link name="lidar">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<geometry>
<cylinder length="0.5" radius="0.25"/>
geometry>
<material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.5" radius="0.25"/>
geometry>
<inertial>
<mass value="1"/>
<inertia ixx="0.000379166666667" ixy="0" ixz="0" iyy="0.000379166666667" iyz="0" izz="0.000625"/>
inertial>
collision>
link>
<joint name="axle_lidar2barrel" type="revolute">
<parent link="lidar"/>
<child link="barrel"/>
<origin rpy="0 -1.57 0" xyz="-0.25 0 0.25"/>
<axis xyz="0 1 0"/>
<limit effort="2.5" lower="0" upper="0.785" velocity="0.1"/>
joint>
<link name="barrel">
<inertial>
<origin xyz="0 0 0.25"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<geometry>
<cylinder length="0.5" radius="0.01"/>
geometry>
<material name="black">
<color rgba="0 0 0 1"/>
material>
visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<geometry>
<cylinder length="0.5" radius="0.01"/>
geometry>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.000379166666667" ixy="0" ixz="0" iyy="0.000379166666667" iyz="0" izz="0.000625"/>
inertial>
collision>
link>
<gazebo reference="base_link">
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="lidar">
<material>Gazebo/Bluematerial>
gazebo>
<gazebo reference="barrel">
<material>Gazebo/Redmaterial>
gazebo>
<transmission name="tran_lidar">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="joint_base2lidar">
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/VelocityJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<transmission name="tran_barrel">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="axle_lidar2barrel">
<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>
joint>
<actuator name="motor2">
<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<robotNamespace>/nbmodrobotNamespace>
plugin>
gazebo>
robot>