How to publish diagnostic data of my robot hardware

Typically: "How do I... ", "How can I... " questions
Post Reply
ssj
Posts: 32
Joined: 27 Feb 2017, 15:40

How to publish diagnostic data of my robot hardware

Post by ssj » 11 May 2018, 16:24

Hey
I have developed a robot in vrep that has two rotatory joints for motors, 8-force sensors and a laser scanner for mapping. I want to collect information from robot hardware for analysis, troubleshooting, and logging using ROS diagnostics stack that contains tools for collecting, publishing, analyzing and viewing diagnostics data. How can I configure my robot in vrep with the ROS packages for publishing diagnostics? Or is there any other way to monitor the status (OK, ERROR, WARNING)of hardware devices in VREP that is also compatible with ROS?? Please help..

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

Re: How to publish diagnostic data of my robot hardware

Post by coppelia » 15 May 2018, 08:21

Hello,

you will have to interface those messages with a script, in the exact same way as you would communicate between ROS and V-REP via the RosInterface. Is your model already communicating with ROS?

Cheers

ssj
Posts: 32
Joined: 27 Feb 2017, 15:40

Re: How to publish diagnostic data of my robot hardware

Post by ssj » 16 May 2018, 05:13

Hey Coppelia

Thanks for your response.
Yes, my robot in vrep is already communicating with ROS. But I m unable to collect diagnostic data of my robot components like joint, hokuyo..
what messages are you talking about, please explain..
can you give me some example like the procedure to get the status of a joint using ROS diagnostic_updater so that I can have idea and do the rest myself?

fferri
Posts: 212
Joined: 09 Sep 2013, 19:28

Re: How to publish diagnostic data of my robot hardware

Post by fferri » 18 May 2018, 14:10

You can send and receive any ROS message with the RosInterface plugin.

Any kind of message is sent and received in the same way. You would need a publisher if you want to broadcast data, and a subscriber if you want to receive data.

Lookup the message definition on the web, or on your system, and create it in a Lua table.

For example for creating a DiagnosticArray message, you would do:

Code: Select all

msg = {
    header = {
        stamp = sim.getSimulationTime(),
        frame_id = 'set this value appropriately'
    },
    status = {
        {level=0, name='...', message='...', hardware_id='...', values={...}},
        {...},
        {...}
    }
}

ssj
Posts: 32
Joined: 27 Feb 2017, 15:40

Re: How to publish diagnostic data of my robot hardware

Post by ssj » 21 May 2018, 06:35

Thank you fferri for your response. The problem I am facing is that how this diagnostic message would fill in automatically to tell the status? like in case of real hardware we can use hokuyo_node package to get the diagnostic data of hokuyo. In the same way cant we use hokuyo_node for the laser scanner of vrep to get diagnostic data? do you mean that I need to create and fill in diagnostic message in my hokuyo script presented below and fill out all options for possible levels like OK, WARN,ERROR,STALE. On which basis am I going to switch between these levels?


if (sim_call_type==sim_childscriptcall_initialization) then
laserHandle=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_laser")
jointHandle=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_joint")
modelRef=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_ref")
modelHandle=simGetObjectAssociatedWithScript(sim_handle_self)
objName=simGetObjectName(modelHandle)
communicationTube=simTubeOpen(0,objName..'_HOKUYO',1)

scanRange=180*math.pi/180 --You can change the scan range. Angle_min=-scanRange/2, Angle_max=scanRange/2-stepSize
stepSize=8*math.pi/1024
pts=math.floor(scanRange/stepSize)
dists={}
points={}
segments={}

for i=1,pts*3,1 do
table.insert(points,0)
end
for i=1,pts*7,1 do
table.insert(segments,0)
end

black={0,0,0}
red={1,0,0}
lines100=simAddDrawingObject(sim_drawing_lines,1,0,-1,1000,black,black,black,red)
points100=simAddDrawingObject(sim_drawing_points,4,0,-1,1000,black,black,black,red)


-- Check if the required plugin is there (libv_repExtRos.so or libv_repExtRos.dylib):
local moduleName=0
local moduleVersion=0
local index=0
local pluginNotFound=true
while moduleName do
moduleName,moduleVersion=simGetModuleName(index)
if (moduleName=='Ros') then
pluginNotFound=false
end
index=index+1
end

