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 * 00008 * Licensed under the Apache License, Version 2.0 (the "License"); 00009 * you may not use this file except in compliance with the License. 00010 * You may obtain a copy of the License at 00011 * 00012 * http://www.apache.org/licenses/LICENSE-2.0 00013 00014 * Unless required by applicable law or agreed to in writing, software 00015 * distributed under the License is distributed on an "AS IS" BASIS, 00016 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00017 * See the License for the specific language governing permissions and 00018 * limitations under the License. 00019 */ 00020 00021 #ifndef ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_NODE_H 00022 #define ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_NODE_H 00023 00024 #include <boost/bind.hpp> 00025 #include <Eigen/Eigen> 00026 #include <stdio.h> 00027 00028 #include <geometry_msgs/PoseStamped.h> 00029 #include <mav_msgs/RollPitchYawrateThrust.h> 00030 #include <mav_msgs/Actuators.h> 00031 #include <nav_msgs/Odometry.h> 00032 #include <ros/ros.h> 00033 #include <ros/callback_queue.h> 00034 00035 #include "rotors_control/common.h" 00036 #include "rotors_control/roll_pitch_yawrate_thrust_controller.h" 00037 00038 namespace rotors_control { 00039 00040 class RollPitchYawrateThrustControllerNode { 00041 public: 00042 RollPitchYawrateThrustControllerNode(); 00043 ~RollPitchYawrateThrustControllerNode(); 00044 00045 void InitializeParams(); 00046 void Publish(); 00047 00048 private: 00049 00050 RollPitchYawrateThrustController roll_pitch_yawrate_thrust_controller_; 00051 00052 std::string namespace_; 00053 00054 // subscribers 00055 ros::Subscriber cmd_roll_pitch_yawrate_thrust_sub_; 00056 ros::Subscriber odometry_sub_; 00057 00058 ros::Publisher motor_velocity_reference_pub_; 00059 00060 void RollPitchYawrateThrustCallback( 00061 const mav_msgs::RollPitchYawrateThrustConstPtr& roll_pitch_yawrate_thrust_reference_msg); 00062 00063 void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); 00064 }; 00065 } 00066 00067 #endif // ROTORS_CONTROL_ROLL_PITCH_YAWRATE_THRUST_CONTROLLER_NODE_H