4 Replies Latest reply on Jan 26, 2018 1:48 AM by FelixAl

    ROS - Measure distance with D435

    FelixAl

      Hello,

       

      I installed librealsense 2.8.1 and the ROS wrapper 2.0.1. When i'm using the realsense-viewer I can view the depth and color streams. I am also able to measure the distance in the viewer.

      How can I measure the distance with a ROS node?

      I tried the sample code posted here : GitHub - IntelRealSense/librealsense at development  but it dosen't work.

      The sample code uses the rs_pipeline. Is it possible to subscribe to the depth topic and calculate the distance from it? If I rostopic echo the depth stream the resulting values range between 0 and 255 (not the distance in m).

       

      Thank you

        • 1. Re: ROS - Measure distance with D435
          FelixAl
          // License: Apache 2.0. See LICENSE file in root directory.
          // Copyright(c) 2017 Intel Corporation. All Rights Reserved.
          #include <ros/ros.h>
          #include <image_transport/image_transport.h>
          #include <cv_bridge/cv_bridge.h>
          #include <sensor_msgs/image_encodings.h>
          #include <iostream>
          
          
          void chatterCallback(const sensor_msgs::Image::ConstPtr& msg) 
          { 
              cv_bridge::CvImagePtr Dest = cv_bridge::toCvCopy(msg); 
              ROS_INFO("Width: %i", msg->width);
              ROS_INFO("Height: %i", msg->height);
              ROS_INFO("Value: %f", Dest->image.at<float>(msg->width/2,msg->height/2)); 
          } 
          
          
          int main(int argc, char **argv)
          {
          
              ros::init(argc, argv, "distance");
              ros::NodeHandle n;
          
              ros::AsyncSpinner spinner(1);
              spinner.start();
          
                 ros::Subscriber sub = n.subscribe("camera/depth/image_raw", 100, chatterCallback); 
              ros::Duration(100).sleep(
          return 0;
          }
          

           

          I wrote this node. I subscribe to the depth stream and the output of width and height of the depth stream are correct.

          The distance value still doesn't work. It always returns 0.00.

           

          Is my conversion from ROS to OpenCV correct?

          • 2. Re: ROS - Measure distance with D435
            Intel Corporation
            This message was posted on behalf of Intel Corporation

            Hello FelixAl,

            We escalated your question to RealSense developers. We'll let you know as soon as we have an answer.

            Regards,
            Jesus
            Intel Customer Support

            • 3. Re: ROS - Measure distance with D435
              Intel Corporation
              This message was posted on behalf of Intel Corporation

              Hello FelixAl, we received this code snippet from the dev team that will do as you asked.

              Hi,
              The following code snippet can do what he asked. I added a ROS conversion from raw depth pixel to a distance in meters.
               
              #include <ros/ros.h>
              #include <image_transport/image_transport.h>
              #include <cv_bridge/cv_bridge.h>
              #include <sensor_msgs/image_encodings.h>
              #include <iostream>
              #include <depth_image_proc/depth_conversions.h>
              #include <depth_image_proc/depth_traits.h>
               
               
              void chatterCallback(const sensor_msgs::Image::ConstPtr& msg)
              {
                  cv_bridge::CvImagePtr Dest = cv_bridge::toCvCopy(msg);
                  ROS_INFO("Width: %i", msg->width);
                  ROS_INFO("Height: %i", msg->height);
                  auto val = Dest->image.at<uint16_t>(msg->width/2,msg->height/2);
                  auto distance = depth_image_proc::DepthTraits<uint16_t>::toMeters(val);
                  ROS_INFO("Value: %f", distance);
              }
               
               
              int main(int argc, char **argv)
              {
               
                  ros::init(argc, argv, "distance");
                  ros::NodeHandle n;
               
                  ros::AsyncSpinner spinner(1);
                  spinner.start();
               
                  ros::Subscriber sub = n.subscribe("camera/depth/image_rect_raw", 100, chatterCallback);
                  ros::Duration(100).sleep();
                  return 0;
              } 

              The attached file shows the terminal output with Height, Width, and Depth.

              Regards,
              Jesus
              Intel Customer Support
               

              1 of 1 people found this helpful
              • 4. Re: ROS - Measure distance with D435
                FelixAl

                Thank you! It works.

                 

                Regards,

                Felix