[ROS] ArbotiX控制+xacro机器人仿真模型设计实现初级建模与车体控制

心已赠人 2023-02-17 11:08 1阅读 0赞

###  1.环境准备 ###

需要：ros系统，ArbotiX功能包

假设读者已经安装好ROS系统（百度有很多安装教程），下面安装arbotix功能包已经ros依赖库

> $ git clone https://gitee.com/linshu1994/arbotix\_ros.git arbotix
> 
> $ apt install ros-kinetic-arbotix-\*

### 2.工作空间以及功能包创建 ###

//创建工作空间
    $ source /opt/ros/kinetic/setup.zsh
    $ mkdir -p ~/catkin_ws/src
    $ cd ~/catkin_ws/src
    $ catkin_init_workspace
    $ cd ~/catkin_ws
    $ catkin_make 
    $ souce ~/catkiin_ws/devel/setup.zsh
    $ cd ~/catkin_ws/src
    
    //编译arbotix
    $ mv arbotix ~/catkin_ws/src
    $ catkin_make -DCATKIN_WHITELIST_PACKAGES="arbotix"
    
    //创建功能包
    $ catkin_create_pkg test1 urdf xacro
    $ cd ~/catkin_ws
    $ catkin_make
    $ source devel/setup.zsh
    
    //创建文件目录
    $ cd ~/catkin_ws/src/test1
    $ mkdir urdf launch 
    $ mkdir urdf/xacro

### 3.创建xacro文件 ###

关于xacro具体配置不做讲解，如果对xacro比较陌生的读者可以阅读这篇文章[xacro入门][xacro]，或者在底下评论留言

**1)机器人骨架**

机器人骨架包括一个圆柱体作为车身，左右驱动轮，两个万向轮（前后，其支撑作用），做成之后大概张这样

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70][]

> $ cd test3/urdf/xacro
> 
> $ touch mbot\_base.xacro

<?xml version="1.0"?>
    <robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
        
        <xacro:property name="M_PI" value="3.1415926"/>
        <xacro:property name="base_radius" value="0.20"/>
        <xacro:property name="base_length" value="0.16"/>
    
        <xacro:property name="wheel_radius" value="0.06"/>
        <xacro:property name="wheel_length" value="0.025"/>
        <xacro:property name="wheel_joint_y" value="0.19"/>
        <xacro:property name="wheel_joint_z" value="0.05"/>
    
        <xacro:property name="caster_radius" value="0.015"/> 
        <xacro:property name="caster_joint_x" value="0.18"/>
    
        
        <material name="yellow">
            <color rgba="1 0.4 0 1"/>
        </material>
        <material name="black">
            <color rgba="0 0 0 0.95"/>
        </material>
        <material name="gray">
            <color rgba="0.75 0.75 0.75 1"/>
        </material>
        
        
        <xacro:macro name="wheel" params="prefix reflect">
            <joint name="${prefix}_wheel_joint" type="continuous">
                <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
                <parent link="base_link"/>
                <child link="${prefix}_wheel_link"/>
                <axis xyz="0 1 0"/>
            </joint>
    
            <link name="${prefix}_wheel_link">
                <visual>
                    <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                    <geometry>
                        <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                    </geometry>
                    <material name="gray" />
                </visual>
            </link>
        </xacro:macro>
    
        
        <xacro:macro name="caster" params="prefix reflect">
            <joint name="${prefix}_caster_joint" type="continuous">
                <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
                <parent link="base_link"/>
                <child link="${prefix}_caster_link"/>
                <axis xyz="0 1 0"/>
            </joint>
    
            <link name="${prefix}_caster_link">
                <visual>
                    <origin xyz="0 0 0" rpy="0 0 0"/>
                    <geometry>
                        <sphere radius="${caster_radius}" />
                    </geometry>
                    <material name="black" />
                </visual>
            </link>
        </xacro:macro>
    
        <xacro:macro name="mbot_base">
            <link name="base_footprint">
                <visual>
                    <origin xyz="0 0 0" rpy="0 0 0" />
                    <geometry>
                        <box size="0.001 0.001 0.001" />
                    </geometry>
                </visual>
            </link>
    
            <joint name="base_footprint_joint" type="fixed">
                <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
                <parent link="base_footprint"/>
                <child link="base_link" />
            </joint>
    
            <link name="base_link">
                <visual>
                    <origin xyz=" 0 0 0" rpy="0 0 0" />
                    <geometry>
                        <cylinder length="${base_length}" radius="${base_radius}"/>
                    </geometry>
                    <material name="yellow" />
                </visual>
            </link>
    
            <wheel prefix="left" reflect="-1"/>
            <wheel prefix="right" reflect="1"/>
    
            <caster prefix="front" reflect="-1"/>
            <caster prefix="back" reflect="1"/>
        </xacro:macro>
    </robot>

