ros_plugin: publish depth data as sensor_msgs/Image.

marcinkaszynski · Sep 11, 2016 · 3e3c1c2 · matteopantano · Mar 4, 2017 · 3e3c1c2
1 parent 481b74d
commit 3e3c1c2
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 0 deletions.
diff --git a/ros_packages/vrep_plugin/include/v_repConst.h b/ros_packages/vrep_plugin/include/v_repConst.h
@@ -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,

diff --git a/ros_packages/vrep_plugin/src/ROS_server.cpp b/ros_packages/vrep_plugin/src/ROS_server.cpp
@@ -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"
 #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);

diff --git a/ros_packages/vrep_plugin/src/vrep_plugin.cpp b/ros_packages/vrep_plugin/src/vrep_plugin.cpp
@@ -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);