文章目录

1. 控制篇
2. 传感器篇
- 2.1 里程计仿真
- 2.2 TF 发布
- 2.3 激光雷达仿真
- 2.4 IMU 仿真

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

CoppeliaSim学习笔记之差速小车的控制与传感器的驱动相关推荐

“物联网开发实战”学习笔记-（二）手机控制智能电灯
"物联网开发实战"学习笔记-(二)手机控制智能电灯如果搭建好硬件平台后,这一次我们的任务主要是调试好智能电灯,并且连接到腾讯云的物联网平台. 腾讯云物联网平台腾讯物联网平台的优 ...
STM32 学习笔记2-智能小车循迹实验
特斯拉镇楼 1.什么是小车循迹? 将小车放在黑色跑道上面,小车沿着黑色跑道运动 → 循迹黑色跑道 2.小车循迹基本原理原理: 介绍原理之前,突然记起来,在电子爱好者上做过一个循迹小车的项目, ...
操作系统学习笔记-2.1.3进程控制
操作系统学习笔记-2019 王道考研操作系统-2.1.3进程控制文章目录 3.进程控制 3.1知识概览 3.2 基本概念 3.2.1什么是进程控制? 3.2.2如何实现进程控制? 3.3进程控制相 ...
阿里云HaaS100物联网开发板学习笔记（二）硬件控制初步--让小灯闪烁起来
摘要:无论是哪种开发板,要想开发特定的功能,必先从GPIO开始,HaaS100开发也是一样.如果仅仅利用HaaS100的联网功能,那简直是太浪费了.HaaS100拥有其他开发板所具备的所有的功能,比如 ...
STM32 学习笔记1-智能小车-基于PWM 调速的电机设置
目录本文章主要介绍 STM32 电机相关软件的配置,PWM的相关介绍,csdn上面有很多资料硬件: PWM(Pulse Width Modulation) 介绍: 基本定时器(TIM6/7) 通用 ...
【嵌入式环境下linux内核及驱动学习笔记-（16）linux总线、设备、驱动模型之input框架】
目录 1.Linux内核输入子系统概念导入 1.1 输入设备工作机制 1.2 运行框架 1.3 分层思想 2.驱动开发步骤 2.1 在init()或probe()函数中 2.2 在exit()或rem ...
CoppeliaSim学习笔记之仿真环境与小车模型的搭建
UE4学习笔记[2]Game-Controlled Cameras/游戏控制的摄像机
第一步:在场景中放置摄像机如果你是虚幻引擎4 (UE4)Unreal Engine 4的新手,你可需要先阅读我们的编程快速入门教程.对于本教程,我们假设你熟悉以下操作:创建项目,向项目添加C++代 ...
CC2540开发板学习笔记（六）——AD控制（自带温度计）
一.实验目的将采集的内部温度传感器信息通过串口发送到上位机二.实验过程 1.寄存器配置 ADCCON1(0XB4) ADC控制寄存器1 BIT7:EOC ADC结束标志位 0:AD转换进行中 ...

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