Go to the documentation of this file.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
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef JOYSTICKDEMO_H_
00036 #define JOYSTICKDEMO_H_
00037
00038 #include <ros/ros.h>
00039 #include <sensor_msgs/Joy.h>
00040 #include <std_msgs/Empty.h>
00041
00042 #include <dbw_pacifica_msgs/AcceleratorPedalCmd.h>
00043 #include <dbw_pacifica_msgs/BrakeCmd.h>
00044 #include <dbw_pacifica_msgs/SteeringCmd.h>
00045 #include <dbw_pacifica_msgs/GearCmd.h>
00046 #include <dbw_pacifica_msgs/MiscCmd.h>
00047 #include <dbw_pacifica_msgs/GlobalEnableCmd.h>
00048
00049 namespace joystick_demo
00050 {
00051
00052 typedef struct {
00053 ros::Time stamp;
00054 float brake_joy;
00055 float accelerator_pedal_joy;
00056 float steering_joy;
00057 bool steering_mult;
00058 int gear_cmd;
00059 int turn_signal_cmd;
00060 bool joy_accelerator_pedal_valid;
00061 bool joy_brake_valid;
00062 } JoystickDataStruct;
00063
00064 class JoystickDemo {
00065 public:
00066 JoystickDemo(ros::NodeHandle &node, ros::NodeHandle &priv_nh);
00067 private:
00068 void recvJoy(const sensor_msgs::Joy::ConstPtr& msg);
00069 void cmdCallback(const ros::TimerEvent& event);
00070
00071
00072 ros::Subscriber sub_joy_;
00073 ros::Publisher pub_accelerator_pedal_;
00074 ros::Publisher pub_brake_;
00075 ros::Publisher pub_steering_;
00076 ros::Publisher pub_gear_;
00077 ros::Publisher pub_misc_;
00078 ros::Publisher pub_enable_;
00079 ros::Publisher pub_disable_;
00080 ros::Publisher pub_global_enable_;
00081
00082
00083 bool ignore_;
00084 bool enable_;
00085 bool count_;
00086 double svel_;
00087
00088
00089 ros::Timer timer_;
00090 JoystickDataStruct data_;
00091 sensor_msgs::Joy joy_;
00092 uint8_t counter_;
00093
00094 enum {
00095 BTN_PARK = 3,
00096 BTN_REVERSE = 1,
00097 BTN_NEUTRAL = 2,
00098 BTN_DRIVE = 0,
00099 BTN_ENABLE = 5,
00100 BTN_DISABLE = 4,
00101 BTN_STEER_MULT_1 = 6,
00102 BTN_STEER_MULT_2 = 7,
00103 BTN_COUNT = 11,
00104 AXIS_ACCELERATOR_PEDAL = 5,
00105 AXIS_BRAKE = 2,
00106 AXIS_STEER_1 = 0,
00107 AXIS_STEER_2 = 3,
00108 AXIS_TURN_SIG = 6,
00109 AXIS_COUNT = 8,
00110 };
00111 };
00112
00113 }
00114
00115 #endif