** 2)激光雷达xacro**

激光雷达充当机器人的眼睛，作为机器人识别世界的重要媒介

> $ cd test3/urdf/xacro
> 
> $ touch lidar.xacro

<?xml version="1.0"?>
    <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">
    
    	<xacro:macro name="rplidar" params="prefix:=laser">
    		<link name="${prefix}_link">
    			<visual>
    				<origin xyz=" 0 0 0 " rpy="0 0 0" />
    				<geometry>
    					<cylinder length="0.05" radius="0.05"/>
    				</geometry>
    				<material name="black"/>
    			</visual>
    		</link>
    	</xacro:macro>
    
    </robot>

**3)组合xacro**

需要一个总的xacro文件做组合

<?xml version="1.0"?>
    <robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
        <xacro:include filename="$(find mbot_description)/urdf/xacro/mbot_base.xacro" />
        <xacro:include filename="$(find mbot_description)/urdf/xacro/lidar.xacro" />
    
        <xacro:property name="lidar_offset_x" value="0" />
        <xacro:property name="lidar_offset_y" value="0" />
        <xacro:property name="lidar_offset_z" value="0.10" />
    
        <mbot_base/>
    
        
        <joint name="lidar_joint" type="fixed">
            <origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
            <parent link="base_link"/>
            <child link="laser_link"/>
        </joint>
    
        <xacro:rplidar prefix="laser"/>
    
    </robot>

### **4.launch文件** ###

导入xacro文件，启动arbotix结点,用于控制车体的移动。启动joint\_state\_publisher，发布机器人的关节状态（左右轮，前后轮,包括转速，差速，旋转方向等），启动robot\_state\_publisher，发布机器人主题的状态（其位姿，欧拉角等）

### 5.车体控制程序 ###

我们需要一个程序来控制车体的移动

$ cd ~/catkin_ws/src
    
    //创建功能包
    $ catkin_create_pkg mybot_teleop roscpp rospy geometry_msgs
    $ cd ~/catkin_ws
    $ catkin_make
    $ source devel/setup.zsh
    
    //创建文件目录
    $ cd mybot_teleop 
    $ mkdir scripts launch 
    $ touch scripts/mbot_teleop.py #创建控制程序
    $ touch launch/mbot_teleop.launch #创建控制启动程序

编写车体控制程序

