Plugin for rotating velodyne around an axis passing through its center

[Former user]

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, and register a callback
  this->sub = this->node->Subscribe(topicName,
                                    &GazeboRotateVelodynePlugin::OnMsg, this);

  this->torque = 50.0;
    if (!_sdf->HasElement("torque")) {
      ROS_WARN_NAMED("rotate_velodyne_plugin", "GazeboRotateVelodynePlugin Plugin (ns = %s) missing <torque>, defaults to %f",
          this->robot_namespace_.c_str(), this->torque);
    } else {
      this->torque = _sdf->GetElement("torque")->Get<double>();
    }

    this->command_topic_ = "cmd_vel";
    if (!_sdf->HasElement("commandTopic")) {
      ROS_WARN_NAMED("rotate_velodyne_plugin", "GazeboRotateVelodynePlugin Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
          this->robot_namespace_.c_str(), this->command_topic_.c_str());
    } else {
      this->command_topic_ = _sdf->GetElement("commandTopic")->Get<std::string>();
    }

    this->robot_base_frame_ = "base_link";
    if (!_sdf->HasElement("robotBaseFrame")) {
      ROS_WARN_NAMED("rotate_velodyne_plugin", "GazeboRotateVelodynePlugin Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
          this->robot_namespace_.c_str(), this->robot_base_frame_.c_str());
    } else {
      this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get<std::string>();
    }

    this->update_rate_ = 100.0;
    if (!_sdf->HasElement("updateRate")) {
      ROS_WARN_NAMED("rotate_velodyne_plugin", "GazeboRosRotateVelodynePlugin Plugin (ns = %s) missing <updateRate>, defaults to %f",
          this->robot_namespace_.c_str(), this->update_rate_);
    } else {
      this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
    }
          // 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);
          }
// Create our ROS node. This acts in a similar manner to
// the Gazebo node
          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(&GazeboRotateVelodynePlugin::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(&GazeboRotateVelodynePlugin::QueueThread, this));
}
       /// \brief Set the velocity of the Velodyne
      /// \param[in] _vel New target velocity
  public: void SetVelocity(const double &_vel)
      {
          // Set the joint's target velocity.
          this->model->GetJointController()->SetVelocityTarget(
                  this->joint->GetScopedName(), _vel);
      }
      /// \brief Handle incoming message
      /// \param[in] _msg Repurpose a vector3 message. This function will
      /// only use the x component.
  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 Handle an incoming message from ROS
      /// \param[in] _msg A float value that is used to set the velocity
      /// of the Velodyne.
  public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
      {
          this->SetVelocity(_msg->data);
      }

      /// \brief ROS helper function that processes messages
  private: void QueueThread()
      {
          static const double timeout = 0.01;
          while (this->rosNode->ok())
          {
              this->rosQueue.callAvailable(ros::WallDuration(timeout));
          }
      }
    /// \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;

      /// \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;

private:
      double update_rate_;
      double velocity;
      double torque;
      double x_;
      double rot_;
      std::string robot_namespace_;
      std::string command_topic_;
      std::string robot_base_frame_;
      boost::mutex lock;
      ros::CallbackQueue queue_;
      boost::thread callback_queue_thread_;
      ros::Subscriber cmd_vel_subscriber_;
      ros::NodeHandle* rosnode_;
  };
  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(GazeboRotateVelodynePlugin)
}
#endif

The problem is when I come to get rviz to publish the PointCloud2 message I get difformed data. All data are published on one single axis.

The second problem is that I can't control the rotating joint by the /cmd_vel topic, I send commmands using : rostopic pub /vel_cmd std_msgs/Float32 5.0 but nothing changes.

Anyone any idea where the problem lies please?