Coppeliasim中的ROS节点分析

Coppeliasim中的ROS节点分析

这一节的学习，我们分析官方的案例：controlTypeExamples/controlledViaRos.ttt，主要带大家理顺操作流程。

Coppeliasim中的脚本配置

首先看到我们的场景中有一系列的物体，以及两个脚本，其中最上面橙色的是主脚本，一般不建议打开，因为一旦打开会变成自定义脚本，后期场景如果变更就无法自动更新。

扩展：coppeliasim中脚本的简单辨识

• 主脚本：最上面，橙色

• 线程子脚本：蓝色

• 非线程子脚本：白色

• 自定义脚本：灰色

这里我把主脚本的内容贴出来满足一下我和大家的好奇心，但我们一般不对这里进行改动。

-- This is the main script. The main script is not supposed to be modified,
-- unless there is a very good reason to do it.
-- Without main script, there is no simulation.
-- A main script marked as "default" (this is the default case) will use the
-- content of following file: system/dltmscpt.txt. This allows your old simulation
-- scenes to be automatically also using newer features, without explicitely coding
-- them. If you modify the main script, it will be marked as "customized", and you
-- won't benefit of that automatic forward compatibility mechanism. 

-- 1、仿真的初始化部分
function sysCall_init() 
    sim.handleSimulationStart()
    sim.openModule(sim.handle_all)
    sim.handleGraph(sim.handle_all_except_explicit,0)
end

-- 2、执行部分（主要执行控制算法和模型运动）
function sysCall_actuation() 
    sim.resumeThreads(sim.scriptthreadresume_default)
    sim.resumeThreads(sim.scriptthreadresume_actuation_first)
    sim.launchThreadedChildScripts()
    sim.handleChildScripts(sim.syscb_actuation)
    sim.resumeThreads(sim.scriptthreadresume_actuation_last)
    sim.handleCustomizationScripts(sim.syscb_actuation)
    sim.handleAddOnScripts(sim.syscb_actuation)
    sim.handleSandboxScript(sim.syscb_actuation)
    sim.handleModule(sim.handle_all,false)
    sim.handleIkGroup(sim.handle_all_except_explicit)
    sim.handleDynamics(sim.getSimulationTimeStep())
end

-- 3、传感器部分（主要处理传感器数据）
function sysCall_sensing()
    -- put your sensing code here
    sim.handleSensingStart()
    sim.handleCollision(sim.handle_all_except_explicit)
    sim.handleDistance(sim.handle_all_except_explicit)
    sim.handleProximitySensor(sim.handle_all_except_explicit)
    sim.handleVisionSensor(sim.handle_all_except_explicit)
    sim.resumeThreads(sim.scriptthreadresume_sensing_first)
    sim.handleChildScripts(sim.syscb_sensing)
    sim.resumeThreads(sim.scriptthreadresume_sensing_last)
    sim.handleCustomizationScripts(sim.syscb_sensing)
    sim.handleAddOnScripts(sim.syscb_sensing)
    sim.handleSandboxScript(sim.syscb_sensing)
    sim.handleModule(sim.handle_all,true)
    sim.resumeThreads(sim.scriptthreadresume_allnotyetresumed)
    sim.handleGraph(sim.handle_all_except_explicit,sim.getSimulationTime()+sim.getSimulationTimeStep())
end

-- 清理部分，结束仿真时调用
function sysCall_cleanup()
    sim.resetCollision(sim.handle_all_except_explicit)
    sim.resetDistance(sim.handle_all_except_explicit)
    sim.resetProximitySensor(sim.handle_all_except_explicit)
    sim.resetVisionSensor(sim.handle_all_except_explicit)
    sim.closeModule(sim.handle_all)
end

-- 暂停仿真时调用
function sysCall_suspend()
    sim.handleChildScripts(sim.syscb_suspend)
    sim.handleCustomizationScripts(sim.syscb_suspend)
    sim.handleAddOnScripts(sim.syscb_suspend)
    sim.handleSandboxScript(sim.syscb_suspend)
end

-- 暂停后调用
function sysCall_suspended()
    sim.handleChildScripts(sim.syscb_suspended)
    sim.handleCustomizationScripts(sim.syscb_suspended)
    sim.handleAddOnScripts(sim.syscb_suspended)
    sim.handleSandboxScript(sim.syscb_suspended)
end

-- 恢复仿真时调用
function sysCall_resume()
    sim.handleChildScripts(sim.syscb_resume)
    sim.handleCustomizationScripts(sim.syscb_resume)
    sim.handleAddOnScripts(sim.syscb_resume)
    sim.handleSandboxScript(sim.syscb_resume)
end

