RosInterface stops publishing after restart of simulation

Report crashes, strange behaviour, or apparent bugs
Gpaolo
Posts: 2
Joined: 09 Nov 2016, 10:09

RosInterface stops publishing after restart of simulation

Postby Gpaolo » 10 Nov 2016, 06:55

Hi all!
I am working on my first project with V-Rep and I am trying to control a robot via ROS+Python.
I am using RosInterface to comunicate with ROS and RosInterfaceHelper to start, pause and stop the simulation from Ros.
I have a problem though, when I stop and restart the sim, after the restart vrep stops publishing to the topics I have created in my script for a while. I tried to figure out why this happens but have not found a solution.
Does anybody have any idea about this?
Thanks for the help!
Here is my Lua script in vrep.

Code: Select all

-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)

-- The controller will send already the speed setted up in the right way (different for each wheel dimension, and zero when in normal mode)
function setMotorSpeed(msg)
    print('Message received')
    for i = 1,16,1 do
        simSetJointTargetVelocity(RightMotors[i],msg.data[i])
        simSetJointTargetVelocity(LeftMotors[i],msg.data[i])
    end
end

-- Manual Control
function setRightSpeed(msg)
    for i = 1,16,1 do
        simSetJointTargetVelocity(RightMotors[i],msg.data[i])
    end
end
-- Manual Control
function setLeftSpeed(msg)
    for i = 1,16,1 do
        simSetJointTargetVelocity(LeftMotors[i],msg.data[i])
    end
end

function setFL_flipper(msg)
    simSetJointTargetPosition(FL_flipper,msg.data)
end

function setFR_flipper(msg)
    simSetJointTargetPosition(FR_flipper,msg.data)
end

function setBL_flipper(msg)
    simSetJointTargetPosition(BL_flipper,msg.data)
end

function setBR_flipper(msg)
    simSetJointTargetPosition(BR_flipper,msg.data)
end

