I'm trying to get and use depth information through ROS using a V-REP scene and your last ROS plugin, ROS interface.
At the beginning, I tried to follow this post (viewtopic.php?f=9&t=5916) because I had similar wrongs but it seems that the codes of the post doesn't work for me and I can't view depth image through RVIZ or other ROS tools. It also doesn't work to generate pointcloud and laser scan topic with this depth information.
This is the code I'm using:
Code: Select all
function sysCall_init()
activeVisionSensor=sim.getObjectHandle('kinect_depth')
depthView=sim.floatingViewAdd(0.9,0.9,0.2,0.2,0)
sim.adjustView(depthView,activeVisionSensor,64)
colorCam=sim.getObjectHandle('kinect_rgb')
colorView=sim.floatingViewAdd(0.69,0.9,0.2,0.2,0)
sim.adjustView(colorView,colorCam,64)
pub=simROS.advertise('/image', 'sensor_msgs/Image')
simROS.publisherTreatUInt8ArrayAsString(pub)
end
if (sim_call_type==sim_childscriptcall_sensing) then
transform={getTransformStamped(activeVisionSensor,'camera_depth_frame', -1,'map')}
simROS.sendTransforms(transform)
-- Publish the image of the active vision sensor:
local data=simGetVisionSensorDepthBuffer(activeVisionSensor+sim_handleflag_codedstring)
[[local res,nearClippingPlane=simGetObjectFloatParameter(activeVisionSensor,sim_visionfloatparam_near_clipping)
local res,farClippingPlane=simGetObjectFloatParameter(activeVisionSensor,sim_visionfloatparam_far_clipping)
nearClippingPlane=nearClippingPlane*1000 -- we want mm
farClippingPlane=farClippingPlane*1000 -- we want mm
data=sim.transformBuffer(data,sim_buffer_float,farClippingPlane-nearClippingPlane,nearClippingPlane,sim_buffer_uint16)]]
local res=sim.getVisionSensorResolution(activeVisionSensor)
d={}
d_depth['header']={seq=0,stamp=simGetSystemTime(), frame_id="camera_depth_frame"}
d['height']=res[2]
d['width']=res[1]
d['encoding']='16UC1'
d['is_bigendian']=1
d['step']=res[1]*2
d['data']=data
simROS.publish(pub,d)
end
function getTransformStamped(obj,n,relTo,relN)
t=sim.getSimulationTime()
--t=simROS.getTime()
--t=sim.getSystemTime()
p=sim.getObjectPosition(obj,relTo)
o=sim.getObjectQuaternion(obj,relTo)
return {
header={
stamp=t,
frame_id=relN
},
child_frame_id=n,
transform={
translation={x=p[1],y=p[2],z=p[3]},
rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
}
}
end
this is the result: https://ibb.co/HTkRRjX
this is the scene: https://ibb.co/K9mmdmW
As it can be seen on the image, the pointcloud is strange. It seems like nearest object are detected as the farest and the gain of the detector isn't arithmetic.
the code:
Code: Select all
if (sim_call_type==sim.syscb_init) then
depth_pub = simROS.advertise('/vrep/depth', 'std_msgs/Float32MultiArray')
rgb_pub = simROS.advertise('/vrep/rgb', 'sensor_msgs/Image')
simROS.publisherTreatUInt8ArrayAsString(rgb_pub)
depthCam=sim.getObjectHandle('kinect_depth')
colorCam=sim.getObjectHandle('kinect_rgb')
colorView=sim.floatingViewAdd(0.69,0.9,0.2,0.2,0)
sim.adjustView(colorView,colorCam,64)
end
if (sim_call_type==sim.syscb_actuation) then
sim.handleVisionSensor(depthCam)
transform={getTransformStamped(depthCam,'camera_depth_frame', -1,'map')}
simROS.sendTransforms(transform)
end
if (sim_call_type==sim.syscb_sensing) then
sim.handleVisionSensor(depthCam)
tb_depth={}
tb_depth['data']=sim.getVisionSensorDepthBuffer(depthCam)
simROS.publish(depth_pub, tb_depth)
t=simROS.getTime()
image=sim.getVisionSensorImage(colorCam,0,0,0,0,1)
tb_rgb={}
tb_rgb['header']={stamp=t, frame_id="camera_depth_frame"}
tb_rgb['height']=480
tb_rgb['width']=640
tb_rgb['encoding']='rgb8'
tb_rgb['step']=640 * 3
tb_rgb['data']=image
simROS.publish(rgb_pub, tb_rgb)
end
if (sim_call_type==sim.syscb_cleanup) then
simROS.shutdownPublisher(depth_pub)
simROS.shutdownPublisher(rgb_pub)
end
function getTransformStamped(obj,n,relTo,relN)
t=sim.getSimulationTime()
--t=simROS.getTime()
--t=sim.getSystemTime()
p=sim.getObjectPosition(obj,relTo)
o=sim.getObjectQuaternion(obj,relTo)
return {
header={
stamp=t,
frame_id=relN
},
child_frame_id=n,
transform={
translation={x=p[1],y=p[2],z=p[3]},
rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
}
}
end
Code: Select all
const float v_res=640;
const float u_res=480;
const float far_clip=3.5;
const float near_clip=0.1;
const float focal_length=69.40*3.141516/180;
Cloud::Ptr img2cloud(cv::Mat rgb, MultiArray depth_msg)
{
std::vector<float> depth = depth_msg.data;
Point punto;
if (rgb.cols != v_res || rgb.rows != u_res || depth.size() != u_res * v_res) {
std::cout << rgb.cols << ", " << rgb.rows <<std::endl;
std::cout << depth.size() << std::endl;
std::cout << "rgb and depth image size mismatch" << '\n';
exit(0);
}
Cloud::Ptr cloud(new Cloud);
for (size_t u = 0; u < u_res; u++) {
for (size_t v = 0; v < v_res; v++) {
auto rgb_ints = rgb.at<cv::Vec3b>(u, v);
int w = v_res * u + v;
float z = far_clip* depth[w] + near_clip;
float y = z*sin((v-v_res/2)*focal_length/v_res);
float x = z*sin((u-u_res/2)*focal_length/u_res);
Point p;
p.x = x, p.y = y, p.z = z;
p.r = (int) rgb_ints[0]; p.g = (int) rgb_ints[1]; p.b = (int) rgb_ints[2];
cloud->points.push_back(p);
}
}
cloud->header.frame_id = "camera_depth_frame";
cloud->width = cloud->points.size();
cloud->height = 1;
return cloud;
}