00001 #ifndef QUADROTOR_HANDLER_H 00002 #define QUADROTOR_HANDLER_H 00003 00004 #include "vrep_ros_plugin/GenericObjectHandler.h" 00005 00006 #include "vrep_ros_plugin/access.h" 00007 #include <ros/time.h> 00008 #include <sensor_msgs/JointState.h> 00009 00013 class QuadrotorHandler : public GenericObjectHandler 00014 { 00015 public: 00016 QuadrotorHandler(); 00017 ~QuadrotorHandler(); 00018 00019 void synchronize(); 00020 void handleSimulation(); 00021 unsigned int getObjectType() const; 00022 00023 protected: 00024 00025 void _initialize(); 00026 00030 float _torqueToForceRatio; 00031 00035 int _handleOfJoint[4]; 00036 00040 int _handleOfCoM; 00041 00045 simFloat _quadrotorMass; 00046 00050 simFloat _jointPosition[4][3]; 00051 00055 ros::Publisher _pubPose; 00059 ros::Publisher _pubTwist; 00060 00061 00065 ros::Publisher _pubIMU; 00066 00070 ros::Subscriber _sub; 00071 00075 ros::Time _lastReceivedCmdTime; 00076 00080 ros::Time _lastPrintedMsg; 00081 00085 sensor_msgs::JointState _lastReceivedCmd; 00086 00090 std::vector<double> _Commands; 00091 00095 simFloat _lastPublishedStatusTime; 00096 00101 double _ObjStatusFrequency; 00102 00107 void CommandsCallback(const sensor_msgs::JointStateConstPtr& msg); 00108 00109 00110 }; 00111 00112 00113 #endif // ndef QUADROTOR_HANDLER_H