if (sim_call_type==sim_childscriptcall_initialization) then
    robotHandle=simGetObjectAssociatedWithScript(sim_handle_self)
    LeftMotors = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
    RightMotors = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
    -- Left Motors Handles
    LeftMotors[1] = simGetObjectHandle('FL_J3')
    LeftMotors[2] = simGetObjectHandle('FL_J2')
    LeftMotors[3] = simGetObjectHandle('FL_J1')
    LeftMotors[4] = simGetObjectHandle('FL_J0')
    LeftMotors[5] = simGetObjectHandle('ML_J0')
    LeftMotors[6] = simGetObjectHandle('ML_J1')
    LeftMotors[7] = simGetObjectHandle('ML_J2')
    LeftMotors[8] = simGetObjectHandle('ML_J3')
    LeftMotors[9] = simGetObjectHandle('ML_J4')
    LeftMotors[10] = simGetObjectHandle('ML_J5')
    LeftMotors[11] = simGetObjectHandle('ML_J6')
    LeftMotors[12] = simGetObjectHandle('ML_J7')
    LeftMotors[13] = simGetObjectHandle('BL_J0')
    LeftMotors[14] = simGetObjectHandle('BL_J1')
    LeftMotors[15] = simGetObjectHandle('BL_J2')
    LeftMotors[16] = simGetObjectHandle('BL_J3')
    -- Right Motors Handles
    RightMotors[1] = simGetObjectHandle('FR_J3')
    RightMotors[2] = simGetObjectHandle('FR_J2')
    RightMotors[3] = simGetObjectHandle('FR_J1')
    RightMotors[4] = simGetObjectHandle('FR_J0')
    RightMotors[5] = simGetObjectHandle('MR_J0')
    RightMotors[6] = simGetObjectHandle('MR_J1')
    RightMotors[7] = simGetObjectHandle('MR_J2')
    RightMotors[8] = simGetObjectHandle('MR_J3')
    RightMotors[9] = simGetObjectHandle('MR_J4')
    RightMotors[10] = simGetObjectHandle('MR_J5')
    RightMotors[11] = simGetObjectHandle('MR_J6')
    RightMotors[12] = simGetObjectHandle('MR_J7')
    RightMotors[13] = simGetObjectHandle('BR_J0')
    RightMotors[14] = simGetObjectHandle('BR_J1')
    RightMotors[15] = simGetObjectHandle('BR_J2')
    RightMotors[16] = simGetObjectHandle('BR_J3')
    -- Flipper Handles
    FL_flipper = simGetObjectHandle('FL_MainJoint')
    BL_flipper = simGetObjectHandle('BL_MainJoint')
    FR_flipper = simGetObjectHandle('FR_MainJoint')
    BR_flipper = simGetObjectHandle('BR_MainJoint')
    -- Cameras Handlers
    frontCamera = simGetObjectHandle('Front_Camera')
    backCamera = simGetObjectHandle('Back_Camera')
    -- Distance Handle
    trav_distance = simGetDistanceHandle('TraveledDistance')
    -- Position Objects Handle
    stop = simGetObjectHandle('Stop')
    robot = simGetObjectHandle('Main_Body')
    -- Topic set-up.
    FL_flipper_Sub=simExtRosInterface_subscribe('/FL_flipper','std_msgs/Float32','setFL_flipper')
    BL_flipper_Sub=simExtRosInterface_subscribe('/BL_flipper','std_msgs/Float32','setBL_flipper')
    FR_flipper_Sub=simExtRosInterface_subscribe('/FR_flipper','std_msgs/Float32','setFR_flipper')
    BR_flipper_Sub=simExtRosInterface_subscribe('/BR_flipper','std_msgs/Float32','setBR_flipper')
    speedSub=simExtRosInterface_subscribe('/motorSpeed','std_msgs/Float32MultiArray','setMotorSpeed')
    frontCameraPub=simExtRosInterface_advertise('/front_camera', 'sensor_msgs/Image')
    simExtRosInterface_publisherTreatUInt8ArrayAsString(frontCameraPub)
    backCameraPub=simExtRosInterface_advertise('/back_camera', 'sensor_msgs/Image')
    simExtRosInterface_publisherTreatUInt8ArrayAsString(backCameraPub)
    distancePub = simExtRosInterface_advertise('/distance', 'std_msgs/Float32')
    -- Manual Control
    RightSpeedSub=simExtRosInterface_subscribe('/right_speed','std_msgs/Float32MultiArray','setRightSpeed')
    LeftSpeedSub=simExtRosInterface_subscribe('/left_speed','std_msgs/Float32MultiArray','setLeftSpeed')
    -- Gyro
    gyroCommunicationTube=simTubeOpen(0,'gyroData'..simGetNameSuffix(nil),1)
    gyroPub = simExtRosInterface_advertise('/gyro', 'geometry_msgs/Vector3')
    -- Acc
    accelCommunicationTube=simTubeOpen(0,'accelerometerData'..simGetNameSuffix(nil),1)
    accPub = simExtRosInterface_advertise('/accelerometer', 'geometry_msgs/Vector3')
    -- Terminal
    terminalPub = simExtRosInterface_advertise('/terminal', 'std_msgs/Bool')
end


if (sim_call_type==sim_childscriptcall_actuation) then



end


