You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
286 lines
9.5 KiB
286 lines
9.5 KiB
function sysCall_threadmain()
|
|
-- Get some handles first:
|
|
-- local leftMotor=sim.getObjectHandle("remoteApiControlledBubbleRobLeftMotor") -- Handle of the left motor
|
|
-- local rightMotor=sim.getObjectHandle("remoteApiControlledBubbleRobRightMotor") -- Handle of the right motor
|
|
-- local noseSensor=sim.getObjectHandle("remoteApiControlledBubbleRobSensingNose") -- Handle of the proximity sensor
|
|
|
|
-- Choose a port that is probably not used (try to always use a similar code):
|
|
--[[
|
|
sim.setThreadAutomaticSwitch(false)
|
|
local portNb=sim.getInt32Parameter(sim.intparam_server_port_next)
|
|
local portStart=sim.getInt32Parameter(sim.intparam_server_port_start)
|
|
local portRange=sim.getInt32Parameter(sim.intparam_server_port_range)
|
|
local newPortNb=portNb+1
|
|
if (newPortNb>=portStart+portRange) then
|
|
newPortNb=portStart
|
|
end
|
|
sim.setInt32Parameter(sim.intparam_server_port_next,newPortNb)
|
|
sim.setThreadAutomaticSwitch(true)
|
|
--]]
|
|
|
|
-- Check what OS we are using:
|
|
platf=sim.getInt32Parameter(sim.intparam_platform)
|
|
if (platf==0) then
|
|
pluginFile='simExtRemoteApi.dll'
|
|
end
|
|
if (platf==1) then
|
|
pluginFile='libsimExtRemoteApi.dylib'
|
|
end
|
|
if (platf==2) then
|
|
pluginFile='libsimExtRemoteApi.so'
|
|
end
|
|
|
|
-- Check if the required legacy remote Api plugin is there:
|
|
moduleName=0
|
|
moduleVersion=0
|
|
index=0
|
|
pluginNotFound=true
|
|
while moduleName do
|
|
moduleName,moduleVersion=sim.getModuleName(index)
|
|
if (moduleName=='RemoteApi') then
|
|
pluginNotFound=false
|
|
end
|
|
index=index+1
|
|
end
|
|
|
|
if (pluginNotFound) then
|
|
-- Plugin was not found
|
|
sim.displayDialog('Error',"Remote Api plugin was not found. ('"..pluginFile.."')&&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
|
else
|
|
-- Ok, we found the plugin.
|
|
-- We first start the remote Api server service (this requires the simExtRemoteApi plugin):
|
|
-- simRemoteApi.start(portNb) -- this server function will automatically close again at simulation end
|
|
simRemoteApi.start(19999)
|
|
-- Now we start the client application:
|
|
|
|
|
|
--result=sim.launchExecutable('bubbleRobClient_remoteApi',portNb.." "..leftMotor.." "..rightMotor.." "..noseSensor,0) -- set the last argument to 1 to see the console of the launched client
|
|
|
|
-- if (result==-1) then
|
|
-- The executable could not be launched!
|
|
-- sim.displayDialog('Error',"'bubbleRobClient_remoteApi' could not be launched. &&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
|
-- end
|
|
end
|
|
|
|
-- This thread ends here. The bubbleRob will however still be controlled by
|
|
-- the client application via the legacy remote Api mechanism!
|
|
end
|
|
|
|
function sysCall_cleanup()
|
|
-- Put some clean-up code here
|
|
end
|
|
|
|
-- See the user manual or the available code snippets for additional callback functions and details
|
|
|
|
|
|
function getRobotHandle(inInts,inFloats,inStrings,inBuffer)
|
|
-- inInts, inFloats and inStrings are tables
|
|
-- inBuffer is a string
|
|
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
|
-- Perform any type of operation here.
|
|
|
|
-- Always return 3 tables and a string, e.g.:
|
|
return {},{},{},robotHandle
|
|
end
|
|
|
|
-- This is an extremely simple control script for the motorbike. It is meant as a SIMPLE example!
|
|
getRobotPos=function(inInts,inFloats,inStrings,inBuffer)
|
|
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
|
position=sim.getObjectPosition(robotHandle,-1)
|
|
return {},position,{},''
|
|
end
|
|
|
|
function setRobotPos(inInts,inFloats,inStrings,inBuffer)
|
|
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
|
position=sim.getObjectPosition(robotHandle,-1)
|
|
return {},position,{},''
|
|
end
|
|
|
|
|
|
--[[
|
|
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
|
-- Retrieve some object handles:
|
|
motor=sim.getObjectHandle('Motorbike_motor')
|
|
steering=sim.getObjectHandle('Motorbike_steeringMotor')
|
|
motorbike=sim.getObjectHandle('Motorbike')
|
|
path=sim.getObjectHandle('Motorbike_path')
|
|
pathFollower=sim.getObjectHandle('Motorbike_pathFollower')
|
|
pathFollowerTarget=sim.getObjectHandle('Motorbike_pathFollowerTarget')
|
|
-- Make the path object orphan (because at the beginning of the simulation it is still built on the motorbike model):
|
|
sim.setObjectParent(path,-1,true)
|
|
-- Set a fixed velocity:
|
|
velocity = 30
|
|
cStep = 5
|
|
step = 0.00
|
|
deviation = 0.0
|
|
|
|
t = 0
|
|
CInfo = {}
|
|
for i = 1,2,1 do
|
|
CInfo[i] = 0
|
|
end
|
|
|
|
oldPos = {}
|
|
nowLA = {}
|
|
basicPos = {}
|
|
for i = 1,3,1 do
|
|
oldPos[i] = 0
|
|
nowLA[2*i-1] = 0.0
|
|
nowLA[2*i] = 0.0
|
|
basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
|
|
basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
|
|
basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
|
|
basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
|
|
end
|
|
inclination = 0
|
|
counter = 0
|
|
------------------------------------------
|
|
-- Check if the required ROS plugin is there:
|
|
moduleName=0
|
|
moduleVersion=0
|
|
index=0
|
|
RosInterface=false
|
|
while moduleName do
|
|
moduleName,moduleVersion=sim.getModuleName(index)
|
|
if (moduleName=='RosInterface') then
|
|
RosInterface=true
|
|
end
|
|
index=index+1
|
|
end
|
|
|
|
if (RosInterface) then
|
|
sim.addStatusbarMessage('Ros Connected')
|
|
-- Prepare the sensor publisher and the motor speed subscribers:
|
|
-- Publisher
|
|
The_Pos = simExtRosInterface_advertise('/The_P', 'std_msgs/Float32MultiArray')
|
|
The_Vec = simExtRosInterface_advertise('/The_V', 'std_msgs/Float32MultiArray')
|
|
The_Inf = simExtRosInterface_advertise('/The_I', 'std_msgs/Float32MultiArray')
|
|
The_Loc = simExtRosInterface_advertise('/The_L', 'std_msgs/Float32MultiArray')
|
|
The_Time = simExtRosInterface_advertise('/The_T', 'std_msgs/Float32MultiArray')
|
|
--Subscriber
|
|
Controller = simExtRosInterface_subscribe('/Ctrl_I','std_msgs/Float32MultiArray','setControlInfo')
|
|
ResetRobot=simExtRosInterface_subscribe('/resetRobot','std_msgs/UInt32','resetRobot_cb')
|
|
end
|
|
end
|
|
--]]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
-- do some initialization here
|
|
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
|
|
|
-- Reset robot subscriber callback
|
|
allModelObjects=sim.getObjectsInTree(robotHandle)
|
|
sim.setThreadAutomaticSwitch(false)
|
|
for i=1,#allModelObjects,1 do
|
|
sim.resetDynamicObject(allModelObjects[i]) -- reset all objects in the model
|
|
end
|
|
--basicEle = sim.getObjectOrientation(robotHandle, -1)
|
|
--basicEle = sim.getObjectQuaternion(robotHandle, -1)
|
|
--sim.addStatusbarMessage(string.format(" -> %f --> msg: %d",basicEle[1],msg.data))
|
|
--sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[2],msg.data))
|
|
--sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[3],msg.data))
|
|
--sim.addStatusbarMessage(string.format("--> %f",basicEle[4]))
|
|
--sim.addStatusbarMessage(string.format("--> Start Statement: %d", msg.data))
|
|
oldPos = {}
|
|
nowLA = {}
|
|
basicPos = {}
|
|
for i = 1,3,1 do
|
|
oldPos[i] = 0
|
|
nowLA[2*i-1] = 0.0
|
|
nowLA[2*i] = 0.0
|
|
basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
|
|
basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
|
|
basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
|
|
basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
|
|
end
|
|
|
|
basicLoc = {}
|
|
basicAng = {}
|
|
nowState = msg.data
|
|
for i = 1,3,1 do
|
|
basicLoc[i] = basicPos[nowState][i]
|
|
basicAng[i] = basicPos[nowState][i+3]
|
|
end
|
|
sim.setObjectPosition(robotHandle,-1,basicLoc)
|
|
sim.setObjectOrientation(robotHandle,-1,basicAng)
|
|
sim.setThreadAutomaticSwitch(true)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
function sysCall_threadmain()
|
|
-- Check if the required plugin is there:
|
|
moduleName=0
|
|
moduleVersion=0
|
|
index=0
|
|
bubbleRobModuleNotFound=true
|
|
while moduleName do
|
|
moduleName,moduleVersion=sim.getModuleName(index)
|
|
if (moduleName=='BubbleRob') then
|
|
bubbleRobModuleNotFound=false
|
|
end
|
|
index=index+1
|
|
end
|
|
if (bubbleRobModuleNotFound) then
|
|
sim.displayDialog('Error','BubbleRob plugin was not found. (v_repExtBubbleRob.dll)&&nSimulation will not run properly',sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
|
else
|
|
local jointHandles={sim.getObjectHandle('leftMotor'),sim.getObjectHandle('rightMotor')}
|
|
local sensorHandle=sim.getObjectHandle('sensingNose')
|
|
local robHandle=simBubble.create(jointHandles,sensorHandle,{0.5,0.25})
|
|
if robHandle>=0 then
|
|
simBubble.start(robHandle,20) -- control happens here
|
|
simBubble.stop(robHandle)
|
|
simBubble.destroy(robHandle)
|
|
end
|
|
end
|
|
end
|
|
|
|
|
|
|
|
|
|
|
|
|
|
function sysCall_init()
|
|
movingObjects={'bubbleRob'}
|
|
pos={}
|
|
orient={}
|
|
for k, v in pairs(movingObjects) do
|
|
h=sim.getObjectHandle(v)
|
|
movingObjects[k]=h
|
|
pos[h]=sim.getObjectPosition(h,-1)
|
|
orient[h]=sim.getObjectOrientation(h,-1)
|
|
end
|
|
posBase={}
|
|
h=sim.getObjectHandle('bubbleRob')
|
|
posBase=sim.getObjectPosition(h,-1)
|
|
|
|
end
|
|
|
|
function reset()
|
|
for k, v in pairs(movingObjects) do
|
|
sim.setObjectPosition(v,-1,pos[v])
|
|
sim.setObjectOrientation(v,-1,orient[v])
|
|
sim.resetDynamicObject(v)
|
|
end
|
|
end |