RosInterface stops publishing after restart of simulation

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

RosInterface stops publishing after restart of simulation

Post by Gpaolo »

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: 10336
Joined: 14 Dec 2012, 00:25

Re: RosInterface stops publishing after restart of simulatio

Post by coppelia »

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

Post by Gpaolo »

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

Post by Elmo »

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

Re: RosInterface stops publishing after restart of simulation

Post by Billie1123 »

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

Re: RosInterface stops publishing after restart of simulation

Post by Billie1123 »

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: 10336
Joined: 14 Dec 2012, 00:25

Re: RosInterface stops publishing after restart of simulation

Post by coppelia »

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

Re: RosInterface stops publishing after restart of simulation

Post by Billie1123 »

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.

SimonBirrell
Posts: 8
Joined: 05 Apr 2019, 10:21

Re: RosInterface stops publishing after restart of simulation

Post by SimonBirrell »

I can confirm this behaviour is still present in October 2019. It's true that a brand new ROS subscriber outside V-REP will get the messages instantly, whereas existing ROS subscribers may receive nothing for 30 seconds or more. Shutting down all internal subscribers and publishers from the Lua script doesn't seem to help.

My guess is that this is something to do with TCP/IP connections. Perhaps the old subscribers timeout on the previous connection and then reconnect.

SimonBirrell
Posts: 8
Joined: 05 Apr 2019, 10:21

Re: RosInterface stops publishing after restart of simulation

Post by SimonBirrell »

Just to clarify my hypothesis:

1) An external ROS node (ie. outside V-REP) asks ROSCORE for the IP address and port of a V-REP ROS node. It gets an answer and connects, to, say, port 10000 on V-REP.

2) The simulation stops. The external nodes are still connected to 10000.

3) The simulation restarts. Instead of reusing the old connections, V-REP tells ROSCORE it is now on port 10001. Newly-created external nodes connect to 10001 and work just fine. Existing nodes are still listening on 10000 and receive nothing.

4) The TCP/IP connections to 10000 timeout and the external nodes get the new address of 10001 and reconnect.

It's just a guess, based on years of bitter experience.

Workarounds are tricky. The simplest one is to restart all your ROS nodes after restarting the simulation, which is a pain. Ideally, V-REP would reuse old connections where possible.

Post Reply