if (sim_call_type==sim_childscriptcall_sensing) then

    -- Publish the image of the cameras:
        -- Front camera
    local data,w,h=simGetVisionSensorCharImage(frontCamera)
    front={}
    front['header']={seq=0,stamp=0, frame_id="a"}
    front['height']=h
    front['width']=w
    front['encoding']='rgb8'
    front['is_bigendian']=1
    front['step']=h*w*3
    front['data']=data
    simExtRosInterface_publish(frontCameraPub,front)
        -- Back camera
    local data,w,h=simGetVisionSensorCharImage(backCamera)
    back={}
    back['header']={seq=0,stamp=0, frame_id="a"}
    back['height']=h
    back['width']=w
    back['encoding']='rgb8'
    back['is_bigendian']=1
    back['step']=h*w*3
    back['data']=data
    simExtRosInterface_publish(backCameraPub,back)

    -- Gyro
    gyroData=simTubeRead(gyroCommunicationTube)
    if (gyroData) then
        ang=simUnpackFloats(gyroData)
        gyroMessage = {}
        gyroMessage['x'] = ang[1]
        gyroMessage['y'] = ang[2]
        gyroMessage['z'] = ang[3]
        simExtRosInterface_publish(gyroPub, gyroMessage)
    end

    -- Acc
    accData=simTubeRead(accelCommunicationTube)
    if (data) then
        accel=simUnpackFloats(accData)
        accMessage = {}
        accMessage['x'] = accel[1]
        accMessage['y'] = accel[2]
        accMessage['z'] = accel[3]
        simExtRosInterface_publish(accPub, accMessage)
    end

    -- Distance
    simHandleDistance(trav_distance)
    dist = {}
    res, dist['data'] = simReadDistance(trav_distance)
    simExtRosInterface_publish(distancePub, dist)

    -- Position
    pos = simGetObjectPosition(robot, stop)
    term = {}
    if (pos[1] < 0.1 and pos[1] > -0.1) then
        term['data'] = true
    else
        term['data'] = false
    end
    simExtRosInterface_publish(terminalPub, term)
end


if (sim_call_type==sim_childscriptcall_cleanup) then

   -- Put some restoration code here

end

coppelia
Site Admin
Posts: 5906
Joined: 14 Dec 2012, 00:25

Re: RosInterface stops publishing after restart of simulatio

Postby coppelia » 11 Nov 2016, 08:45

Hello,

have you tried to explicitely shut down all the publishers/subscribers you have created during simulation? You can do this in the clean-up phase with simExtRosInterface_shutdownPublisher and simExtRosInterface_shutdownSubscriber.

Cheers

Gpaolo
Posts: 2
Joined: 09 Nov 2016, 10:09

Re: RosInterface stops publishing after restart of simulatio

Postby Gpaolo » 11 Nov 2016, 11:06

Yes I did that, but it seems that I get no changes at all

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: RosInterface stops publishing after restart of simulatio

Postby Elmo » 12 Nov 2016, 23:52

when you stop the simulation.

try rostopic list in the terminal. Are the topics still up?
the code looks good to me.

little example code perhaps it will help you

Code: Select all

function waterTopic_cb(msg)
    waterStatus = msg.data
    -- do sth.
end

if (sim_call_type==sim_childscriptcall_initialization) then
    local plant = simGetObjectHandle('indoorPlant')
   
    -- Publisher
    waterPub_topic=simExtRosInterface_advertise ('/PlantWaterStatus',
    'std_msgs/Float32')

    -- Subsriber
    local PersonBill = simGetObjectHandle('Bill')
   WaterSub_top = simExtRosInterface_subscribe ('/PlantWaterStatus',
   'std_msgs/Float32','waterTopic_cb')
       
end

if (sim_call_type==sim_childscriptcall_actuation) then
   -- send water status 50%
    simExtRosInterface_publish(waterPub_topic,{data=50.0})
end

if (sim_call_type==sim_childscriptcall_cleanup) then
   -- Put some restoration code here
    -- shutdown topics
     simExtRosInterface_shutdownPublisher(waterPub_topic)
    simExtRosInterface_shutdownSubscriber(WaterSub_top)
    --shutdown services
    simExtRosInterface_shutdownServiceServer(cmd_velServer)
end

Billie1123
Posts: 57
Joined: 08 Jun 2016, 22:47

Re: RosInterface stops publishing after restart of simulation

Postby Billie1123 » 13 Mar 2017, 12:00

