asctec_proc.h
Go to the documentation of this file.00001 #ifndef ASCTEC_PROC_ASCTEC_PROC_H 00002 #define ASCTEC_PROC_ASCTEC_PROC_H 00003 00004 #include <ros/ros.h> 00005 #include <std_msgs/Float64.h> 00006 #include <std_msgs/Bool.h> 00007 #include <mav_msgs/common.h> 00008 #include <mav_msgs/State.h> 00009 #include <mav_msgs/Height.h> 00010 #include <sensor_msgs/Imu.h> 00011 #include <asctec_msgs/common.h> 00012 #include <asctec_msgs/IMUCalcData.h> 00013 #include <asctec_msgs/CtrlInput.h> 00014 #include <asctec_msgs/LLStatus.h> 00015 #include <boost/thread/mutex.hpp> 00016 #include <tf/transform_datatypes.h> 00017 00018 #include <mav_msgs/SetMotorsOnOff.h> 00019 #include <mav_msgs/GetMotorsOnOff.h> 00020 00021 namespace asctec 00022 { 00023 00024 // **** conversion units 00025 00026 const double ASC_TO_ROS_ANGLE = (1.0 / 1000.0) * 3.14159265 / 180.0; // converts to rad 00027 const double ASC_TO_ROS_ANGVEL = (1.0 / 64.8) * 3.14159265 / 180.0; // convetts to rad/s 00028 const double ASC_TO_ROS_ACC = (1.0 / 10000.0) * 9.81; // converts to m/s^s 00029 const double ASC_TO_ROS_HEIGHT = (1.0 / 1000.0); // converts to m 00030 00031 // from asctec CtrlInput definitions 00032 const double ROS_TO_ASC_THRUST = 4095.0; // converts from [ 0, 1] to thrust stick counts 00033 const double ROS_TO_ASC_ROLL = 2047.0; // converts from [-1, 1] to roll stick counts 00034 const double ROS_TO_ASC_PITCH = 2047.0; // converts from [-1, 1] to pitch stick counts 00035 00036 // Per email from AscTec, 00037 // """The standard parameter for K_stick_yaw is 120, resulting in a maximum rate of 00038 // 254.760 degrees per second = 4.43 rad/s. I.e. a 360° turn takes about 1.5 seconds.""" 00039 const double ROS_TO_ASC_YAW_RATE = 2047.0/4.43; // converts from rad/s to yaw_rate stick counts 00040 00041 class AsctecProc 00042 { 00043 private: 00044 00045 // **** ROS-related 00046 ros::NodeHandle nh_; 00047 ros::NodeHandle nh_private_; 00048 00049 ros::Subscriber cmd_thrust_subscriber_; 00050 ros::Subscriber cmd_roll_subscriber_; 00051 ros::Subscriber cmd_pitch_subscriber_; 00052 ros::Subscriber cmd_yaw_subscriber_; 00053 ros::Subscriber ll_status_subscriber_; 00054 ros::Subscriber imu_calcdata_subscriber_; 00055 ros::Subscriber state_subscriber_; 00056 ros::Subscriber estop_subscriber_; 00057 00058 ros::Publisher imu_publisher_; 00059 ros::Publisher height_publisher_; 00060 ros::Publisher height_filtered_publisher_; 00061 ros::Publisher ctrl_input_publisher_; 00062 00063 ros::ServiceServer set_motors_on_off_srv_; 00064 ros::ServiceServer get_motors_on_off_srv_; 00065 00066 // **** state variables 00067 00068 boost::mutex state_mutex_; 00069 00070 int ctrl_roll_; 00071 int ctrl_pitch_; 00072 int ctrl_yaw_; 00073 int ctrl_thrust_; 00074 00075 asctec_msgs::CtrlInputPtr ctrl_input_toggle_msg_; // stick to the lower left 00076 asctec_msgs::CtrlInputPtr ctrl_input_zero_msg_; // zero message (sticks centered) 00077 00078 bool motors_on_; 00079 bool engaging_; 00080 00081 // **** parameters 00082 00083 bool enable_ctrl_thrust_; 00084 bool enable_ctrl_roll_; 00085 bool enable_ctrl_pitch_; 00086 bool enable_ctrl_yaw_; 00087 00088 bool enable_state_changes_; // if true, allow motor on/off service 00089 00090 int max_ctrl_thrust_; // max output - in asctec units 00091 int max_ctrl_roll_; 00092 int max_ctrl_pitch_; 00093 int max_ctrl_yaw_; 00094 00095 // **** member functions 00096 00097 void initializeParams(); 00098 void assembleCtrlCommands(); 00099 00100 void cmdThrustCallback(const std_msgs::Float64ConstPtr& cmd_thrust_msg); 00101 void cmdRollCallback (const std_msgs::Float64ConstPtr& cmd_roll_msg); 00102 void cmdPitchCallback (const std_msgs::Float64ConstPtr& cmd_pitch_msg); 00103 void cmdYawCallback (const std_msgs::Float64ConstPtr& cmd_yaw_rate_msg); 00104 //void stateCallback (const mav_msgs::StatePtr& state_msg); 00105 void llStatusCallback (const asctec_msgs::LLStatusPtr& ll_status_msg); 00106 void imuCalcDataCallback(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg); 00107 00108 void createImuMsg (const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00109 sensor_msgs::ImuPtr& imu_msg); 00110 void createHeightMsg (const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00111 mav_msgs::HeightPtr& height_msg); 00112 void createHeightFilteredMsg(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00113 mav_msgs::HeightPtr& height_filtered_msg); 00114 00115 void startMotors(); 00116 void stopMotors(); 00117 void publishCtrlInputMsg(); 00118 00119 bool setMotorsOnOff(mav_msgs::SetMotorsOnOff::Request &req, 00120 mav_msgs::SetMotorsOnOff::Response &res); 00121 bool getMotorsOnOff(mav_msgs::GetMotorsOnOff::Request &req, 00122 mav_msgs::GetMotorsOnOff::Response &res); 00123 00124 public: 00125 00126 AsctecProc(ros::NodeHandle nh, ros::NodeHandle nh_private); 00127 virtual ~AsctecProc(); 00128 00129 }; 00130 00131 } // end namespace asctec 00132 00133 #endif //ASCTEC_PROC_ASCTEC_PROC_H