if (sim_call_type==sim_childscriptcall_initialization) then visionSensor1Handle=simGetObjectHandle("SICK_TiM310_sensor1") visionSensor2Handle=simGetObjectHandle("SICK_TiM310_sensor2") joint1Handle=simGetObjectHandle("SICK_TiM310_joint1") joint2Handle=simGetObjectHandle("SICK_TiM310_joint2") sensorRefHandle=simGetObjectHandle("SICK_TiM310_ref") maxScanDistance=simGetScriptSimulationParameter(sim_handle_self,'maxScanDistance') if maxScanDistance>1000 then maxScanDistance=1000 end if maxScanDistance<0.1 then maxScanDistance=0.1 end simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_far_clipping,maxScanDistance) simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_far_clipping,maxScanDistance) maxScanDistance_=maxScanDistance*0.9999 scanningAngle=simGetScriptSimulationParameter(sim_handle_self,'scanAngle') if scanningAngle>270 then scanningAngle=270 end if scanningAngle<2 then scanningAngle=2 end scanningAngle=scanningAngle*math.pi/180 simSetObjectFloatParameter(visionSensor1Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2) simSetObjectFloatParameter(visionSensor2Handle,sim_visionfloatparam_perspective_angle,scanningAngle/2) simSetJointPosition(joint1Handle,-scanningAngle/4) simSetJointPosition(joint2Handle,scanningAngle/4) red={1,0,0} lines=simAddDrawingObject(sim_drawing_lines,1,0,-1,1000,nil,nil,nil,red) if (simGetInt32Parameter(sim_intparam_program_version)<30004) then simDisplayDialog("ERROR","This version of the SICK sensor is only supported from V-REP V3.0.4 and upwards.&&nMake sure to update your V-REP.",sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end -- Enable an LaserScan publisher: pub = simExtRosInterface_advertise('/scan', 'sensor_msgs/LaserScan') --After calling this function, this publisher will treat uint8 arrays as string. Using strings should be in general much faster that using int arrays in Lua. simExtRosInterface_publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua) angle_min= -135 * (math.pi/180); -- angle correspond to FIRST beam in scan ( in rad) angle_max= 135 * (math.pi/180) -- angle correspond to LAST beam in scan ( in rad) angle_increment = 270*(math.pi/180)/512 -- Angular resolution i.e angle between 2 beams -- sensor scans every 50ms with 512 beams. Each beam is measured in (50 ms/ 512 ) time_increment = (1 / 20) / 512 range_min = 0.05 range_max = maxScanDistance -- scan can measure upto this range end if (sim_call_type==sim_childscriptcall_cleanup) then simRemoveDrawingObject(lines) simExtRosInterface_shutdownPublisher(pub) end if (sim_call_type==sim_childscriptcall_sensing) then measuredData={} distanceData={} if notFirstHere then -- We skip the very first reading simAddDrawingObjectItem(lines,nil) showLines=simGetScriptSimulationParameter(sim_handle_self,'showLaserSegments') r,t1,u1=simReadVisionSensor(visionSensor1Handle) r,t2,u2=simReadVisionSensor(visionSensor2Handle) m1=simGetObjectMatrix(visionSensor1Handle,-1) m01=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1)) m01=simMultiplyMatrices(m01,m1) m2=simGetObjectMatrix(visionSensor2Handle,-1) m02=simGetInvertedMatrix(simGetObjectMatrix(sensorRefHandle,-1)) m02=simMultiplyMatrices(m02,m2) if u1 then p={0,0,0} p=simMultiplyVector(m1,p) t={p[1],p[2],p[3],0,0,0} for j=0,u1[2]-1,1 do for i=0,u1[1]-1,1 do w=2+4*(j*u1[1]+i) v1=u1[w+1] v2=u1[w+2] v3=u1[w+3] v4=u1[w+4] table.insert(distanceData,v4) if (v4<maxScanDistance_) then p={v1,v2,v3} p=simMultiplyVector(m01,p) table.insert(measuredData,p[1]) table.insert(measuredData,p[2]) table.insert(measuredData,p[3]) end if showLines then p={v1,v2,v3} p=simMultiplyVector(m1,p) t[4]=p[1] t[5]=p[2] t[6]=p[3] simAddDrawingObjectItem(lines,t) end end end end if u2 then p={0,0,0} p=simMultiplyVector(m2,p) t={p[1],p[2],p[3],0,0,0} for j=0,u2[2]-1,1 do for i=0,u2[1]-1,1 do w=2+4*(j*u2[1]+i) v1=u2[w+1] v2=u2[w+2] v3=u2[w+3] v4=u2[w+4] table.insert(distanceData,v4) if (v4<maxScanDistance_) then p={v1,v2,v3} p=simMultiplyVector(m02,p) table.insert(measuredData,p[1]) table.insert(measuredData,p[2]) table.insert(measuredData,p[3]) end if showLines then p={v1,v2,v3} p=simMultiplyVector(m2,p) t[4]=p[1] t[5]=p[2] t[6]=p[3] simAddDrawingObjectItem(lines,t) end end end end end notFirstHere=true -- populate the LaserScan message scan={} scan['header']={seq=0,stamp=simExtRosInterface_getTime(), frame_id="SICK_TiM310_ref"} scan['angle_min']=angle_min scan['angle_max']=angle_max scan['angle_increment']=angle_increment scan['time_increment']=time_increment scan['scan_time']=simExtRosInterface_getTime() -- Return the current ROS time i.e. the time returned by ros::Time::now() scan['range_min']=range_min scan['range_max']=range_max scan['ranges'] = distanceData scan['intensities']={} simExtRosInterface_publish(pub, scan) end