flyer_interface: flyer_interface.h Source File

Go to the documentation of this file.
00001 /*
00002  *  AscTec Autopilot Interface
00003  *  Copyright (C) 2011, CCNY Robotics Lab
00004  *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00005  *
00006  *  http://robotics.ccny.cuny.edu
00007  *
00008  *  This program is free software: you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation, either version 3 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00020  */
00021 
00022 #ifndef FLYER_INTERFACE_FLYER_INTERFACE_H
00023 #define FLYER_INTERFACE_FLYER_INTERFACE_H
00024 
00025 #include <ros/ros.h>
00026 #include <geometry_msgs/Pose2D.h>
00027 #include <geometry_msgs/PoseStamped.h>
00028 #include <geometry_msgs/TwistStamped.h>
00029 #include <tf/transform_datatypes.h>
00030 #include <tf/transform_broadcaster.h>
00031 #include <tf/transform_listener.h>
00032 #include <ros/service_server.h>
00033 #include <dynamic_reconfigure/server.h>
00034 #include <message_filters/subscriber.h>
00035 #include <message_filters/synchronizer.h>
00036 #include <message_filters/sync_policies/approximate_time.h>
00037 
00038 #include <sensor_msgs/Imu.h>
00039 #include <std_msgs/UInt8.h>
00040 #include <std_msgs/Float32.h>
00041 #include <std_msgs/Int16.h>
00042 #include <mav_msgs/Height.h>
00043 #include <mav_msgs/RC.h>
00044 #include <mav_msgs/Roll.h>
00045 #include <mav_msgs/Pitch.h>
00046 #include <mav_msgs/YawRate.h>
00047 #include <mav_msgs/Thrust.h>
00048 
00049 #include <mav_srvs/AdvanceState.h>
00050 #include <mav_srvs/RetreatState.h>
00051 #include <mav_srvs/GetFlightState.h>
00052 #include <mav_srvs/ESTOP.h>
00053 #include <mav_srvs/ToggleEngage.h>
00054 #include <mav_srvs/Takeoff.h>
00055 #include <mav_srvs/Land.h>
00056 #include <mav_srvs/SetCtrlType.h>
00057 
00058 #include <mav_common/comm.h>
00059 #include <mav_common/comm_packets.h>
00060 #include <mav_common/comm_util.h>
00061 #include <mav_common/control_types.h>
00062 
00063 #include "flyer_interface/serial_communication.h"
00064 
00065 #include "flyer_interface/PIDXConfig.h"
00066 #include "flyer_interface/PIDYConfig.h"
00067 #include "flyer_interface/PIDZConfig.h"
00068 #include "flyer_interface/PIDVXConfig.h"
00069 #include "flyer_interface/PIDVYConfig.h"
00070 #include "flyer_interface/PIDVZConfig.h"
00071 #include "flyer_interface/PIDYawConfig.h"
00072 #include "flyer_interface/CommConfig.h"
00073 #include "flyer_interface/CtrlConfig.h"
00074 
00075 namespace mav
00076 {
00077 
00078 using namespace message_filters::sync_policies;
00079 
00080 class FlyerInterface
00081 {
00082   typedef flyer_interface::PIDXConfig   PIDXConfig;
00083   typedef flyer_interface::PIDYConfig   PIDYConfig;
00084   typedef flyer_interface::PIDZConfig   PIDZConfig;
00085   typedef flyer_interface::PIDVXConfig  PIDVXConfig;
00086   typedef flyer_interface::PIDVYConfig  PIDVYConfig;
00087   typedef flyer_interface::PIDVZConfig  PIDVZConfig;
00088   typedef flyer_interface::PIDYawConfig PIDYawConfig;
00089   typedef flyer_interface::CommConfig   CommConfig;
00090   typedef flyer_interface::CtrlConfig   CtrlConfig;
00091 
00092   typedef dynamic_reconfigure::Server<PIDXConfig>   CfgXServer;
00093   typedef dynamic_reconfigure::Server<PIDYConfig>   CfgYServer;
00094   typedef dynamic_reconfigure::Server<PIDZConfig>   CfgZServer;
00095   typedef dynamic_reconfigure::Server<PIDVXConfig>  CfgVXServer;
00096   typedef dynamic_reconfigure::Server<PIDVYConfig>  CfgVYServer;
00097   typedef dynamic_reconfigure::Server<PIDVZConfig>  CfgVZServer;
00098   typedef dynamic_reconfigure::Server<PIDYawConfig> CfgYawServer;
00099   typedef dynamic_reconfigure::Server<CommConfig>   CfgCommServer;
00100   typedef dynamic_reconfigure::Server<CtrlConfig>   CfgCtrlServer;
00101 
00102   typedef geometry_msgs::PoseStamped  PoseStamped;
00103   typedef geometry_msgs::TwistStamped TwistStamped;
00104 
00105   typedef message_filters::sync_policies::ApproximateTime<PoseStamped, TwistStamped> SyncPolicy;
00106   typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00107   typedef message_filters::Subscriber<PoseStamped>  PoseStampedSubscriber; 
00108   typedef message_filters::Subscriber<TwistStamped> TwistStampedSubscriber;
00109 
00110   public:
00111 
00112     FlyerInterface(ros::NodeHandle nh, ros::NodeHandle nh_private);
00113     virtual ~FlyerInterface();
00114 
00115   private:
00116 
00117     // **** ROS-related
00118 
00119     ros::NodeHandle nh_;
00120     ros::NodeHandle nh_private_;
00121 
00122     // dynamic reconfigure
00123     CfgCtrlServer cfg_ctrl_srv_;
00124     CfgXServer cfg_x_srv_;
00125     CfgYServer cfg_y_srv_;
00126     CfgZServer cfg_z_srv_;
00127     CfgVXServer cfg_vx_srv_;
00128     CfgVYServer cfg_vy_srv_;
00129     CfgVZServer cfg_vz_srv_;
00130     CfgYawServer cfg_yaw_srv_;
00131     CfgCommServer cfg_comm_srv_;
00132 
00133     // subscriber for laser pose and velocity
00134     boost::shared_ptr<Synchronizer> sync_;
00135     boost::shared_ptr<PoseStampedSubscriber>  laser_pose_subscriber_;
00136     boost::shared_ptr<TwistStampedSubscriber> laser_vel_subscriber_;
00137 
00138     // subscriber for laser height
00139     ros::Subscriber height_subscriber_;
00140 
00141     // subscribers for direct ctrl input
00142     ros::Subscriber cmd_roll_subscriber_;
00143     ros::Subscriber cmd_pitch_subscriber_;
00144     ros::Subscriber cmd_yaw_rate_subscriber_;
00145     ros::Subscriber cmd_thrust_subscriber_;
00146     ros::Subscriber cmd_pose_subscriber_;
00147     ros::Subscriber cmd_vel_subscriber_;
00148 
00149     // publishers
00150     ros::Publisher pose_publisher_;
00151     ros::Publisher vel_publisher_;
00152     ros::Publisher imu_publisher_;
00153     ros::Publisher flight_state_publisher_;
00154     ros::Publisher battery_voltage_publisher_;
00155     ros::Publisher cpu_load_publisher_;
00156     ros::Publisher cpu_load_avg_publisher_;
00157 
00158     // debug publishers
00159     ros::Publisher debug_imu_wf_publisher_;
00160     ros::Publisher debug_cmd_roll_publisher_;
00161     ros::Publisher debug_cmd_pitch_publisher_;
00162     ros::Publisher debug_cmd_yaw_rate_publisher_;
00163     ros::Publisher debug_cmd_thrust_publisher_;
00164     ros::Publisher debug_cmd_yaw_publisher_;
00165     ros::Publisher debug_pid_x_i_term_publisher_;
00166     ros::Publisher debug_pid_y_i_term_publisher_;
00167     ros::Publisher debug_pid_yaw_i_term_publisher_;
00168     ros::Publisher debug_pid_z_i_term_publisher_;
00169     ros::Publisher debug_roll_publisher_;
00170     ros::Publisher debug_pitch_publisher_;
00171     ros::Publisher debug_yaw_publisher_;
00172     ros::Publisher debug_err_x_bf_publisher_;
00173     ros::Publisher debug_err_y_bf_publisher_;
00174     ros::Publisher debug_err_vx_bf_publisher_;
00175     ros::Publisher debug_err_vy_bf_publisher_;
00176     ros::Publisher debug_ax_bf_publisher_;
00177     ros::Publisher debug_ay_bf_publisher_;
00178     ros::Publisher debug_az_publisher_;
00179     ros::Publisher debug_vx_bf_publisher_;
00180     ros::Publisher debug_vy_bf_publisher_;
00181 
00182     tf::TransformListener    tf_listener_;
00183     tf::TransformBroadcaster tf_broadcaster_;
00184 
00185     ros::ServiceServer advance_state_srv_;
00186     ros::ServiceServer retreat_state_srv_;
00187     ros::ServiceServer toggle_engage_srv_;
00188     ros::ServiceServer takeoff_srv_;
00189     ros::ServiceServer land_srv_;
00190     ros::ServiceServer set_ctrl_type_srv_;
00191     ros::ServiceServer get_flight_state_srv_;
00192     ros::ServiceServer estop_srv_;
00193 
00194     ros::Timer cmd_timer_;
00195   
00196     // **** state variables    
00197 
00198     boost::mutex state_mutex_;
00199 
00200     bool connected_;
00201     uint8_t flight_state_;
00202 
00203     std::vector<double> cpu_loads_;
00204     int cpu_load_index_;
00205 
00206     SerialCommunication comm_;
00207 
00208     float cmd_roll_, cmd_pitch_, cmd_yaw_rate_, cmd_thrust_;
00209 
00210     MAV_TX_FREQ_CFG_PKT tx_freq_cfg_packet_;
00211     MAV_PID_CFG_PKT     pid_cfg_packet_;
00212     MAV_CTRL_CFG_PKT    ctrl_cfg_packet_;
00213   
00214     // **** parameters
00215     
00216     bool publish_debug_;
00217 
00218     bool publish_pose_; // if set to true, the pose received over the serial
00219                         // interface is published as a pose and tf
00220 
00221     int baudrate_;
00222     std::string serial_port_tx_;
00223     std::string serial_port_rx_;
00224 
00225     std::string fixed_frame_;
00226     std::string base_frame_;
00227 
00228     bool enable_kf_x_;
00229     bool enable_kf_y_;
00230     bool enable_kf_z_;
00231     bool enable_kf_yaw_;
00232 
00233     double q_x_, q_y_, q_z_, q_yaw_;     // process matric covariance
00234     double r_x_, r_y_, r_z_, r_yaw_;     // measurement matrix covariance
00235     double r_vx_, r_vy_, r_vz_, r_vz_p_; // measurement matrix covariance (velocities)
00236 
00237     // **** member functions
00238 
00239     void initializeParams();
00240 
00241     void sendCommConfig();    
00242     void sendKFConfig(bool reset);
00243     void sendCtrlConfig();
00244     void sendPIDConfig();    
00245 
00246     void processImuData(uint8_t * buf, uint32_t bufLength);
00247     void processPoseData(uint8_t * buf, uint32_t bufLength);
00248     void processRCData(uint8_t * buf, uint32_t bufLength);
00249     void processFlightStateData(uint8_t * buf, uint32_t bufLength);
00250     void processTimeSyncData(uint8_t * buf, uint32_t bufLength);
00251     void processStatusData(uint8_t * buf, uint32_t bufLength);
00252     void processCtrlDebugData(uint8_t * buf, uint32_t bufLength);
00253 
00254     void laserCallback(const PoseStamped::ConstPtr  pose_msg,
00255                        const TwistStamped::ConstPtr twist_msg);
00256     void heightCallback(const mav_msgs::Height::ConstPtr height_msg);
00257 
00258     void cmdRollCallback(const mav_msgs::Roll::ConstPtr roll_msg);
00259     void cmdPitchCallback(const mav_msgs::Pitch::ConstPtr pitch_msg);
00260     void cmdYawRateCallback(const mav_msgs::YawRate::ConstPtr yaw_rate_msg);
00261     void cmdThrustCallback(const mav_msgs::Thrust::ConstPtr thrust_msg);
00262     void cmdPoseCallback(const geometry_msgs::PoseStamped::ConstPtr cmd_pose_msg);
00263     void cmdVelCallback(const geometry_msgs::TwistStamped::ConstPtr cmd_vel_msg);
00264     
00265     void cmdTimerCallback(const ros::TimerEvent& event);
00266 
00267     bool advanceState(mav_srvs::AdvanceState::Request  &req,
00268                       mav_srvs::AdvanceState::Response &res);
00269 
00270     bool retreatState(mav_srvs::AdvanceState::Request  &req,
00271                       mav_srvs::AdvanceState::Response &res);
00272 
00273     bool getFlightState(mav_srvs::GetFlightState::Request  &req,
00274                         mav_srvs::GetFlightState::Response &res);
00275 
00276     bool estop(mav_srvs::ESTOP::Request  &req,
00277                mav_srvs::ESTOP::Response &res);
00278 
00279     bool toggleEngage(mav_srvs::ToggleEngage::Request  &req,
00280                       mav_srvs::ToggleEngage::Response &res);
00281 
00282     bool land(mav_srvs::Land::Request  &req,
00283               mav_srvs::Land::Response &res);
00284 
00285     bool takeoff(mav_srvs::Takeoff::Request  &req,
00286                  mav_srvs::Takeoff::Response &res);
00287 
00288     bool setCtrlType(mav_srvs::SetCtrlType::Request  &req,
00289                      mav_srvs::SetCtrlType::Response &res);
00290 
00291     void reconfig_x_callback(PIDXConfig& config, uint32_t level);
00292     void reconfig_y_callback(PIDYConfig& config, uint32_t level);
00293     void reconfig_z_callback(PIDZConfig& config, uint32_t level);
00294     void reconfig_vx_callback(PIDVXConfig& config, uint32_t level);
00295     void reconfig_vy_callback(PIDVYConfig& config, uint32_t level);
00296     void reconfig_vz_callback(PIDVZConfig& config, uint32_t level);
00297     void reconfig_yaw_callback(PIDYawConfig& config, uint32_t level);
00298     void reconfig_comm_callback(CommConfig& config, uint32_t level);
00299     void reconfig_ctrl_callback(CtrlConfig& config, uint32_t level);
00300 };
00301 
00302 } // end namespace mav
00303 
00304 #endif // FLYER_INTERFACE_FLYER_INTERFACE_H