## How to publish diagnostic data of my robot hardware

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

### How to publish diagnostic data of my robot hardware

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
Posts: 7396
Joined: 14 Dec 2012, 00:25

### Re: How to publish diagnostic data of my robot hardware

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

Hey Coppelia

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..
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: 408
Joined: 09 Sep 2013, 19:28

### Re: How to publish diagnostic data of my robot hardware

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

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}

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

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
end
if (showLaserPoints and segments[7*i+7]~=0)then
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: 408
Joined: 09 Sep 2013, 19:28

### Re: How to publish diagnostic data of my robot hardware

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.