if (pluginNotFound) then
-- Display an error message if the plugin was not found:
simDisplayDialog('Hokuyo_URG_04LX_UG01_ROS error','ROS plugin was not found.&&nSimulation will not run properly',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
end

topicName=simExtROS_enablePublisher('/scan',1,simros_strmcmd_get_laser_scanner_data ,modelHandle,-1,'rangeDat'..objName)

parentTf=simGetObjectHandle("LayerB") --get handle to parent object in tf tree. Change this to your needs
tfname=simExtROS_enablePublisher('tf',1,simros_strmcmd_get_transform ,modelHandle,parentTf,'/Hokuyo%/base_link') --publish the tf (child%parent)
--tfname=simExtROS_enablePublisher('tf',1,simros_strmcmd_get_transform ,modelHandle,parentTf,'/base_link%/base_link') --publish the tf (child%parent)
tfname=simExtROS_enablePublisher('tf',1,simros_strmcmd_get_transform ,parentTf,-1,'/base_link%/odom') --publish the tf (child%parent)
end

if (sim_call_type==sim_childscriptcall_cleanup) then
simRemoveDrawingObject(lines100)
simRemoveDrawingObject(points100)
end
if (sim_call_type==sim_childscriptcall_sensing) then
showLaserPoints=simGetScriptSimulationParameter(sim_handle_self,'showLaserPoints')
showLaserSegments=simGetScriptSimulationParameter(sim_handle_self,'showLaserSegments')
dists={}
angle=-scanRange*0.5
simSetJointPosition(jointHandle,angle)
jointPos=angle

laserOrigin=simGetObjectPosition(jointHandle,-1)
modelInverseMatrix=simGetInvertedMatrix(simGetObjectMatrix(modelRef,-1))

for ind=0,pts-1,1 do

r,dist,pt=simHandleProximitySensor(laserHandle) -- pt is relative to the laser ray! (rotating!)
m=simGetObjectMatrix(laserHandle,-1)
if r>0 then
dists[ind]=dist
-- We put the RELATIVE coordinate of that point into the table that we will return:
ptAbsolute=simMultiplyVector(m,pt)
ptRelative=simMultiplyVector(modelInverseMatrix,ptAbsolute)
points[3*ind+1]=ptRelative[1]
points[3*ind+2]=ptRelative[2]
points[3*ind+3]=ptRelative[3]
segments[7*ind+7]=1 -- indicates a valid point
else
dists[ind]=0
-- If we didn't detect anything, we specify (0,0,0) for the coordinates:
ptAbsolute=simMultiplyVector(m,{0,0,6})
points[3*ind+1]=0
points[3*ind+2]=0
points[3*ind+3]=0
segments[7*ind+7]=0 -- indicates an invalid point
end
segments[7*ind+1]=laserOrigin[1]
segments[7*ind+2]=laserOrigin[2]
segments[7*ind+3]=laserOrigin[3]
segments[7*ind+4]=ptAbsolute[1]
segments[7*ind+5]=ptAbsolute[2]
segments[7*ind+6]=ptAbsolute[3]

ind=ind+1
angle=angle+stepSize
jointPos=jointPos+stepSize
simSetJointPosition(jointHandle,jointPos)
end



simAddDrawingObjectItem(lines100,nil)
simAddDrawingObjectItem(points100,nil)

if (showLaserPoints or showLaserSegments) then
t={0,0,0,0,0,0}
for i=0,pts-1,1 do
t[1]=segments[7*i+4]
t[2]=segments[7*i+5]
t[3]=segments[7*i+6]
t[4]=segments[7*i+1]
t[5]=segments[7*i+2]
t[6]=segments[7*i+3]
if showLaserSegments then
simAddDrawingObjectItem(lines100,t)
end
if (showLaserPoints and segments[7*i+7]~=0)then
simAddDrawingObjectItem(points100,t)
end
end
end


-- Now send the data:
if #points>0 then
table.insert(dists,-scanRange*0.5) --append angle_min to [ranges]
table.insert(dists,scanRange*0.5-stepSize) --append angle_max to [ranges]
table.insert(dists,stepSize) --append stepsize to [ranges]
-- The publisher defined further up uses the data stored in signal 'rangeDat'..objName
simSetStringSignal('rangeDat'..objName,simPackFloats(dists))
end
end

fferri
Posts: 212
Joined: 09 Sep 2013, 19:28

Re: How to publish diagnostic data of my robot hardware

Post by fferri » 21 May 2018, 07:54

It's not possible with the old ROS plugin.

Use the new RosInterface plugin.

You have to manually create a publisher with simROS.advertise, and publish the data with simROS.publish.

Have a look at the rosInterfaceTopicPublisherAndSubscriber.ttt scene, and the ROS tutorial.

Post Reply