How to stop a ROS subscriber while the script is running?

Typically: "How do I... ", "How can I... " questions
Post Reply
kat
Posts: 13
Joined: 02 Sep 2021, 15:50

How to stop a ROS subscriber while the script is running?

Post by kat »

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

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: How to stop a ROS subscriber while the script is running?

Post by coppelia »

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

kat
Posts: 13
Joined: 02 Sep 2021, 15:50

Re: How to stop a ROS subscriber while the script is running?

Post by kat »

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:

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
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?
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).
Thank you for your help,
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



coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: How to stop a ROS subscriber while the script is running?

Post by coppelia »

Not sure I understand your code. Here a very simple example (not really functional, but more for explanation purpose):

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

Post Reply