gazebo_controller_interface.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 
00023 #ifndef ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H
00024 #define ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H
00025 
00026 #include <boost/bind.hpp>
00027 #include <Eigen/Eigen>
00028 #include <stdio.h>
00029 
00030 #include <gazebo/common/Plugin.hh>
00031 #include <gazebo/gazebo.hh>
00032 #include <gazebo/physics/physics.hh>
00033 
00034 #include <mav_msgs/default_topics.h>  // This comes from the mav_comm repo
00035 
00036 #include "Actuators.pb.h"
00037 
00038 #include "rotors_gazebo_plugins/common.h"
00039 
00040 namespace gazebo {
00041 
00045 static const std::string kDefaultMotorVelocityReferenceTopic = "gazebo/command/motor_speed";
00046 
00047 typedef const boost::shared_ptr<const gz_sensor_msgs::Actuators> GzActuatorsMsgPtr;
00048 
00049 class GazeboControllerInterface : public ModelPlugin {
00050  public:
00051   GazeboControllerInterface()
00052       : ModelPlugin(),
00053         received_first_reference_(false),
00054         pubs_and_subs_created_(false),
00055         namespace_(kDefaultNamespace),
00056         // DEFAULT TOPICS
00057         motor_velocity_reference_pub_topic_(kDefaultMotorVelocityReferenceTopic),
00058         command_motor_speed_sub_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS),
00059         //---------------
00060         node_handle_(NULL){}
00061   ~GazeboControllerInterface();
00062 
00063   void InitializeParams();
00064   void Publish();
00065 
00066  protected:
00067   void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00068   void OnUpdate(const common::UpdateInfo& /*_info*/);
00069 
00070  private:
00071 
00074   bool received_first_reference_;
00075 
00078   bool pubs_and_subs_created_;
00079 
00084   void CreatePubsAndSubs();
00085 
00088   Eigen::VectorXd input_reference_;
00089 
00090   //===== VARIABLES READ FROM SDF FILE =====//
00091   std::string namespace_;
00092   std::string motor_velocity_reference_pub_topic_;
00093   std::string command_motor_speed_sub_topic_;
00094 
00095 
00096   gazebo::transport::NodePtr node_handle_;
00097   gazebo::transport::PublisherPtr motor_velocity_reference_pub_;
00098   gazebo::transport::SubscriberPtr cmd_motor_sub_;
00099 
00100   physics::ModelPtr model_;
00101   physics::WorldPtr world_;
00102 
00104   event::ConnectionPtr updateConnection_;
00105 
00106   boost::thread callback_queue_thread_;
00107 
00108   void QueueThread();
00109 
00110   void CommandMotorCallback(GzActuatorsMsgPtr& actuators_msg);
00111 
00112 };
00113 
00114 } // namespace gazebo
00115 
00116 #endif // ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43