Hi everyone,
I'm trying to control the height of a UAV using predictive control. To do so I'm using the "Iris with Standoff" model, but I'm having some trouble to set the velocity of the rotor's joint.
I followed the tutorial "Setting Velocity on Links And Joints" (http://gazebosim.org/tutorials?tut=set_velocity&cat=) and tried to use the "Set Velocity Instantaneously", but the UAV doesn't take off, independent of the velocity that I set, even though the rotors rotate.
It seems to me, that I should set the velocity with joint motors, but I didn't understand how to use. I can't set the velocity using PID Controllers, because that is already in my controller.
I'm setting the velocity with a plugin that is subscribed in a ROS Topic, all the "ROS Thing" is working, I followed the velodyne tutorial (http://gazebosim.org/tutorials?tut=guided_i1).
So, in my situation, how should I set the velocity? Instantaneously or with joint motors? And how can I do this?
I'm using Gazebo7, ROS Kinetic and Ubuntu 16.04.
The model.sdf and the plugin that i developed are below. Sorry if the post is too big, it is because of the models and plugins code, first time here.
Mode.sdf
0 0 0.194923 0 0 0 0.0 0.0 0 0 0 0 0 0 0.468 0.004856 0 0 0.004856 0 0.008801 0 0 -0.08 0 0 0 0.47 0.47 0.23 100.0 0.001 1.0 1.0 model://iris_with_standoffs/meshes/iris.dae 0.123 0.22 -0.11 0 0 0 0.005 0.17 0.123 -0.22 -0.11 0 0 0 0.005 0.17 -0.140 0.21 -0.11 0 0 0 0.005 0.17 -0.140 -0.21 -0.11 0 0 0 0.005 0.17 0 0 0 0 -0 0 0 0 0 0 -0 0 0.15 0.0001 0 0 0.0002 0 0.0002 iris/ground_truth/odometry_sensorgt_link base_link 0 0 1 0 0 0 0 1.0 1 1 0 0 0 0 0 0 0.15 0.00001 0 0 0.00002 0 0.00002 0 0 0 3.141593 0 0 1 1000.0 iris/imu_link base_link 0 0 1 0 0 0 0 1.0 1 1 0.13 -0.22 0.023 0 -0 0 0 0 0 0 -0 0 0.025 9.75e-06 0 0 0.000166704 0 0.000167604 0 0 0 0 -0 0 0.005 0.1 0 0 0 0 -0 0 1 1 1 model://iris_with_standoffs/meshes/iris_prop_ccw.dae 1 0 rotor_0 base_link 0 0 1 -1e+16 1e+16 0.004 1 1 -0.13 0.2 0.023 0 -0 0 0 0 0 0 -0 0 0.025 9.75e-06 0 0 0.000166704 0 0.000167604 0 0 0 0 -0 0 0.005 0.1 0 0 0 0 -0 0 1 1 1 model://iris_with_standoffs/meshes/iris_prop_ccw.dae 1 0 rotor_1 base_link 0 0 1 -1e+16 1e+16 0.004 1 1 0.13 0.22 0.023 0 -0 0 0 0 0 0 -0 0 0.025 9.75e-06 0 0 0.000166704 0 0.000167604 0 0 0 0 -0 0 0.005 0.1 0 0 0 0 -0 0 1 1 1 model://iris_with_standoffs/meshes/iris_prop_cw.dae 1 0 rotor_2 base_link 0 0 1 -1e+16 1e+16 0.004 1 1 -0.13 -0.2 0.023 0 -0 0 0 0 0 0 -0 0 0.025 9.75e-06 0 0 0.000166704 0 0.000167604 0 0 0 0 -0 0 0.005 0.1 0 0 0 0 -0 0 1 1 1 model://iris_with_standoffs/meshes/iris_prop_cw.dae 1 0 rotor_3 base_link 0 0 1 -1e+16 1e+16 0.004 1 1 0
#include
#include
#include
#include
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
namespace gazebo
{
/// \brief A plugin to control a Velodyne sensor.
class IrisPlugin : public ModelPlugin
{
/// \brief Constructor
public: IrisPlugin() {}
/// \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)
{
// Just output a message for now
std::cerr << "\nThe iris plugin is attach to model[" << _model->GetName() << "\n";
// Store the model pointer for convenience.
this->model = _model;
/*
// 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,&IrisPlugin::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);
}
// 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(
"/" + this->model->GetName() + "/vel_cmd",1,
boost::bind(&IrisPlugin::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(&IrisPlugin::QueueThread, this));
}
public: void SetVelocity(const double &_vel)
{
std::cerr <<_vel<< "\n";
std::cout << "Funcao" << "\n";
this->model->GetJoints()[2]->SetVelocity(0, _vel);
this->model->GetJoints()[3]->SetVelocity(0, _vel);
this->model->GetJoints()[4]->SetVelocity(0, -1*_vel);
this->model->GetJoints()[5]->SetVelocity(0, -1*_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)
{
std::cerr << "Mensagem" << "\n";
this->SetVelocity(_msg->x());
}*/
/// \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 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 node use for ROS transport
private: std::unique_ptr 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(IrisPlugin)
}
#endif
↧