## 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

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.
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)
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')
simExtRosInterface_publisherTreatUInt8ArrayAsString(frontCameraPub)
simExtRosInterface_publisherTreatUInt8ArrayAsString(backCameraPub)
-- 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)
-- Acc
accelCommunicationTube=simTubeOpen(0,'accelerometerData'..simGetNameSuffix(nil),1)
-- Terminal
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['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['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
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
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 = {}
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
Posts: 8450
Joined: 14 Dec 2012, 00:25

### Re: RosInterface stops publishing after restart of simulatio

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

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

when you stop the simulation.

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

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
'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: 62
Joined: 08 Jun 2016, 22:47

### Re: RosInterface stops publishing after restart of simulation

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
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: 62
Joined: 08 Jun 2016, 22:47

### Re: RosInterface stops publishing after restart of simulation

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

### Re: RosInterface stops publishing after restart of simulation

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: 62
Joined: 08 Jun 2016, 22:47

### Re: RosInterface stops publishing after restart of simulation

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

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

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.