I am having exactly the same problem.
V-REP seems to stop publishing the topics, but it actually takes a while to publish again if the subscribers are left open.

I have this code to show what happens:

Code: Select all

if (sim_call_type==sim_childscriptcall_initialization) then
    _pubClock=simExtRosInterface_advertise('/clock','rosgraph_msgs/Clock')
    _pubTime=simExtRosInterface_advertise('/time', 'std_msgs/Float64')
end

if (sim_call_type==sim_childscriptcall_sensing) then
    simExtRosInterface_publish(_pubClock,{clock=simGetSimulationTime()})
    simExtRosInterface_publish(_pubTime, {data=simGetSimulationTime()})
    print(simGetSimulationTime())
end

if (sim_call_type==sim_childscriptcall_cleanup) then
   simExtRosInterface_shutdownPublisher (_pubTime)
    simExtRosInterface_shutdownPublisher (_pubClock)
end


Put the code above in a non-threaded child script in a new empty scene.

In two consoles run "rostopic echo /clock" and "rostopic echo /time", respectively.

If you run the simulation, it will work just fine. Stop it and start it again and it will take some time to start publishing the messages (sometimes I have waited for 10 seconds or more, that's why V-REP gives the impression of being halted).
This observation made by fferri seems reasonable, because ROS does need some time to establish all connections since a publisher is created, so it would be normal if the first messages drop.

But what I cannot explain is why when having stopped the simulation you halt the subscribers (Ctrl+C in each console) and start them again and re-start the simulation, the first messages arrive flawlessly. This is what makes me think it has nothing to do with the time to establish each connection. Furthermore, sometimes the time it takes to start publishing the messages again is more than enough to make all connections.

Another observation I have made is that I have a custom plugin from which I publish data (without using the RosInterface plugin) and it does not show these problems. This makes me think that the problem must reside in the way the RosInterface plugin works.

Any ideas? Can you reproduce the behaviour described with the code I wrote? I am using Ubuntu 14.04 64 bits, ROS Indigo, and the latest version of V-REP.

Regards

edit: a more compelling test that shows a bad behaviour: if you only restart one of the subscribers, the other remains halted.

Billie1123
Posts: 57
Joined: 08 Jun 2016, 22:47

Re: RosInterface stops publishing after restart of simulation

Postby Billie1123 » 13 Mar 2017, 14:08

Ok, It gets weirder.
I have made a simple c++ node that subscribes to /time and prints it. If I run at the same time this node I made and a console with rostopic echo /time, the node works as expected (it starts publishing regularly after 0.3 seconds, the time it takes to the ros communication system establish the connections) but the rostopic console still has a much bigger delay. That is definitely a ROS issue.

But still, If I publish data from a plugin myself, I do not have the problem I am experiencing with the RosInterface plugin. That is what makes me think there might be a problem there as well.

coppelia
Site Admin
Posts: 5906
Joined: 14 Dec 2012, 00:25

Re: RosInterface stops publishing after restart of simulation

Postby coppelia » 15 Mar 2017, 09:15

Hello,

I have tried and it behaves exactly how you described it. But what makes me think that it is not related to V-REP/RosInterface is following scenario:

Each time you have stopped a simulation, open 2 additional terminal windows and echo the time and the clock there too: those two newly opened terminals will immediately pick up the messages at simulation start, while the older terminals will need time.

Cheers

Billie1123
Posts: 57
Joined: 08 Jun 2016, 22:47

Re: RosInterface stops publishing after restart of simulation

Postby Billie1123 » 15 Mar 2017, 11:00

Hello,

exactly, that's why there must be something wrong with ROS.

But also, I have observed that with a custom plugin I made that directly publishes to ROS, this delay does not occur. I'll put a pin in this and take a look at the code of both plugins later on to see if I spot any difference.


Return to “Bug reports”

Who is online

Users browsing this forum: No registered users and 4 guests