Plugin for rotating velodyne around an axis passing through its center.
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?
Asked by lotfishtaine on 2019-09-27 10:38:51 UTC
Comments
Could you please stop bumping your question (ie: editing without actually editing it). If you haven't received any answers, that probably means that no one knows how to answer.
I would actually suggest you post this over at
answers.gazebosim.org
, as right now it looks like a Gazebo-specific question. Not a ROS question.Asked by gvdhoorn on 2019-10-01 08:52:23 UTC
Okay, Thanks
Asked by lotfishtaine on 2019-10-01 09:38:54 UTC
https://gazebosim.org/tutorials?cat=guided_i&tut=guided_i5 This should help.
Asked by BM on 2022-03-04 08:53:35 UTC