hl_interface.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef ASCTECINTERFACE_H_
00033 #define ASCTECINTERFACE_H_
00034 
00035 #include <ros/ros.h>
00036 #include <tf/transform_datatypes.h>
00037 #include "comm.h"
00038 
00039 // message includes
00040 #include <asctec_hl_comm/mav_rcdata.h>
00041 #include <asctec_hl_comm/mav_ctrl.h>
00042 #include <asctec_hl_comm/mav_imu.h>
00043 #include <asctec_hl_comm/mav_status.h>
00044 #include <asctec_hl_comm/GpsCustom.h>
00045 #include <sensor_msgs/Imu.h>
00046 #include <sensor_msgs/NavSatFix.h>
00047 #include <geometry_msgs/Vector3Stamped.h>
00048 
00049 // service includes
00050 #include <asctec_hl_comm/MavCtrlSrv.h>
00051 #include <asctec_hl_comm/mav_ctrl_motors.h>
00052 
00053 // dynamic reconfigure includes
00054 #include <dynamic_reconfigure/server.h>
00055 #include <asctec_hl_interface/HLInterfaceConfig.h>
00056 
00057 typedef dynamic_reconfigure::Server<asctec_hl_interface::HLInterfaceConfig> ReconfigureServer;
00058 
00059 #include <diagnostic_updater/diagnostic_updater.h>
00060 #include <diagnostic_updater/update_functions.h>
00061 
00062 /*
00063  * This class provides basic access to the HighLevel Processor of the AutoPilot Board. It subscribes to position / velocity / acceleration commands, publishes IMU, GPS, RC and status data and synchronizes the HL Processor time with the host computer.
00064  * HighLevel Processor and LowLevel Processor of the AscTec AutoPilot board are referenced to HLP and LLP in the following.
00065  */
00066 
00067 class HLInterface
00068 {
00069 private:
00070 
00071   ros::NodeHandle nh_;
00072   ros::NodeHandle pnh_;
00073   CommPtr & comm_;
00074 
00075   std::string frame_id_;
00076 
00077   ros::Publisher gps_pub_;
00078   ros::Publisher gps_custom_pub_;
00079   ros::Publisher imu_ros_pub_; 
00080   ros::Publisher imu_pub_; 
00081   ros::Publisher motors_pub_; 
00082   ros::Publisher rc_pub_;
00083   ros::Publisher status_pub_;
00084   ros::Publisher mag_pub_;
00085   ros::Subscriber control_sub_;
00086 
00087   ros::ServiceServer motor_srv_;
00088   ros::ServiceServer crtl_srv_;
00089 
00090   // callback functions for data from the serial port
00091   void processImuData(uint8_t * buf, uint32_t bufLength);
00092   void processGpsData(uint8_t * buf, uint32_t bufLength);
00093   void processRcData(uint8_t * buf, uint32_t bufLength);
00094   void processStatusData(uint8_t * buf, uint32_t bufLength);
00095   void processTimeSyncData(uint8_t * buf, uint32_t bufLength);
00096   void processPoseEKFData(uint8_t * buf, uint32_t bufLength);
00097   void processMagData(uint8_t * buf, uint32_t bufLength);
00098 
00100   bool cbMotors(asctec_hl_comm::mav_ctrl_motors::Request &req, asctec_hl_comm::mav_ctrl_motors::Response &resp);
00101 
00103   bool cbCtrl(asctec_hl_comm::MavCtrlSrv::Request & req, asctec_hl_comm::MavCtrlSrv::Response & resp);
00104 
00109   void controlCmdCallback(const asctec_hl_comm::mav_ctrlConstPtr &msg);
00110 
00112   void sendControlCmd(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result=NULL);
00113 
00115   void sendAccCommandLL(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result=NULL);
00116 
00118   void sendVelCommandLL(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result=NULL);
00119 
00121   void sendVelCommandHL(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result=NULL);
00122 
00124   void sendPosCommandHL(const asctec_hl_comm::mav_ctrl & ctrl, asctec_hl_comm::mav_ctrl * ctrl_result=NULL);
00125 
00126   int16_t gps_status_;
00127   int16_t gps_satellites_used_;
00128 
00129   double angular_velocity_variance_;
00130   double linear_acceleration_variance_;
00131   asctec_hl_comm::mav_status status_;
00132 
00134   short enable_ctrl_;
00135 
00137 
00141   int k_stick_;
00142 
00144 
00148   int k_stick_yaw_;
00149 
00150 
00151   // dynamic reconfigure
00152   ReconfigureServer *reconf_srv_;
00153   void cbConfig(asctec_hl_interface::HLInterfaceConfig & config, uint32_t level);
00154   asctec_hl_interface::HLInterfaceConfig config_;
00155 
00156   // diagnostic update
00157   diagnostic_updater::Updater diag_updater_;
00158   diagnostic_updater::FrequencyStatus diag_imu_freq_;
00159   double diag_imu_freq_min_;
00160   double diag_imu_freq_max_;
00161   void diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
00162 
00163 public:
00164   HLInterface(ros::NodeHandle & nh, CommPtr & comm);
00165   ~HLInterface();
00166 };
00167 
00168 #endif /* ASCTECINTERFACE_H_ */


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52