以上这些函数基本都可以在官方手册的Regular API中找到，感兴趣可以自行去了解作用，例如：

我们在coppeliasim中主要的编程工作集中在子脚本中，我们打开场景中的子脚本

这是一个非线程子脚本

function sysCall_init()
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
    leftMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobLeftMotor")
    rightMotor=sim.getObjectHandle("rosInterfaceControlledBubbleRobRightMotor")
    noseSensor=sim.getObjectHandle("rosInterfaceControlledBubbleRobSensingNose")
    -- Launch the ROS client application:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        local sysTime=sim.getSystemTimeInMs(-1) 
        local leftMotorTopicName='leftMotorSpeed'..sysTime
        local rightMotorTopicName='rightMotorSpeed'..sysTime
        local sensorTopicName='sensorTrigger'..sysTime
        local simulationTimeTopicName='simTime'..sysTime
        -- 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')

        -- 这里“类似”于rosrun指令， rosBubbleRob是一个可执行文件
        result=sim.launchExecutable('rosBubbleRob',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end

end
function setLeftMotorVelocity_cb(msg)
    -- Left motor speed subscriber callback
    sim.setJointTargetVelocity(leftMotor,msg.data)
end

function setRightMotorVelocity_cb(msg)
    -- Right motor speed subscriber callback
    sim.setJointTargetVelocity(rightMotor,msg.data)
end


function getTransformStamped(objHandle,name,relTo,relToName)
    t=sim.getSystemTime()
    p=sim.getObjectPosition(objHandle,relTo)
    o=sim.getObjectQuaternion(objHandle,relTo)
    
    -- geometry_msgs/TransformStamped类型的ROS消息
    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


function sysCall_actuation()
    if simROS then
        local result=sim.readProximitySensor(noseSensor)
        local detectionTrigger={}
        detectionTrigger['data']=result>0
        simROS.publish(sensorPub,detectionTrigger)
        simROS.publish(simTimePub,{data=sim.getSimulationTime()})
        -- 上面这句使用这种表达更好理解
        -- local message = {}
        -- message.data = sim.getSimulationTime()
        -- simROS.publish(simTimePub, message)
        -- Send the robot's transform:
        simROS.sendTransform(getTransformStamped(robotHandle,'rosInterfaceControlledBubbleRob',-1,'world'))
        -- To send several transforms at once, use simROS.sendTransforms instead
    end
end

-- Following not really needed in a simulation script (i.e. automatically shut down at simulation end):
function sysCall_cleanup()
    if simROS then
        simROS.shutdownPublisher(sensorPub)
        simROS.shutdownSubscriber(leftMotorSub)
        simROS.shutdownSubscriber(rightMotorSub)
    end
end

这里不带大家逐行去读代码了，具体阅读的方法是搭配官方的文档进行分析

关键函数查找举例：

控制节点的编写

我们可以理解coppeliasim是一个平台，和真实的机器人类似，它接收我们的控制指令，然后运动。

我们上面的脚本可以理解为对已有平台进行配置，并留出了一些控制接口。

具体控制的指令我们要新写一个节点，如下：

注意上面提到的子脚本中的这句：

result=sim.launchExecutable('rosBubbleRob',leftMotorTopicName.." "..rightMotorTopicName.." "..sensorTopicName.." "..simulationTimeTopicName,0)

我们详细看看这个函数：

可见，rosBubbleRob是一个可执行文件，即用于控制的节点文件，我们找到它的位置，但cat后发现看不懂。

为了了解节点的控制逻辑及它的写法，我们在github上找到rosBubbleRob的源码，

为方便大家观看，这里直接把源码贴出来

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

// Global variables (also modified by the topic subscriber):
bool sensorTrigger=false;
struct timeval tv;
unsigned int currentTime_updatedByTopicSubscriber=0;
float simulationTime=0.0;

// Topic subscriber callbacks:
void sensorCallback(const std_msgs::Bool& sensTrigger)
{
    if (gettimeofday(&tv,NULL)==0)
        currentTime_updatedByTopicSubscriber=tv.tv_sec;
    sensorTrigger=sensTrigger.data;
}

void simulationTimeCallback(const std_msgs::Float32& simTime)
{
    simulationTime=simTime.data;
}


int main(int argc,char* argv[])
{
    // The robot motor velocities and the sensor topic names are given in the argument list
    // (when CoppeliaSim launches this executable, CoppeliaSim will also provide the argument list)
    std::string leftMotorTopic;
    std::string rightMotorTopic;
    std::string sensorTopic;
    std::string simulationTimeTopic;
    if (argc>=5)
    {
        leftMotorTopic=argv[1];
        rightMotorTopic=argv[2];
        sensorTopic=argv[3];
        simulationTimeTopic=argv[4];
        leftMotorTopic="/"+leftMotorTopic;
        rightMotorTopic="/"+rightMotorTopic;
        sensorTopic="/"+sensorTopic;
        simulationTimeTopic="/"+simulationTimeTopic;
    }
    else
    {
        printf("Indicate following arguments: 'leftMotorTopic rightMotorTopic sensorTopic simulationTimeTopic'!\n");
        sleep(5000);
        return 0;
    }

    // Create a ROS node. The name has a random component: 
    int _argc = 0;
    char** _argv = NULL;
    if (gettimeofday(&tv,NULL)==0)
        currentTime_updatedByTopicSubscriber=(tv.tv_sec*1000+tv.tv_usec/1000)&0x00ffffff;
    std::string nodeName("rosBubbleRob");
    std::string randId(boost::lexical_cast<std::string>(currentTime_updatedByTopicSubscriber+int(999999.0f*(rand()/(float)RAND_MAX))));
    /* 拼接得到完整的节点名称 */
    nodeName+=randId;    
    
    /* 初始化ROS节点 */
    ros::init(_argc,_argv,nodeName.c_str());

    if(!ros::master::check())
        return(0);
    
    ros::NodeHandle node("~");  
    printf("rosBubbleRob2 just started with node name %s\n",nodeName.c_str());

    // 1. Let's subscribe to the sensor and simulation time stream
    ros::Subscriber subSensor=node.subscribe(sensorTopic.c_str(),1,sensorCallback);
    ros::Subscriber subSimulationTime=node.subscribe(simulationTimeTopic.c_str(),1,simulationTimeCallback);

    // 2. Let's prepare publishers for the motor speeds:
    ros::Publisher leftMotorSpeedPub=node.advertise<std_msgs::Float32>(leftMotorTopic.c_str(),1);
    ros::Publisher rightMotorSpeedPub=node.advertise<std_msgs::Float32>(rightMotorTopic.c_str(),1);

    // 3. Finally we have the control loop:
    float driveBackStartTime=-99.0f; 
    unsigned int currentTime;
    if (gettimeofday(&tv,NULL)==0)
    {
        currentTime_updatedByTopicSubscriber=tv.tv_sec;
        currentTime=currentTime_updatedByTopicSubscriber;
    }
    
    while (ros::ok()) 
    { // this is the control loop (very simple, just as an example)
        if (gettimeofday(&tv,NULL)==0)
        {
            currentTime=tv.tv_sec;
            if (currentTime-currentTime_updatedByTopicSubscriber>9)
                break; // we didn't receive any sensor information for quite a while... we leave
        }
        float desiredLeftMotorSpeed;
        float desiredRightMotorSpeed;
        if (simulationTime-driveBackStartTime<3.0f)
        { // driving backwards while slightly turning:
            desiredLeftMotorSpeed=-7.0*0.5;
            desiredRightMotorSpeed=-7.0*0.25;
        }
        else
        { // going forward:
            desiredLeftMotorSpeed=7.0;
            desiredRightMotorSpeed=7.0;
            if (sensorTrigger)
                driveBackStartTime=simulationTime; // We detected something, and start the backward mode
            sensorTrigger=false;
        }

        // publish the motor speeds:
        std_msgs::Float32 d;
        d.data=desiredLeftMotorSpeed;
        leftMotorSpeedPub.publish(d);
        d.data=desiredRightMotorSpeed;
        rightMotorSpeedPub.publish(d);

        // handle ROS messages:
        ros::spinOnce();

        // sleep a bit:
        usleep(5000);
    }
    ros::shutdown();
    printf("rosBubbleRob just ended!\n");
    return(0);
}

详细的代码要大家自己分析，不过我们可以一起来梳理一下控制的逻辑，见下图：

• 当在coppeliasim中电机运行仿真时，会配合使用/sim_ros_interface这个节点

• 我们在子脚本中编程，启动了/rosBubbleRob（我们的数字后缀可能不一样，我这里暂略）这个节点

• rosBubbleRob节点订阅/sim_ros_interface节点中的时间以及传感器的触发标志

• rosBubbleRob节点根据订阅的内容发布速度控制指令给/sim_ros_interface节点

• /sim_ros_interface节点订阅并发布对应消息，并控制coppeliasim中的仿真

• 现象解释如下

  3秒是指coppeliasim中的3秒，不一定是真实世界中的3秒

• 

---

小结

至此，我们完成了对这个例程的分析，理解之后我们可以通过模仿它的写法写出自己的小demo。

后面的文章计划写一个简单的机械臂轨迹规划。