00001 /* 00002 * Unpublished Copyright (c) 2009-2018 AutonomouStuff, LLC, All Rights Reserved. 00003 * 00004 * This file is part of the PACMod ROS 1.0 driver which is released under the MIT license. 00005 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details. 00006 */ 00007 00008 #ifndef PUBLISH_CONTROL_H 00009 #define PUBLISH_CONTROL_H 00010 00011 #include "globals.h" 00012 00013 namespace AS 00014 { 00015 namespace Joystick 00016 { 00017 00018 class PublishControl 00019 { 00020 public: 00021 // public functions 00022 PublishControl(); 00023 virtual void callback_control(const sensor_msgs::Joy::ConstPtr& msg); 00024 static void callback_veh_speed(const pacmod_msgs::VehicleSpeedRpt::ConstPtr& msg); 00025 static void callback_pacmod_enable(const std_msgs::Bool::ConstPtr& msg); 00026 00027 // public variables 00028 static JoyAxis steering_axis; 00029 static float max_rot_rad; 00030 static int vehicle_type; 00031 static GamepadType controller; 00032 static int board_rev; 00033 static double max_veh_speed; 00034 static double accel_scale_val; 00035 static double brake_scale_val; 00036 static double steering_max_speed; 00037 static std::unordered_map<JoyAxis, int, EnumHash> axes; 00038 static std::unordered_map<JoyButton, int, EnumHash> btns; 00039 static pacmod_msgs::VehicleSpeedRpt::ConstPtr last_speed_rpt; 00040 static bool pacmod_enable; 00041 static bool prev_enable; 00042 static bool last_pacmod_state; 00043 static bool accel_0_rcvd; 00044 static bool brake_0_rcvd; 00045 static int headlight_state; 00046 static bool headlight_state_change; 00047 static int wiper_state; 00048 00049 protected: 00050 virtual void check_is_enabled(const sensor_msgs::Joy::ConstPtr& msg); 00051 00052 // ROS node handle 00053 ros::NodeHandle n; 00054 00055 // ROS publishers 00056 ros::Publisher turn_signal_cmd_pub; 00057 ros::Publisher headlight_cmd_pub; 00058 ros::Publisher horn_cmd_pub; 00059 ros::Publisher wiper_cmd_pub; 00060 ros::Publisher shift_cmd_pub; 00061 ros::Publisher accelerator_cmd_pub; 00062 ros::Publisher steering_set_position_with_speed_limit_pub; 00063 ros::Publisher brake_set_position_pub; 00064 ros::Publisher enable_pub; 00065 00066 // ROS subscribers 00067 ros::Subscriber joy_sub; 00068 ros::Subscriber speed_sub; 00069 ros::Subscriber enable_sub; 00070 00071 // state vectors 00072 std::vector<float> last_axes; 00073 std::vector<int> last_buttons; 00074 00075 // Other Variables 00076 static bool local_enable; 00077 static bool recent_state_change; 00078 static uint8_t state_change_debounce_count; 00079 00080 private: 00081 // private functions 00082 virtual void publish_steering_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00083 virtual void publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00084 virtual void publish_shifting_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00085 virtual void publish_accelerator_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00086 virtual void publish_brake_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00087 virtual void publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr& msg) = 0; 00088 }; 00089 00090 } 00091 } 00092 00093 #endif