CoppeliaSim学习笔记之差速小车的控制与传感器的驱动_cupplieasim小车避障 lua

  上一章节 CoppeliaSim学习笔记之仿真环境与小车模型的搭建 我们将环境和小车都已经搭建完成，并且通过软件界面设置左右轮关节（电机）转速实现了小车的原地转圈。其实实际应用过程中，我们一般不这么搞，都是通过调用 API 接口去实现电机转速的实时设置，这就需要编写一些嵌入式代码了。CoppeliaSim默认的嵌入脚本是 Lua 语言，不会的可以直接看菜鸟教程，学习基本的就行。脚本的编写可参考 翻译手册二的第六章 在CoppeliaSim及其周围编写代码。

1. 控制篇

  右击 Robot Add-->Associated cild script-->Non threaded 添加非线程子脚本，并双击子脚本进行编辑。默认打开如下

function sysCall_init()
    -- do some initialization here
end

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

  其中，sysCall_init 函数为初始化函数，与C++中的构造函数类似，仅在仿真开始时执行一次，一般用来为仿真做一些准备工作；sysCall_actuation 可理解为驱动函数，在每次仿真过程中执行，主要负责处理仿真过程中的所有驱动功能；sysCall_sensing 同样在仿真过程中执行，主要负责所有传感器功能；sysCall_cleanup类似于C++中的析构函数，恢复初始状态，清除传感器状态等。

  此时，我想通过 rostopic 去控制小车的移动，那么此非线程子脚本实现步骤为：（前提是ros_interface已经配置完成，未配置的可参考博客）

• 使用 coppeliaSim 的ROS Interface API 订阅 rostopic；
• 推导小车差速移动模型，参考博客；
• 按照 coppeliaSim 嵌入式 lua 结构实现差速模型控制。

function sysCall_init()
    -- do some initialization here
    leftMotor  = sim.getObjectHandle("robot_leftMotor")  -- Handle of the left motor
    rightMotor = sim.getObjectHandle("robot_rightMotor") -- Handle of the right motor
    -- Launch the ROS client application:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        cmdVelSub  = simROS.subscribe('/cmd_vel','geometry_msgs/Twist','cmd_vel_callback')
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing()
    -- put your sensing code here
end

function cmd_vel_callback(msg)
    -- This is the velocity callback function
    local wheel_radius = 0.17/2.0         -- wheel radius(drive wheel)
    local wheel_tread  = 0.19*2           -- the distance between left and right wheel

    vLeft  = (1.0/wheel_radius)*(msg.linear.x - wheel_tread/2.0*msg.angular.z)
    vRight = (1.0/wheel_radius)*(msg.linear.x + wheel_tread/2.0*msg.angular.z)
    sim.setJointTargetVelocity(leftMotor,  vLeft)
    sim.setJointTargetVelocity(rightMotor, vRight)
end

function sysCall_cleanup()
    -- do some clean-up here
    if simROS then
        simROS.shutdownSubscriber(cmdVelSub)
    end
end
-- See the user manual or the available code snippets for additional callback functions and details

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

  init 函数通过 sim.getObjectHandle获取左右电机的句柄，simROS.subscribe订阅 ros 中的控制速度，cmd_vel_callback回调函数实时计算预下发给电机的左右轮转速，最终通过sim.setJointTargetVelocity 下发给指定电机。

注意：仿真时需选择 real-time mode 按指定布长仿真。

2. 传感器篇

2.1 里程计仿真

  里程计理论上是两个轮子累积的距离上报出来，在 CoppeliaSim 中，我首先用理想的距离值去替代的方案，后期再推导累积里程的方案。理想的距离值就是分别创建机器人的坐标系 base_link 和里程计的坐标系 odom，通过 CoppeliaSim 的 world 坐标系获取两个坐标系之间的相对位置、姿态信息，然后通过 ROS 的方式发布出来。

function sysCall_init()
    -- do some initialization here
    -- Greeting message
    sim.addStatusbarMessage('Starting robot simulation')
    
    -- Get the body handler, needed to get the absolute position and velocity of the robot in the scene
    odomHandle = sim.getObjectHandle("Odom")
    baselinkHandle = sim.getObjectHandle("Base_link")
    leftMotor  = sim.getObjectHandle("robot_leftMotor")  -- Handle of the left motor
    rightMotor = sim.getObjectHandle("robot_rightMotor") -- Handle of the right motor
    -- Launch the ROS client application:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        cmdVelSub = simROS.subscribe('/cmd_vel','geometry_msgs/Twist','cmd_vel_callback')
        odomPub = simROS.advertise('/odom', 'nav_msgs/Odometry')
        simROS.publisherTreatUInt8ArrayAsString(odomPub)
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing()
    -- put your sensing code here
    -- Get the robot position, orientation and velocities
    local pos = sim.getObjectPosition(baselinkHandle,odomHandle)
    local quat = sim.getObjectQuaternion(baselinkHandle,odomHandle)
    local velocity_linear, velocity_angular = sim.getObjectVelocity(baselinkHandle)
    if simROS then
        local odom_data = {}
        odom_data['header'] = {seq = 0, stamp = simROS.getTime(), frame_id = "odom"}
        odom_data['child_frame_id'] = "base_link"
        odom_data['pose'] = {}
        odom_data['pose']['pose'] = {}
        odom_data['pose']['pose']['position'] = {x = pos[1], y = pos[2], z = pos[3]}
        odom_data['pose']['pose']['orientation'] = {x = quat[1], y = quat[2], z = quat[3], w = quat[4]}
        odom_data['pose']['covariance'] = {0}
        odom_data['twist'] = {}
        odom_data['twist']['twist'] = {}
        odom_data['twist']['twist']['linear'] = {x = velocity_linear[1], y = velocity_linear[2], z = velocity_linear[3]}
        odom_data['twist']['twist']['angular'] = {x = velocity_angular[1], y = velocity_angular[2], z = velocity_angular[3]}
        odom_data['twist']['covariance'] = {0}
        simROS.publish(odomPub, odom_data)
    end
