ROS学习记录12【SLAM】仿真学习1——ROS Control

最新推荐文章于 2024-06-09 23:12:00 发布

康娜喵

最新推荐文章于 2024-06-09 23:12:00 发布

阅读量3k

点赞数 13

分类专栏： ROS学习记录文章标签：自动驾驶人工智能 slam ros 仿真

本文链接：https://blog.csdn.net/u011017694/article/details/122357808

版权

ROS学习记录专栏收录该内容

17 篇文章 53 订阅

订阅专栏

零.前言

学完了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/White</material>
    </gazebo>
    <gazebo reference="lidar">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="barrel">
        <material>Gazebo/Red</material>
    </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/SimpleTransmission</type>
        <joint name="joint_base2lidar">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="motor1">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="tran_barrel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="axle_lidar2barrel">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="motor2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="ros_control">
            <robotNamespace>/nbmod</robotNamespace>
        </plugin>
    </gazebo>

我们需要修改的：

<transmission name="tran_lidar">，给这个transmission命名
<joint name="joint_base2lidar">，改为urdf里的关节名
<hardwareInterface> ，这里面内容根据你所用的类型以及文档上给的名称而定
<actuator name="motor1">，执行器名称，和硬件有关，改个名字就行，现在用不到
<mechanicalReduction>1</mechanicalReduction>，传动比，这个根据硬件而定，写1就行

然后还需要的是注册我们的插件：

    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="ros_control">
            <robotNamespace>/nbmod</robotNamespace>
        </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/White</material>
    </gazebo>
    <gazebo reference="lidar">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="barrel">
        <material>Gazebo/Red</material>
    </gazebo>

    <transmission name="tran_lidar">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_base2lidar">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="motor1">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="tran_barrel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="axle_lidar2barrel">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="motor2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="ros_control">
            <robotNamespace>/nbmod</robotNamespace>
        </plugin>
    </gazebo>
</robot>

康娜喵

关注

13
点赞
踩
55

收藏

觉得还不错? 一键收藏
打赏
6
评论
ROS学习记录12【SLAM】仿真学习1——ROS Control

零.前言学完了ROS的基础应用，我们就可以开始向更复杂的应用开始学习了，比如我们的SLAM。但是昂贵的底盘与激光雷达是学习这一部分的阻碍，为了弥补经费的不足，我们可以从0开始，创建一个自己的仿真小车，然后实现SLAM过程。最后真的有幸用到激光雷达和机器人底盘，那么直接移植写好的内容就可以了.本次实验的平台为ubuntu 20.04 + ros/noetic，无特殊说明，后面的教程都为这个。后面会把整个项目打包上传至github上，地址待定，写完再改。一.创建工作空间与包在工作空间下创建一个SLAM
复制链接

扫一扫