Permalink
Browse files

ros_plugin: publish depth data as sensor_msgs/Image.

  • Loading branch information...
marcinkaszynski committed Sep 11, 2016
1 parent 481b74d commit 3e3c1c22703a14e55cf32aff4603c23f82b2a5ab
@@ -1793,6 +1793,7 @@ enum { simros_strmcmdnull_start =0,
simros_strmcmd_get_string_parameter,
simros_strmcmd_get_ui_event_button,
simros_strmcmd_get_vision_sensor_depth_buffer,
simros_strmcmd_get_vision_sensor_depth_image,
simros_strmcmd_get_vision_sensor_image,
simros_strmcmd_read_collision,
simros_strmcmd_read_distance,
@@ -29,6 +29,7 @@
// This file was automatically created for V-REP release V3.3.2 on August 29th 2016
#include "sensor_msgs/distortion_models.h"
#include "sensor_msgs/image.h"

This comment has been minimized.

Show comment
Hide comment
@matteopantano

matteopantano Mar 4, 2017

Image should be with capital "I", Image.h

@matteopantano

matteopantano Mar 4, 2017

Image should be with capital "I", Image.h

#include <boost/algorithm/string/replace.hpp>
#include <map>
#include "../include/vrep_plugin/ROS_server.h"
@@ -725,6 +726,15 @@ bool ROS_server::launchPublisher(SPublisherData& pub,int queueSize)
return(true);
}
if (pub.cmdID==simros_strmcmd_get_vision_sensor_depth_image)
{
if (simGetObjectType(pub.auxInt1)!=sim_object_visionsensor_type)
return(false); // invalid data!
pub.generalPublisher=node->advertise<sensor_msgs::Image>(pub.topicName,queueSize);
pub.dependencyCnt++;
return(true);
}
if (pub.cmdID==simros_strmcmd_get_depth_sensor_data) {
if (simGetObjectType(pub.auxInt1)!=sim_object_visionsensor_type)
return(false); // invalid data!
@@ -1494,6 +1504,60 @@ void ROS_server::streamAllData()
removeThisPublisher=true;
}
if (publishers[pubI].cmdID==simros_strmcmd_get_vision_sensor_depth_image)
{
int resol[2];
if (simGetVisionSensorResolution(publishers[pubI].auxInt1,resol)!=-1)
{
float* buff=simGetVisionSensorDepthBuffer(publishers[pubI].auxInt1);
if (buff!=NULL)
{
// Values returned by simGetVisionSensorDepthBuffer are
// positions between near and far clipping planes.
// Map the [0..1] range to millimeters required by the
// OpenNI format.
float clip_near = 1;
simGetObjectFloatParameter(publishers[pubI].auxInt1,
1000, &clip_near);
float clip_far = 2;
simGetObjectFloatParameter(publishers[pubI].auxInt1,
1001, &clip_far);
float clip_range = clip_far - clip_near;
sensor_msgs::Image out;
char* nm=simGetObjectName(publishers[pubI].auxInt1);
out.header.frame_id = objNameToFrameId(nm);
simReleaseBuffer(nm);
out.header.stamp = inf.headerInfo.stamp;
out.encoding = "mono16";
out.width=resol[0];
out.height=resol[1];
out.is_bigendian = 0;
out.step = out.width * 2;
for (int y = out.height-1; y >= 0; y--) {
for (int x = 0; x < out.width; x++) {
float depth = buff[y * out.width + x];
float depth_mm = 1000 * (clip_near + depth * clip_range);
// OpenNI format reserves 0 for invalid value.
uint16_t v = ((depth_mm > 0) && (depth_mm < (1 << 16))) ? depth_mm : 0;
out.data.push_back(v & 0xff);
out.data.push_back((v >> 8) & 0xff);
}
}
simReleaseBuffer((char*)buff);
publishedSomething=true;
publishers[pubI].generalPublisher.publish(out);
}
else
removeThisPublisher=true;
}
else
removeThisPublisher=true;
}
if (publishers[pubI].cmdID==simros_strmcmd_read_collision)
{
int r=simReadCollision(publishers[pubI].auxInt1);
@@ -362,6 +362,7 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
simRegisterScriptVariable("simros_strmcmd_get_string_parameter",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_string_parameter))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_ui_event_button",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_ui_event_button))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_depth_buffer",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_depth_buffer))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_depth_image",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_depth_image))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_get_vision_sensor_image",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_vision_sensor_image))).c_str(),0);
// simRegisterScriptVariable("simros_strmcmd_get_joint_force",(boost::lexical_cast<std::string>(int(simros_strmcmd_get_joint_force))).c_str(),0);
simRegisterScriptVariable("simros_strmcmd_read_collision",(boost::lexical_cast<std::string>(int(simros_strmcmd_read_collision))).c_str(),0);

3 comments on commit 3e3c1c2

@HammadIftikhar

This comment has been minimized.

Show comment
Hide comment
@HammadIftikhar

HammadIftikhar Apr 14, 2017

I am relatively new on this Vrep and Ros platforms and have recently begun experimenting with Kinect Sensor on Vrep. Please tell me what is the next step after these changes? In my non threaded child-script where I am enabling publishers, "simros_strmcmd_get_vision_sensor_depth_image" is still uncoloured, and also I am not getting any data out using rostopic echo.
Can someone plz guide me ? Maybe I missed something

HammadIftikhar replied Apr 14, 2017

I am relatively new on this Vrep and Ros platforms and have recently begun experimenting with Kinect Sensor on Vrep. Please tell me what is the next step after these changes? In my non threaded child-script where I am enabling publishers, "simros_strmcmd_get_vision_sensor_depth_image" is still uncoloured, and also I am not getting any data out using rostopic echo.
Can someone plz guide me ? Maybe I missed something

@marcinkaszynski

This comment has been minimized.

Show comment
Hide comment
@marcinkaszynski

marcinkaszynski Apr 21, 2017

Owner

Hey @HammadIftikhar. I switched to other projects in the meantime, but take a look at my post about it, maybe it will help you: http://marcinkaszynski.com/2016/09/10/vrep-ros-rgbdslam-simulated-kinect.html

Owner

marcinkaszynski replied Apr 21, 2017

Hey @HammadIftikhar. I switched to other projects in the meantime, but take a look at my post about it, maybe it will help you: http://marcinkaszynski.com/2016/09/10/vrep-ros-rgbdslam-simulated-kinect.html

@HammadIftikhar

This comment has been minimized.

Show comment
Hide comment
@HammadIftikhar

HammadIftikhar May 11, 2017

Hey @marcinkaszynski. Thank you. I read that article of yours and it helped a lot. Now I have another question. The depth image that is being published now has encoding "mono16". How can I modify the code to get "uint16" based depth image?

HammadIftikhar replied May 11, 2017

Hey @marcinkaszynski. Thank you. I read that article of yours and it helped a lot. Now I have another question. The depth image that is being published now has encoding "mono16". How can I modify the code to get "uint16" based depth image?

Please sign in to comment.