Plugin for rotating velodyne around an axis passing through its center.

edit

Hi,

I'm trying to create a plugin to make velodyne VLP-16 Lidar rotate around an axis passing through its center (X or Y axis). This generates 16 rotating scan lines subject to the effect of their natural rotation around the Z axis and another rotation of the velodyne body around X or Y axis.

I'm using gazebo 9.0 and ros Melodic.

The URDF joint definition is :

    <joint name="${name}_base_mount_joint" type="continuous">
      <xacro:insert_block name="origin" />
      <parent link="${namespace}/${parent}"/>
      <child link="${namespace}/${name}"/>
      <!-- origin xyz="0 0 -0.11885" rpy="3.14 0 0" /-->
      <axis xyz="1 0 0" rpy="0 0 0" />
      <limit effort="100" velocity="1"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

  <!-- Transmission is important to link the joints and the controller -->
    <transmission name="${name}_base_mount_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${name}_base_mount_joint">
     <hardwareInterface>EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="${name}_base_mount_joint_motor">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

  <gazebo>
    <plugin name="gazebo_rotate_velodyne_plugin" filename="libgazebo_ros_rotate_velodyne.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <velocity>50</velocity>
      <MotorJoint>velodyne_base_mount_joint</MotorJoint>
      <torque>60</torque>
      <broadcastTF>1</broadcastTF>
      <commandTopic>cmd_vel</commandTopic>
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
  </gazebo>

The plugin configuration is this:

 #ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_    
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
// Boost
#include <boost/thread.hpp>
// Custom Callback Queue
#include <ros/callback_queue.h>
#include <gazebo/physics/physics.hh>
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class GazeboRotateVelodynePlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: GazeboRotateVelodynePlugin() {}
    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
    // Output this message to see if this plugin is attached to our velodyne model
    std::cerr << "\nThe velodyne plugin is attach to model[" <<
              _model->GetName() << "]\n";
  // Safety check
  if (_model->GetJointCount() == 0)
  {
    std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";
    return;
  }
  // Store the model pointer for convenience.
  this->model = _model;
  // Get the first joint. We are making an assumption about the model
  // having one joint that is the rotational joint.
  this->joint = _model->GetJoints()[0];
  // Setup a P-controller, with a gain of 0.1.
  this->pid = common::PID(0.1, 0.1, 0.1);
  // Apply the P-controller to the joint.
  this->model->GetJointController()->SetVelocityPID(
      this->joint->GetScopedName(), this->pid);
  // Default to zero velocity
  this->velocity = 0;
  // Check that the velocity element exists, then read the value
  if (_sdf->HasElement("velocity"))
      velocity = _sdf->Get<double>("velocity");

  this->SetVelocity(velocity);

  // Create the node
  this->node = transport::NodePtr(new transport::Node());
    #if GAZEBO_MAJOR_VERSION < 8
              this->node->Init(this->model->GetWorld()->GetName());
    #else
              this->node->Init(this->model->GetWorld()->Name());
    #endif

  // Create a topic name
  std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

  // Subscribe to the topic ...