上一章节 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
其中，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
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
首先获取 odom 和 base_link 的句柄
扫描二维码关注公众号，回复：
16963052 查看本文章
odomHandle = sim.getObjectHandle("Odom")
baselinkHandle = sim.getObjectHandle("Base_link")
然后以 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
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'))
最终发布出来的效果可以通过 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 算法都可以跑着玩啦， 哈哈 抓紧时间浪去吧～
