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