00001 00006 #ifndef YOCS_KEYOP_KEYOP_HPP_ 00007 #define YOCS_KEYOP_KEYOP_HPP_ 00008 00009 #include <ros/ros.h> 00010 #include <termios.h> // for keyboard input 00011 #include <ecl/threads.hpp> 00012 #include <geometry_msgs/Twist.h> // for velocity commands 00013 #include <geometry_msgs/TwistStamped.h> // for velocity commands 00014 00015 00016 namespace yocs_keyop 00017 { 00022 class KeyOp 00023 { 00024 public: 00025 /********************* 00026 ** C&D 00027 **********************/ 00028 KeyOp(); 00029 ~KeyOp(); 00030 bool init(); 00031 00032 /********************* 00033 ** Runtime 00034 **********************/ 00035 void spin(); 00036 00037 private: 00038 ros::Publisher velocity_publisher_, enable_motors_publisher_, disable_motors_publisher_; 00039 bool last_zero_vel_sent_; 00040 bool accept_incoming_; 00041 bool power_status_; 00042 bool wait_for_connection_; 00043 geometry_msgs::TwistPtr cmd_; 00044 geometry_msgs::TwistStampedPtr cmd_stamped_; 00045 double linear_vel_step_, linear_vel_max_; 00046 double angular_vel_step_, angular_vel_max_; 00047 std::string name_; 00048 00049 /********************* 00050 ** Commands 00051 **********************/ 00052 void enable(); 00053 void disable(); 00054 void incrementLinearVelocity(); 00055 void decrementLinearVelocity(); 00056 void incrementAngularVelocity(); 00057 void decrementAngularVelocity(); 00058 void resetVelocity(); 00059 00060 /********************* 00061 ** Keylogging 00062 **********************/ 00063 00064 void keyboardInputLoop(); 00065 void processKeyboardInput(char c); 00066 void restoreTerminal(); 00067 bool quit_requested_; 00068 int key_file_descriptor_; 00069 struct termios original_terminal_state_; 00070 ecl::Thread thread_; 00071 }; 00072 00073 } // namespace yocs_keyop 00074 00075 #endif /* YOCS_KEYOP_KEYOP_HPP_ */