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_LEE_POSITION_CONTROLLER_NODE_H 00022 #define ROTORS_CONTROL_LEE_POSITION_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/Actuators.h> 00030 #include <mav_msgs/AttitudeThrust.h> 00031 #include <mav_msgs/eigen_mav_msgs.h> 00032 #include <nav_msgs/Odometry.h> 00033 #include <ros/callback_queue.h> 00034 #include <ros/ros.h> 00035 #include <trajectory_msgs/MultiDOFJointTrajectory.h> 00036 00037 #include "rotors_control/common.h" 00038 #include "rotors_control/lee_position_controller.h" 00039 00040 namespace rotors_control { 00041 00042 class LeePositionControllerNode { 00043 public: 00044 LeePositionControllerNode(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh); 00045 ~LeePositionControllerNode(); 00046 00047 void InitializeParams(); 00048 void Publish(); 00049 00050 private: 00051 ros::NodeHandle nh_; 00052 ros::NodeHandle private_nh_; 00053 00054 LeePositionController lee_position_controller_; 00055 00056 std::string namespace_; 00057 00058 // subscribers 00059 ros::Subscriber cmd_trajectory_sub_; 00060 ros::Subscriber cmd_multi_dof_joint_trajectory_sub_; 00061 ros::Subscriber cmd_pose_sub_; 00062 ros::Subscriber odometry_sub_; 00063 00064 ros::Publisher motor_velocity_reference_pub_; 00065 00066 mav_msgs::EigenTrajectoryPointDeque commands_; 00067 std::deque<ros::Duration> command_waiting_times_; 00068 ros::Timer command_timer_; 00069 00070 void TimedCommandCallback(const ros::TimerEvent& e); 00071 00072 void MultiDofJointTrajectoryCallback( 00073 const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); 00074 00075 void CommandPoseCallback( 00076 const geometry_msgs::PoseStampedConstPtr& pose_msg); 00077 00078 void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); 00079 }; 00080 } 00081 00082 #endif // ROTORS_CONTROL_LEE_POSITION_CONTROLLER_NODE_H