00001 00009 #ifndef EPOS_DRIVER_HPP 00010 #define EPOS_DRIVER_HPP 00011 /* ROS HEADERS*/ 00012 #include "ros/ros.h" 00013 #include "std_msgs/String.h" 00014 #include "tf/transform_broadcaster.h" 00015 /* CUSTOM HEADERS*/ 00016 #include "libepos/epos.h" 00017 /* BOOST HEADERS*/ 00018 #include <boost/thread/mutex.hpp> 00019 /* C/C++ HEADERS */ 00020 #include <string> 00021 #include <sstream> 00022 #include <signal.h> 00023 /* MSG/SRV HEADERS */ 00024 #include <epos_driver/EPOSState.h> 00025 #include <epos_driver/MoveTo.h> 00026 #include <epos_driver/MoveCycle.h> 00027 00028 // ____ _ 00029 // ___ _ __ ___ ___| _ \ _ __(_)_ _____ _ __ 00030 // / _ \ '_ \ / _ \/ __| | | | '__| \ \ / / _ \ '__| 00031 // | __/ |_) | (_) \__ \ |_| | | | |\ V / __/ | 00032 // \___| .__/ \___/|___/____/|_| |_| \_/ \___|_| 00033 // |_| 00034 00040 class eposdriver{ 00041 public: 00043 eposdriver(ros::NodeHandle parameters); 00044 /* todo do we need explicite destructor?*/ 00045 ~eposdriver(){}; 00047 int On(); 00049 int Off(); 00051 int Main(); 00052 private: 00053 // todo add radPerTick constat 00054 ros::NodeHandle nodeHandler; 00055 ros::Publisher statePublisher; 00056 std::string port; 00057 bool synchronize; 00058 bool useRadps; 00059 double pMaxVelocityRadps; 00060 double pAccelRadpss; 00061 double pDeccelRadpss; 00062 int pMaxVelocityRpm; 00063 int pAccelRpmps; 00064 int pDeccelRpmps; 00065 bool useTrapezoidal; 00066 unsigned int maxVelocity; 00067 unsigned int accel; 00068 unsigned int deccel; 00069 int motorState; 00070 //todo where is it used & why we need this? 00071 unsigned int moduleCount; 00072 //todo where is it used & why we need this? 00073 double topicFrequency; 00074 double highLimit; 00075 double lowLimit; 00076 bool moveDown; 00077 bool moveUp; 00078 bool moveSingle; 00079 std::string myFrameId; 00080 std::string parentFrameId; 00081 double sensorPoseX; 00082 double sensorPoseY; 00083 double sensorPoseZ; 00084 int tresholdTicks; 00085 int timeShift; 00086 00088 unsigned int Radps2rpm(double radps); 00090 int EposError(); 00092 int EposState(); 00094 bool MoveTo(epos_driver::MoveTo::Request &req,epos_driver::MoveTo::Response &res); 00096 bool MoveCycle(epos_driver::MoveCycle::Request &req,epos_driver::MoveCycle::Response &res); 00097 }; 00098 #endif