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