00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00032 #include <ros/ros.h>
00033 #include <string>
00034 #include <iostream>
00035 #include <vector>
00036 #include <sensor_msgs/Joy.h>
00037 #include <sensor_msgs/JointState.h>
00038 #include <atlascar_base/AtlascarCommand.h>
00039
00040
00041
00042 #ifndef _ATLASCAR_XBOX_TELEOP_
00043 #define _ATLASCAR_XBOX_TELEOP_
00044
00045
00046
00047
00048 using namespace std;
00049 using namespace ros;
00050
00051
00055 class XboxTeleopAtlascar
00056 {
00057 public:
00064 XboxTeleopAtlascar(int num_but, int num_axis);
00068 ~XboxTeleopAtlascar();
00069
00071 struct TYPE_analog{
00072 int toggled;
00073 float val;
00074
00075 friend ostream& operator<<(ostream &o, const TYPE_analog& i)
00076 {
00077 o<<"toogle:"<<i.toggled<<"\nval:"<<i.val<<endl;
00078 return o;
00079 }
00080 };
00081
00083 enum{MAN_AUTO_STEER=0, MAN_AUTO_BRAKE=1, MAN_AUTO_THROTTLE=2, MAN_AUTO_CLUTCH=3, _B4=4, MAN_AUTO_IGNITION=5,\
00084 START=6, E_STOP=7, _B8=8, _B9=9, HIGH_LIGHTS=10, WARNING_LIGHTS=11, MEDIUM_LIGHTS=12,\
00085 _B13=13, STOP=14};
00087 enum{STEERING=0, _A1=1, BRAKE=2, _A3=3, CLUTCH=4, THROTTLE=5};
00088
00089
00090 private:
00092 void XboxCallback(const sensor_msgs::Joy::ConstPtr& joy);
00093
00094 ros::NodeHandle nh_teleop;
00096 vector<int> button;
00098 vector<TYPE_analog> analog;
00099
00101 double steering_max,steering_min;
00103 double brake_min,brake_max;
00105 double clutch_min,clutch_max;
00107 double throttle_min,throttle_max;
00108
00109
00111 double steering_left_ang, steering_right_ang;
00113 double m_left_ang, b_left_ang, m_right_ang, b_right_ang;
00114
00116 double speed_min, speed_max;
00118 double m_speed, b_speed;
00120 double m_brake, b_brake;
00122 int num_buttons, num_axes;
00123
00125 atlascar_base::AtlascarCommand command;
00126 ros::Publisher atlascar_cmd_;
00127 ros::Subscriber joy_sub_;
00128 ros::Publisher ptu_pub;
00129 };
00130
00131
00132 #endif