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

 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入门，或者在底下评论​​​​​留言

1)机器人骨架

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

$ cd test3/urdf/xacro

$ touch mbot_base.xacro

 2)激光雷达xacro

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

$ cd test3/urdf/xacro

$ touch lidar.xacro

3)组合xacro

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

 

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给小车加减速