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
00031 #include <ros/ros.h>
00032 #include <string>
00033 #include <iostream>
00034 #include <vector>
00035 #include <sensor_msgs/Joy.h>
00036 #include <atlascar_base/AtlascarCommand.h>
00037
00038
00039 #ifndef _ATLASCAR_XBOX_TELEOP_
00040 #define _ATLASCAR_XBOX_TELEOP_
00041
00042 using namespace std;
00043 using namespace ros;
00044
00045
00049 class XboxTeleopAtlascar
00050 {
00051 public:
00058 XboxTeleopAtlascar(int num_but, int num_axis);
00062 ~XboxTeleopAtlascar();
00063
00065 struct TYPE_analog{
00066 int toggled;
00067 float val;
00068
00069 friend ostream& operator<<(ostream &o, const TYPE_analog& i)
00070 {
00071 o<<"toogle:"<<i.toggled<<"\nval:"<<i.val<<endl;
00072 return o;
00073 }
00074 };
00075
00077 enum{MAN_AUTO_STEER=0, MAN_AUTO_BRAKE=1, MAN_AUTO_THROTTLE=2, MAN_AUTO_CLUTCH=3, _B4=4, MAN_AUTO_IGNITION=5,\
00078 START=6, E_STOP=7, _B8=8, _B9=9, HIGH_LIGHTS=10, WARNING_LIGHTS=11, MEDIUM_LIGHTS=12,\
00079 _B13=13, STOP=14};
00081 enum{STEERING=0, _A1=1, BRAKE=2, _A3=3, CLUTCH=4, THROTTLE=5};
00082
00083
00084 private:
00085
00091 void XboxCallback(const sensor_msgs::Joy::ConstPtr& joy);
00092
00096 ros::NodeHandle nh_teleop;
00097
00101 vector<int> button;
00102
00106 vector<TYPE_analog> analog;
00107
00111 float steering_left_ang, steering_right_ang;
00115 float m_left_ang, b_left_ang, m_right_ang, b_right_ang;
00116
00120 float speed_min, speed_max;
00124 float m_speed, b_speed;
00128 float m_brake, b_brake;
00132 int num_buttons, num_axes;
00133
00137 atlascar_base::AtlascarCommand command;
00141 ros::Publisher atlascar_cmd_;
00145 ros::Subscriber joy_sub_;
00146
00147 };
00148
00149
00150 #endif