Hello everyone,
Hoping someone can help
I've been using a simple set up with ROS and python to send a pose to coppeliasim. I have a python script which publishes the pose to a topic and have a simROS.subscribe to that topic in a threaded child script. Once coppeliasim receives the pose, it plans a path and moves to that pose. However the motion is quite jerky. I notice that if I manually shut down the python publisher, after coppeliasim receives the pose, the motion is smooth.
I want python to automatically shutdown the publisher once coppeliasim has received the pose and I read that I could do this by publishing until the number of subscribers has dropped. I've tried to get the subscriber to stop after it has received the pose using simROS.shutdownSubscriber but it doesn't seem to stop the subscriber. When I do rostopic info on the topic, it still shows that Coppeliasim is subscribed and it shows this until I stop the simulation.
Do you know if there's a way I can stop the ROS subscriber once it has received the information from the publisher?
Thank you,
Kat
How to stop a ROS subscriber while the script is running?
Re: How to stop a ROS subscriber while the script is running?
Hello Kat,
not sure I understand your problem exactly. If the motion is not smooth, then your logic is probably bad. E.g. if messages keep coming, then they should be buffered somewhere, and executed as soon as they are allowed to (e.g. when the previous movement was fully executed). Or, while the movement is executing, you should discard all incoming messages (e.g. by setting an enabled flag: if it is off, then incoming messages are simply dropped). Shutting down a subscribed and starting it continuously is not the right approach.
Cheers
not sure I understand your problem exactly. If the motion is not smooth, then your logic is probably bad. E.g. if messages keep coming, then they should be buffered somewhere, and executed as soon as they are allowed to (e.g. when the previous movement was fully executed). Or, while the movement is executing, you should discard all incoming messages (e.g. by setting an enabled flag: if it is off, then incoming messages are simply dropped). Shutting down a subscribed and starting it continuously is not the right approach.
Cheers
Re: How to stop a ROS subscriber while the script is running?
hello,
Thanks for getting back to me,
I don't really understand how I have the problem.
I have the subscriber in a loop which breaks once the callback receives some data:
and then after this, it runs the path finding and execution. So I don't understand how having the publisher still running slows down the motion of the arm but it speeds up as soon as I kill the python script.
Would you be able to explain more what this means in terms of what I should do?
Kat
Here's all of the code - mostly copied from the tutorials:
Thanks for getting back to me,
I don't really understand how I have the problem.
I have the subscriber in a loop which breaks once the callback receives some data:
Code: Select all
function py_cop_callback(msg) -- retrieves data published by python
pydata=nil
pydata=(msg.data)
if pydata then
recieved_data=true -- has recieved new data from python
end
end
function recieve_data() -- subscribes to python
recieved_data=false
while not (recieved_data==true) do
py_cop_sub=simROS.subscribe('/new_pose','std_msgs/Float64MultiArray','py_cop_callback')
end
return pydata
end
Would you be able to explain more what this means in terms of what I should do?
Thank you for your help,coppelia wrote: ↑07 Oct 2021, 19:39 E.g. if messages keep coming, then they should be buffered somewhere, and executed as soon as they are allowed to (e.g. when the previous movement was fully executed). Or, while the movement is executing, you should discard all incoming messages (e.g. by setting an enabled flag: if it is off, then incoming messages are simply dropped).
Kat
Here's all of the code - mostly copied from the tutorials:
Code: Select all
function sysCall_init()
rosInterfacePresent=simROS
corout=coroutine.create(coroutineMain)
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end
--************************* ROS pub/sub functions ****************************************
function py_cop_callback(msg) -- retrieves data published by python
pydata=nil
pydata=(msg.data)
if pydata then
recieved_data=true -- has recieved new data from python
end
end
function recieve_data() -- subscribes to python
recieved_data=false
while not (recieved_data==true) do
py_cop_sub=simROS.subscribe('/new_pose','std_msgs/Float64MultiArray','py_cop_callback')
end
return pydata
end
--****************Collision Avoidance ****************************************
function visualizePath(path) --draws the path
if not _lineContainer then
_lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{1,1,0})
end
sim.addDrawingObjectItem(_lineContainer,nil)
if path then
local lb=sim.setThreadAutomaticSwitch(false)
local initConfig=getConfig()
local l=#jh
local pc=#path/l
for i=1,pc-1,1 do
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]}
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]}
setConfig(config1)
local lineDat=sim.getObjectPosition(simTip,-1)
setConfig(config2)
local p=sim.getObjectPosition(simTip,-1)
lineDat[4]=p[1]
lineDat[5]=p[2]
lineDat[6]=p[3]
sim.addDrawingObjectItem(_lineContainer,lineDat)
end
setConfig(initConfig)
sim.setThreadAutomaticSwitch(lb)
end
sim.switchThread()
end
--************** Path Finding Functions ****************************************
function _getJointPosDifference(startValue,goalValue,isRevolute)
local dx=goalValue-startValue
if (isRevolute) then
if (dx>=0) then
dx=math.mod(dx+math.pi,2*math.pi)-math.pi
else
dx=math.mod(dx-math.pi,2*math.pi)+math.pi
end
end
return(dx)
end
function _applyJoints(jointHandles,joints)
for i=1,#jointHandles,1 do
sim.setJointTargetPosition(jointHandles[i],joints[i])
end
end
function generatePathLengths(path)
-- Returns a table that contains a distance along the path for each path point
local d=0
local l=#jh
local pc=#path/l
local retLengths={0}
for i=1,pc-1,1 do
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]}
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]}
d=d+getConfigConfigDistance(config1,config2)
retLengths[i+1]=d
end
return retLengths
end
function validationCb(config,auxData)
collisionColors={{1,0,0},{1,0,1}} -- collider and collidee
local retVal=true
local prev={}
for i=1,#jh,1 do
prev[i]=sim.getJointPosition(jh[i])
sim.setJointPosition(jh[i],config[i])
end
for i=1,#collisionPairs/2,1 do
local result,pairHandles=sim.checkCollision(collisionPairs[(i-1)*2+1],collisionPairs[(i-1)*2+2])
if result>0 then
print('Robot is colliding. Colliding pair is '..sim.getObjectName(pairHandles[1])..','..sim.getObjectName(pairHandles[2]))
-- Change color of the collider and collidee objects:
-- changePairColor({pairHandles[1],pairHandles[2]},collisionColors)
retVal=false
break
else
--print('not colliding')
end
end
for i=1,#jh,1 do
sim.setJointPosition(jh[i],prev[i])
end
return retVal
end
function getConfig()
-- Returns the current robot configuration
local config={}
for i=1,#jh,1 do
config[i]=sim.getJointPosition(jh[i])
end
return config
end
function setConfig(config)
-- Applies the specified configuration to the robot
if config then
for i=1,#jh,1 do
sim.setJointPosition(jh[i],config[i])
end
end
end
function getConfigConfigDistance(config1,config2)
-- Returns the distance (in configuration space) between two configurations
local d=0
for i=1,#jh,1 do
local dx=config1[i]-config2[i]
d=d+dx*dx
end
return math.sqrt(d)
end
function getPathLength(path)
-- Returns the length of the path in configuration space
local d=0
local l=#jh --number of joints
local pc=#path/l -- path is list of joint agles; 1pc = 1 pose set of joint angles
for i=1,pc-1,1 do -- loop over all poses in path
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]} -- pose in list
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]} -- next pose in list
d=d+getConfigConfigDistance(config1,config2) -- each next pose in list adds the distance
end
return d
end
function _findPath(startConfig,goalConfig)
local jointLimitsL={-170,-120,-170,-120,-170,-120,-175,-90}
local jointLimitsH={170,120,170,120,170,120,175,90}
local task=simOMPL.createTask('task')
simOMPL.setAlgorithm(task,OMPLAlgo)
local jSpaces={}
for i=1,#jh,1 do
local proj=i
if i>3 then proj=0 end -- creates a projection for only the 1st 3 joints?
jSpaces[#jSpaces+1]=simOMPL.createStateSpace('j_space'..i,simOMPL.StateSpaceType.joint_position,jh[i],{jointLimitsL[i]},{jointLimitsH[i]},proj)
end
simOMPL.setStateSpace(task,jSpaces)
simOMPL.setCollisionPairs(task,collisionPairs)
simOMPL.setStartState(task,startConfig)
simOMPL.setGoalState(task,goalConfig)-- sets goal state
simOMPL.setup(task)
local l=nil
local res,path=simOMPL.compute(task,maxOMPLCalculationTime,-1,200) -- return a min of 200 states --
-- it finds path to each goal state but stops once it reaches goal (doesn't check all goal states)
if #path >0 then
visualizePath(path)
l=getPathLength(path)
end
simOMPL.destroyTask(task)
return path,l
end
function findPath(startConfig,goalConfig)
-- This function will search for a path between the specified start configuration,
-- and several of the specified goal configurations.
local onePath,onePathLength=_findPath(startConfig,goalConfig)
return onePath,generatePathLengths(onePath) -- returns path and table of point to point lenghts on path
end
function path_attempt(maxtry,pose)
returnpath={}
for t=1,maxtry,1 do
print('planning to config ')
path,lengths=findPath(initConfig,pose) -- using OMPL to find the path between the current pose and multiple valid goal poses
if (path and #path>0) then -- if a non zero path has been found
print('found a path')
pathfail=1 -- default to failed
poseinpath={}
for colcheck=1,#path,#jh do -- loops through path to see if any poses in path are colliding
for ja=0,#jh-1,1 do
poseinpath[ja+1] = path[colcheck+ja]
end
notcollide = validationCb(poseinpath)
if (notcollide == false) then --has collided
pathfail=1
print('found a path to the target but it collides ')
break -- out of collision check loop over this path
else
pathfail=0
end
end
if (pathfail==0) then -- no collision
print('found a valid path')
returnpath=path
break -- out of loop through try to plan a path for this pose
end
else
print('found no path to config')
end
end -- loop to next attempt out of maxtry
return returnpath
end
--******Move robot *****************************************************************************
function executeMotion(path,lengths,maxVel,maxAccel,maxJerk)
dt=sim.getSimulationTimeStep()
-- 1. Make sure we are not going too fast for each individual joint (i.e. calculate a correction factor (velCorrection)):
jointsUpperVelocityLimits={}
for j=1,8,1 do
jointsUpperVelocityLimits[j]=sim.getObjectFloatParam(jh[j],sim.jointfloatparam_upper_limit) -- gets the joint velocity limit from joint handle
end
velCorrection=1
sim.setThreadSwitchTiming(200)
while true do
posVelAccel={0,0,0}
targetPosVel={lengths[#lengths],0}
pos=0
res=0
previousQ={path[1],path[2],path[3],path[4],path[5],path[6],path[7],path[8]}
local rMax=0
rmlHandle=sim.rmlPos(1,0.0001,-1,posVelAccel,{maxVel*velCorrection,maxAccel,maxJerk},{1},targetPosVel)
while res==0 do
res,posVelAccel,sync=sim.rmlStep(rmlHandle,dt)
if (res>=0) then
l=posVelAccel[1]
for i=1,#lengths-1,1 do
l1=lengths[i]
l2=lengths[i+1]
if (l>=l1)and(l<=l2) then
t=(l-l1)/(l2-l1)
for j=1,8,1 do
q=path[8*(i-1)+j]+_getJointPosDifference(path[8*(i-1)+j],path[8*i+j],jt[j]==sim.joint_revolute_subtype)*t
dq=_getJointPosDifference(previousQ[j],q,jt[j]==sim.joint_revolute_subtype)
previousQ[j]=q
r=math.abs(dq/dt)/jointsUpperVelocityLimits[j]
if (r>rMax) then
rMax=r
end
end
break
end
end
end
end
sim.rmlRemove(rmlHandle)
if rMax>1.001 then
velCorrection=velCorrection/rMax
else
break
end
end
sim.setThreadSwitchTiming(2)
-- 2. Execute the movement:
posVelAccel={0,0,0}
targetPosVel={lengths[#lengths],0}
pos=0
res=0
jointPos={}
rmlHandle=sim.rmlPos(1,0.0001,-1,posVelAccel,{maxVel*velCorrection,maxAccel,maxJerk},{1},targetPosVel)
while res==0 do
dt=sim.getSimulationTimeStep()
res,posVelAccel,sync=sim.rmlStep(rmlHandle,dt)
if (res>=0) then
l=posVelAccel[1]
for i=1,#lengths-1,1 do
l1=lengths[i]
l2=lengths[i+1]
if (l>=l1)and(l<=l2) then
t=(l-l1)/(l2-l1)
for j=1,8,1 do
jointPos[j]=path[8*(i-1)+j]+_getJointPosDifference(path[8*(i-1)+j],path[8*i+j],jt[j]==sim.joint_revolute_subtype)*t
end
_applyJoints(jh,jointPos)
break
end
end
end
sim.switchThread()
end
sim.rmlRemove(rmlHandle)
end
-- *************************************START HERE:********************************************************************
function coroutineMain()
--** initialise send/recieve variables
pydata={}
recieved_data=false
--**joint setups
jh={-1,-1,-1,-1,-1,-1,-1,-1}
jt={-1,-1,-1,-1,-1,-1,-1,-1}
for i=1,8,1 do
jh[i]=sim.getObjectHandle('LBR_iiwa_14_j'..i)
jt[i]=sim.getJointType(jh[i])
end
--** IK setups
simBase=sim.getObjectHandle(sim.handle_self)
simTarget=sim.getObjectHandle('LBR_iiwa_target')
simTip=sim.getObjectHandle('LBR_iiwa_tip')
ikEnv=simIK.createEnvironment()
ikGroup=simIK.createIkGroup(ikEnv)
local ikElement,simToIkMap=simIK.addIkElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,simIK.constraint_pose)
ikJoints={}
for i=1,#jh,1 do
ikJoints[i]=simToIkMap[jh[i]]
end
ikTip=simToIkMap[simTip]
--**Collision Pairs
local collection=sim.createCollection(0)
sim.addItemToCollection(collection,sim.handle_tree,simBase,0)
-- 2 collision pairs: the first for robot self-collision detection, the second for robot-environment collision detection:
collisionPairs={collection,collection,collection,sim.handle_all}
--** Trajectory setup
maxVel=0.1
maxAccel=0.1
maxJerk=80
ikSteps=20 --steps for IK trajectory
--** OMPL Parameters
maxOMPLCalculationTime=4 -- for one calculation. Higher is better, but takes more time
OMPLAlgo=simOMPL.Algorithm.KPIECE1 -- the OMPL algorithm to use
--** Start pose
initConfig=getConfig() -- function to find joint positions
print('found current robot pose')
--** Recieve joint angles for goal pose
print('waiting for goal poses')
recieve_data()
-- process poses array
poses_array=pydata
pose={}
e=1
for e=1,#jh,1 do
pose[e]=poses_array[e]*math.pi/180 -- convert to rad
end
print(pose)
--** For sent pose try up to a set number of times to find a path
maxtry=10
path_solution={}
path_solution=path_attempt(maxtry, pose)
if #path_solution > 0 then
visualizePath(path_solution)
executeMotion(path_solution,lengths,maxVel,maxAccel,maxJerk) --joint speed position controller
print('path motion completed')
else
print('no path found')
end
sim.wait(45)
sim.stopSimulation()
sim.wait(5)
end
Re: How to stop a ROS subscriber while the script is running?
Not sure I understand your code. Here a very simple example (not really functional, but more for explanation purpose):
In above, your script has a subscriber message that constantly receives messages, commands or data. Those are queues in a buffer. The coroutine function will then check if something is in the queue, extract the first message, handle it (e.g. execute a movement), then check if another message is present, etc.
Your coroutine can also drop all other messages in the queue, once a movement was executed. But in both cases, there is no need to stop the subscriber.
Cheers
Code: Select all
function message(data)
messageQueue[#messageQueue+1]=data
end
function sysCall_init()
messageQueue={}
-- start a subscriber for function 'message'
...
corout = coroutine.create(coroutineStart)
end
function coroutineStart()
while true do
if #messageQueue>0 then
local data=table.remove(messageQueue,1)
-- now you can e.g. execute a movement described by 'data'
...
else
sim.switchThread() -- nothing in the queue, so wait for next simulation step
end
end
end
function sysCall_actuation()
if coroutine.status(corout)~='dead' then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end
Your coroutine can also drop all other messages in the queue, once a movement was executed. But in both cases, there is no need to stop the subscriber.
Cheers