How to calculate distance of a target using Velodyne hdl32 on Ubuntu 18.04
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 hdl32
to 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 ...