#!/usr/bin/env python
    # -*- coding: utf-8 -*-
    
    import rospy
    from geometry_msgs.msg import Twist
    import sys, select, termios, tty
    
    msg = """
    Control mbot!
    ---------------------------
    #实现对车体控制,输入i给车体一个向前1m/s的速度，角速度0，以此推列
    Moving around:
       u    i    o
       j    k    l
       m    ,    .
    
    q/z : increase/decrease max speeds by 10%
    w/x : increase/decrease only linear speed by 10%
    e/c : increase/decrease only angular speed by 10%
    space key, k : force stop
    anything else : stop smoothly
    
    CTRL-C to quit
    """
    
    moveBindings = {
            'i':(1,0),
            'o':(1,-1),
            'j':(0,1),
            'l':(0,-1),
            'u':(1,1),
            ',':(-1,0),
            '.':(-1,1),
            'm':(-1,-1),
               }
    
    speedBindings={
            'q':(1.1,1.1),
            'z':(.9,.9),
            'w':(1.1,1),
            'x':(.9,1),
            'e':(1,1.1),
            'c':(1,.9),
              }
    
    # 功能函数：用于获取键盘输入
    def getKey():
        tty.setraw(sys.stdin.fileno())
        rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
        if rlist:
            key = sys.stdin.read(1)
        else:
            key = ''
    
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
        return key
    
    speed = .2
    turn = 1
    
    #输出角速度线速度
    def vels(speed,turn):
        return "currently:\tspeed %s\tturn %s " % (speed,turn)
    
    if __name__=="__main__":
        settings = termios.tcgetattr(sys.stdin)
        
        rospy.init_node('mbot_teleop')
        # 发布速度话题
        pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    
        x = 0
        th = 0
        status = 0
        count = 0
        acc = 0.1
        target_speed = 0
        target_turn = 0
        control_speed = 0
        control_turn = 0
        try:
            print msg
            print vels(speed,turn)
            while(1):
                key = getKey()
                # 运动控制方向键（1：正方向，-1负方向）
                if key in moveBindings.keys():
                    x = moveBindings[key][0]
                    th = moveBindings[key][1]
                    count = 0
                # 速度修改键
                elif key in speedBindings.keys():
                    speed = speed * speedBindings[key][0]  # 线速度增加0.1倍
                    turn = turn * speedBindings[key][1]    # 角速度增加0.1倍
                    count = 0
    
                    print vels(speed,turn)
                    if (status == 14):
                        print msg
                    status = (status + 1) % 15
                # 停止键
                elif key == ' ' or key == 'k' :
                    x = 0
                    th = 0
                    control_speed = 0
                    control_turn = 0
                else:
                    count = count + 1
                    if count > 4:
                        x = 0
                        th = 0
                    if (key == '\x03'):
                        break
    
                # 目标速度=速度值*方向值
                target_speed = speed * x
                target_turn = turn * th
    
                # 速度限位，防止速度增减过快
                if target_speed > control_speed:
                    control_speed = min( target_speed, control_speed + 0.02 )
                elif target_speed < control_speed:
                    control_speed = max( target_speed, control_speed - 0.02 )
                else:
                    control_speed = target_speed
    
                if target_turn > control_turn:
                    control_turn = min( target_turn, control_turn + 0.1 )
                elif target_turn < control_turn:
                    control_turn = max( target_turn, control_turn - 0.1 )
                else:
                    control_turn = target_turn
    
                # 创建并发布twist消息
                twist = Twist()
                twist.linear.x = control_speed; 
                twist.linear.y = 0; 
                twist.linear.z = 0
                twist.angular.x = 0; 
                twist.angular.y = 0; 
                twist.angular.z = control_turn
                pub.publish(twist)
    
        except:
            print e
    
        finally:
            twist = Twist()
            twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
            pub.publish(twist)
    
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

编写launch文件

### 6.执行 ###

开启2个终端,arbotix\_mbot\_xacro.launch用于发布车体仿真模型，mbot\_teleop.launch用于控制小车

> $ roslaunch test3 arbotix\_mbot\_xacro.launch
> 
> $ roslaunch mbot\_teleop mbot\_teleop.launch

开启mbot\_teleop.launch 后，留在这个终端，输入字符控制移动，i代表前进，逗号向后退，可自行尝试，q/z给小车加减速

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 1][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 2][]

[xacro]: https://blog.csdn.net/ck784101777/article/details/106183873
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200613230022126.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200613231519338.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20200613230825374.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NrNzg0MTAxNzc3,size_16,color_FFFFFF,t_70