How to calculate distance of a target using Velodyne hdl32 on Ubuntu 18.04 [closed]

edit

Hi all, I am trying to calculate the distance from a target using the Velodyne hdl32. For this reason I set up a simulation using Gazebo and followed the intermediate tutorial as I am new to Gazebo. Specifically I did this tutorial. I finished it and was able to run it. Now as exercise I wanted to understand to calculate the distance from the Velodyne hdl32to the target. In this case the target is simply a cube I positioned as is possible to see in the print screen below. I am stuck and am not able to move on:

The code I used is the following below

velodyne_plugin.cc

namespace gazebo
{
  class VelodynePlugin : public ModelPlugin
  {
    public: VelodynePlugin() {}
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
          if (_model->GetJointCount() == 0)
          {
            std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";
            return;
          }
          this->model = _model;
          this->joint = _model->GetJoints()[0];
          this->pid = common::PID(0.1, 0, 0);
          this->model->GetJointController()->SetVelocityPID(
              this->joint->GetScopedName(), this->pid);

          double velocity = 0;
          if (_sdf->HasElement("velocity"))
            velocity = _sdf->Get<double>("velocity");

          this->node = transport::NodePtr(new transport::Node());
          this->node->Init(this->model->GetWorld()->GetName());
          // create topic name
            std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";
          // subscribe the topic
            this->sub = this->node->Subscribe(topicName,
                                              &VelodynePlugin::OnMsg, this);
            // Initialize ros, if it has not already bee initialized.
            if (!ros::isInitialized())
            {
              int argc = 0;
              char **argv = NULL;
              ros::init(argc, argv, "gazebo_client",
                  ros::init_options::NoSigintHandler);
            }
            this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
            // Create a named topic, and subscribe to it.
            ros::SubscribeOptions so =
              ros::SubscribeOptions::create<std_msgs::Float32>(
                  "/" + this->model->GetName() + "/vel_cmd",
                  1,
                  boost::bind(&VelodynePlugin::OnRosMsg, this, _1),
                  ros::VoidPtr(), &this->rosQueue);
            this->rosSub = this->rosNode->subscribe(so);

            // Spin up the queue helper thread.
            this->rosQueueThread =
              std::thread(std::bind(&VelodynePlugin::QueueThread, this));
      }
      public: void SetVelocity(const double &_vel)
      {
          // Set the joints target velocity
          this->model->GetJointController()->SetVelocityTarget(
                      this->joint->GetScopedName(), _vel);
      }
      /// \brief Handle an incoming message from ROS
      public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
      {
        this->SetVelocity(_msg->data);
      }
      private: void QueueThread()
      {
        static const double timeout = 0.01;
        while (this->rosNode->ok())
        {
          this->rosQueue.callAvailable(ros::WallDuration(timeout));
        }
      }
      /// \brief Handle incoming message
      private: void OnMsg(ConstVector3dPtr &_msg)
      {
        this->SetVelocity(_msg->x());
      }
      /// \brief A node used for transport
      private: transport::NodePtr node;
      /// \brief A subscriber to a named topic.
      private: transport::SubscriberPtr sub;
      /// \brief Pointer to the model.
      private: physics::ModelPtr model;
      /// \brief Pointer to the joint.
      private: physics::JointPtr joint;
      /// \brief A PID controller for the joint.
      private: common::PID pid;
      // Communication with ROS
      /// \brief A node use for ROS transport
      private: std::unique_ptr<ros::NodeHandle> rosNode;
      /// \brief A ROS subscriber
      private: ros::Subscriber rosSub;

      /// \brief A ROS callbackqueue that helps process messages
      private: ros::CallbackQueue rosQueue;

      /// \brief A thread the keeps running the rosQueue
      private: std::thread rosQueueThread;
  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}

below the vel.cc from the same tutorial

int main(int _argc, char **_argv)
{
    // Load Gazebo as client
    gazebo::setupClient(_argc, _argv);
    // create our node for communication
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();
    // Publish to the Velodyne topic
    gazebo::transport::PublisherPtr pub =
            node->Advertise<gazebo::msgs::Vector3d>("~/my_velodyne/vel_cmd");
    // wait for a subscriber to connect to this publisher ...