基于ros和coppliasim驱动小车避障运动

环境说明

ubuntu系统：ubuntu18.04
coppliasim：V4.2.0
场景文件：coppliasim自带的controlleViaRos.ttt

操作流程

一、修改controlleViaRos.ttt中对应的lua脚本文件

修改前：

local sysTime=sim.getSystemTimeInMs(-1) 
local leftMotorTopicName='leftMotorSpeed'..sysTime -- we add a random component so that we can have several instances of this robot running
local rightMotorTopicName='rightMotorSpeed'..sysTime -- we add a random component so that we can have several instances of this robot running
local sensorTopicName='sensorTrigger'..sysTime -- we add a random component so that we can have several instances of this robot running
local simulationTimeTopicName='simTime'..sysTime -- we add a random component so that we can have several instances of this robot running
-- Prepare the sensor publisher and the motor speed subscribers:
sensorPub=simROS.advertise('/'..sensorTopicName,'std_msgs/Bool')
simTimePub=simROS.advertise('/'..simulationTimeTopicName,'std_msgs/Float32')
leftMotorSub=simROS.subscribe('/'..leftMotorTopicName,'std_msgs/Float32','setLeftMotorVelocity_cb')
rightMotorSub=simROS.subscribe('/'..rightMotorTopicName,'std_msgs/Float32','setRightMotorVelocity_cb')
-- Now we start the client application:
result=sim.launchExecutable('rosBubbleRob',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

修改后：

local sysTime=sim.getSystemTimeInMs(-1) 
local leftMotorTopicName='leftMotorSpeed'  -- we add a random component so that we can have several instances of this robot running
local rightMotorTopicName='rightMotorSpeed'  -- we add a random component so that we can have several instances of this robot running
local sensorTopicName='sensorTrigger' -- we add a random component so that we can have several instances of this robot running
local simulationTimeTopicName='simTime' -- we add a random component so that we can have several instances of this robot running
-- Prepare the sensor publisher and the motor speed subscribers:
sensorPub=simROS.advertise('/'..sensorTopicName,'std_msgs/Bool')
simTimePub=simROS.advertise('/'..simulationTimeTopicName,'std_msgs/Float32')
leftMotorSub=simROS.subscribe('/'..leftMotorTopicName,'std_msgs/Float32','setLeftMotorVelocity_cb')
rightMotorSub=simROS.subscribe('/'..rightMotorTopicName,'std_msgs/Float32','setRightMotorVelocity_cb')
-- Now we start the client application:
--result=sim.launchExecutable('rosBubbleRob',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

主要修改其中的话题名，删除其中的随机量。注释21行，以免sim.launchExecutable启动rosBubbleRob可执行文件。

二、创建ROS文件

创建 ROS 工作空间

mkdir -p learning_ws/src

进入刚创建的工作空间,编译工作空间

cd ~/learning_ws
catkin_make
code .

先使用快捷键 ctrl + shift + B 调用编译选项配置
选择: catkin_make:build
可以点击配置设置为默认，修改 .vscode/tasks.json 文件
以下为vscode添加的默认配置。

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

创建 ROS 功能包
选定 src 右击 —> create catkin package
设置包名和依赖：control和roscpp rospy std_msgs geometry_msgs message_generation

在src文件下新建control.cpp

#include  <sstream>
#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Bool.h"

bool sensorTrigger=false; //初始化传感器值
void sensorCallback(const std_msgs::Bool& sensTrigger)//接受传感器消息，并在终端打印
{
    sensorTrigger=sensTrigger.data;
    ROS_INFO("sensorTrigger: %d",sensorTrigger);
}
int main(int argc,char **argv)
{
    ros::init(argc,argv,"control");//初始化部分
    ros::NodeHandle n;//创建节点句柄
    ros::Publisher leftMotorSpeed_pub=n.advertise<std_msgs::Float32>("/leftMotorSpeed",1000);//注册左轮话题发布
    ros::Publisher rightMotorSpeed_pub=n.advertise<std_msgs::Float32>("/rightMotorSpeed",1000);//注册右轮话题发布
    ros::Subscriber sensorTrigger_sub=n.subscribe("/sensorTrigger",1000,sensorCallback);//订阅传感器话题
    ros::Rate loop_rate(10);//循环频率
    float    desiredLeftMotorSpeed=7.0;//初始化速度
    float     desiredRightMotorSpeed=7.0;
    std_msgs::Float32 leftMotorSpeed;
    std_msgs::Float32 rightMotorSpeed;
    while (ros::ok())
    {
         if (sensorTrigger)
        { // 有障碍物时转弯
            desiredLeftMotorSpeed=-7.0*0.5;
            desiredRightMotorSpeed=-7.0*0.25;
            leftMotorSpeed.data=desiredLeftMotorSpeed;
            rightMotorSpeed.data=desiredRightMotorSpeed;
            leftMotorSpeed_pub.publish(leftMotorSpeed);
            rightMotorSpeed_pub.publish(rightMotorSpeed);//发布左右轮话题
            ros::Duration(3).sleep();//延时器3s，以免转弯小
        }
        else
        { // 直行
            desiredLeftMotorSpeed=7.0;
            desiredRightMotorSpeed=7.0;
            leftMotorSpeed.data=desiredLeftMotorSpeed;
            rightMotorSpeed.data=desiredRightMotorSpeed;
            leftMotorSpeed_pub.publish(leftMotorSpeed);
            rightMotorSpeed_pub.publish(rightMotorSpeed);//发布左右轮话题
        }
        ROS_INFO("leftMotorSpeed:%f,rightMotorSpeed:%f,",leftMotorSpeed.data,rightMotorSpeed.data);//打印左右轮速度
        ros::spinOnce();
        loop_rate.sleep();//休眠10ms

        //++speed;
    }
    return 0;
}

创建launch和对应的control.launch

<launch>
    <node pkg="control" type="control" name="control" />
</launch>

在CMakeLists中添加如下语句：

add_executable(control src/control.cpp)
target_link_libraries(control ${catkin_LIBRARIES})
add_dependencies(control  ${PROJECT_NAME}_generate_messages_cpp)

运行小车避障运动

编译并导入环境变量：

cd ~/learning_ws
catkin_make
source ./devel/setup.bash

运行launch文件

roslaunch control control.launch 

打开coppeliasim与对应的controlleViaRos.ttt并运行，可看到小车运动并具备避障功能。