end

function cmd_vel_callback(msg)
    -- This is the sub_velocity callback function
    local wheel_radius = 0.17/2.0         -- wheel radius(drive wheel)
    local wheel_tread  = 0.19*2           -- the distance between left and right wheel

    vLeft  = (1.0/wheel_radius)*(msg.linear.x - wheel_tread/2.0*msg.angular.z)
    vRight = (1.0/wheel_radius)*(msg.linear.x + wheel_tread/2.0*msg.angular.z)
    sim.setJointTargetVelocity(leftMotor,  vLeft)
    sim.setJointTargetVelocity(rightMotor, vRight)
end

function sysCall_cleanup()
    -- do some clean-up here
    if simROS then
        simROS.shutdownSubscriber(cmdVelSub)
        simROS.shutdownPublisher(odomPub)
    end
end
-- See the user manual or the available code snippets for additional callback functions and details
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

  首先获取 odom 和 base_link 的句柄

odomHandle = sim.getObjectHandle("Odom")
baselinkHandle = sim.getObjectHandle("Base_link")
1
2

  然后以 ros nav_msgs/Odometry 的消息格式发布 /odom topic

-- sysCall_init
odomPub = simROS.advertise('/odom', 'nav_msgs/Odometry')
simROS.publisherTreatUInt8ArrayAsString(odomPub)

-- sysSensing
-- Get the robot position, orientation and velocities
local pos = sim.getObjectPosition(baselinkHandle,odomHandle)
local quat = sim.getObjectQuaternion(baselinkHandle,odomHandle)
local velocity_linear, velocity_angular = sim.getObjectVelocity(baselinkHandle)
if simROS then
    local odom_data = {}
    odom_data['header'] = {seq = 0, stamp = simROS.getTime(), frame_id = "odom"}
    odom_data['child_frame_id'] = "base_link"
    odom_data['pose'] = {}
    odom_data['pose']['pose'] = {}
    odom_data['pose']['pose']['position'] = {x = pos[1], y = pos[2], z = pos[3]}
    odom_data['pose']['pose']['orientation'] = {x = quat[1], y = quat[2], z = quat[3], w = quat[4]}
    odom_data['pose']['covariance'] = {0}
    odom_data['twist'] = {}
    odom_data['twist']['twist'] = {}
    odom_data['twist']['twist']['linear'] = {x = velocity_linear[1], y = velocity_linear[2], z = velocity_linear[3]}
    odom_data['twist']['twist']['angular'] = {x = velocity_angular[1], y = velocity_angular[2], z = velocity_angular[3]}
    odom_data['twist']['covariance'] = {0}
    simROS.publish(odomPub, odom_data)
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

2.2 TF 发布

  TF 的发布直接调用成熟的函数 getTransformStamped 即可。

function getTransformStamped(objHandle,name,relTo,relToName)
    t = sim.getSystemTime()
    p = sim.getObjectPosition(objHandle,relTo)
    o = sim.getObjectQuaternion(objHandle,relTo)
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end

-- sysSensing
simROS.sendTransform(getTransformStamped(baselinkHandle,'base_link',odomHandle,'odom'))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

  最终发布出来的效果可以通过 rosrun tf tf_echo base_link odom 查看。

2.3 激光雷达仿真

  据我所知，经典的激光雷达仿真方案有两种：

• 博主测试过软件提供的2D laser scanner.ttm，其是使用近距离传感器仿真的激光雷达，在场景中使用会比较卡，频率较低，，貌似还有其他问题；（非常不建议使用） Hokuyo_URG 也是通过近距离传感器仿真的结果，用过一段时间，发布出来的数据频率较低，可能在几Hz的样子，其他问题貌似没发现；（server 版本系统的时候能忍着用一用，反正有比没有好）
• SICK_TiM310_fast方案。此方案使用两个视觉传感器组合仿真一个270度的单线激光雷达，频率上比近距离的好很多，能到到15Hz+，肯定够用了。在此主要搭建此种方案。

2.4 IMU 仿真

至此，基础的 SLAM 算法都可以跑着玩啦， 哈哈 抓紧时间浪去吧～