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

1. 控制篇

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

function sysCall_init()-- do some initialization here
endfunction sysCall_actuation()-- put your actuation code here
endfunction sysCall_sensing()-- put your sensing code here
endfunction 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 hereleftMotor  = sim.getObjectHandle("robot_leftMotor")  -- Handle of the left motorrightMotor = sim.getObjectHandle("robot_rightMotor") -- Handle of the right motor-- Launch the ROS client application:if simROS thensim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")cmdVelSub  = simROS.subscribe('/cmd_vel','geometry_msgs/Twist','cmd_vel_callback')elsesim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")end
endfunction sysCall_actuation()-- put your actuation code here
endfunction sysCall_sensing()-- put your sensing code here
endfunction cmd_vel_callback(msg)-- This is the velocity callback functionlocal wheel_radius = 0.17/2.0         -- wheel radius(drive wheel)local wheel_tread  = 0.19*2           -- the distance between left and right wheelvLeft  = (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)
endfunction sysCall_cleanup()-- do some clean-up hereif simROS thensimROS.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 messagesim.addStatusbarMessage('Starting robot simulation')-- Get the body handler, needed to get the absolute position and velocity of the robot in the sceneodomHandle = sim.getObjectHandle("Odom")baselinkHandle = sim.getObjectHandle("Base_link")leftMotor  = sim.getObjectHandle("robot_leftMotor")  -- Handle of the left motorrightMotor = sim.getObjectHandle("robot_rightMotor") -- Handle of the right motor-- Launch the ROS client application:if simROS thensim.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)elsesim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")end
endfunction sysCall_actuation()-- put your actuation code here
endfunction sysCall_sensing()-- put your sensing code here-- Get the robot position, orientation and velocitieslocal pos = sim.getObjectPosition(baselinkHandle,odomHandle)local quat = sim.getObjectQuaternion(baselinkHandle,odomHandle)local velocity_linear, velocity_angular = sim.getObjectVelocity(baselinkHandle)if simROS thenlocal 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
endfunction cmd_vel_callback(msg)-- This is the sub_velocity callback functionlocal wheel_radius = 0.17/2.0         -- wheel radius(drive wheel)local wheel_tread  = 0.19*2           -- the distance between left and right wheelvLeft  = (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)
endfunction sysCall_cleanup()-- do some clean-up hereif simROS thensimROS.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 的句柄

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 thenlocal 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 算法都可以跑着玩啦， 哈哈 抓紧时间浪去吧～