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
00036 #include "JoystickDemo.h"
00037
00038 namespace joystick_demo
00039 {
00040
00041 JoystickDemo::JoystickDemo(ros::NodeHandle &node, ros::NodeHandle &priv_nh) : counter_(0)
00042 {
00043 joy_.axes.resize(AXIS_COUNT, 0);
00044 joy_.buttons.resize(BTN_COUNT, 0);
00045
00046 ignore_ = false;
00047 enable_ = true;
00048 count_ = false;
00049 svel_ = 0.0;
00050 priv_nh.getParam("ignore", ignore_);
00051 priv_nh.getParam("enable", enable_);
00052 priv_nh.getParam("count", count_);
00053 priv_nh.getParam("svel", svel_);
00054
00055 sub_joy_ = node.subscribe("/joy", 1, &JoystickDemo::recvJoy, this);
00056
00057 data_.brake_joy = 0.0;
00058 data_.gear_cmd = dbw_pacifica_msgs::Gear::NONE;
00059 data_.steering_joy = 0.0;
00060 data_.steering_mult = false;
00061 data_.accelerator_pedal_joy = 0.0;
00062 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::NONE;
00063 data_.joy_accelerator_pedal_valid = false;
00064 data_.joy_brake_valid = false;
00065
00066 pub_accelerator_pedal_ = node.advertise<dbw_pacifica_msgs::AcceleratorPedalCmd>("accelerator_pedal_cmd", 1);
00067 pub_brake_ = node.advertise<dbw_pacifica_msgs::BrakeCmd>("brake_cmd", 1);
00068 pub_misc_ = node.advertise<dbw_pacifica_msgs::MiscCmd>("misc_cmd", 1);
00069 pub_steering_ = node.advertise<dbw_pacifica_msgs::SteeringCmd>("steering_cmd", 1);
00070 pub_global_enable_ = node.advertise<dbw_pacifica_msgs::GlobalEnableCmd>("global_enable_cmd", 1);
00071 pub_gear_ = node.advertise<dbw_pacifica_msgs::GearCmd>("gear_cmd", 1);
00072 if (enable_) {
00073 pub_enable_ = node.advertise<std_msgs::Empty>("enable", 1);
00074 pub_disable_ = node.advertise<std_msgs::Empty>("disable", 1);
00075 }
00076
00077 timer_ = node.createTimer(ros::Duration(0.02), &JoystickDemo::cmdCallback, this);
00078 }
00079
00080 void JoystickDemo::cmdCallback(const ros::TimerEvent& event)
00081 {
00082
00083 if (event.current_real - data_.stamp > ros::Duration(0.1)) {
00084 data_.joy_accelerator_pedal_valid = false;
00085 data_.joy_brake_valid = false;
00086 return;
00087 }
00088
00089
00090 if (count_) {
00091 counter_++;
00092 if (counter_ > 15)
00093 {
00094 counter_ = 0;
00095 }
00096 }
00097
00098
00099 dbw_pacifica_msgs::AcceleratorPedalCmd accelerator_pedal_msg;
00100 accelerator_pedal_msg.enable = true;
00101 accelerator_pedal_msg.ignore = ignore_;
00102 accelerator_pedal_msg.rolling_counter = counter_;
00103 accelerator_pedal_msg.pedal_cmd = data_.accelerator_pedal_joy * 100;
00104 accelerator_pedal_msg.control_type.value = dbw_pacifica_msgs::ActuatorControlMode::open_loop;
00105 pub_accelerator_pedal_.publish(accelerator_pedal_msg);
00106
00107
00108 dbw_pacifica_msgs::BrakeCmd brake_msg;
00109 brake_msg.enable = true;
00110 brake_msg.rolling_counter = counter_;
00111 brake_msg.pedal_cmd = data_.brake_joy * 100;
00112 brake_msg.control_type.value = dbw_pacifica_msgs::ActuatorControlMode::open_loop;
00113 pub_brake_.publish(brake_msg);
00114
00115
00116 dbw_pacifica_msgs::SteeringCmd steering_msg;
00117 steering_msg.enable = true;
00118 steering_msg.ignore = ignore_;
00119 steering_msg.rolling_counter = counter_;
00120 steering_msg.angle_cmd = data_.steering_joy;
00121 steering_msg.angle_velocity = svel_;
00122 steering_msg.control_type.value = dbw_pacifica_msgs::ActuatorControlMode::closed_loop_actuator;
00123 if (!data_.steering_mult) {
00124 steering_msg.angle_cmd *= 0.5;
00125 }
00126 pub_steering_.publish(steering_msg);
00127
00128
00129 dbw_pacifica_msgs::GearCmd gear_msg;
00130 gear_msg.cmd.gear = data_.gear_cmd;
00131 gear_msg.enable = true;
00132 gear_msg.rolling_counter = counter_;
00133 pub_gear_.publish(gear_msg);
00134
00135
00136 dbw_pacifica_msgs::MiscCmd misc_msg;
00137 misc_msg.cmd.value = data_.turn_signal_cmd;
00138 misc_msg.rolling_counter = counter_;
00139 pub_misc_.publish(misc_msg);
00140
00141 dbw_pacifica_msgs::GlobalEnableCmd globalEnable_msg;
00142 globalEnable_msg.global_enable = true;
00143 globalEnable_msg.enable_joystick_limits = true;
00144 globalEnable_msg.rolling_counter = counter_;
00145 pub_global_enable_.publish(globalEnable_msg);
00146 }
00147
00148 void JoystickDemo::recvJoy(const sensor_msgs::Joy::ConstPtr& msg)
00149 {
00150
00151 if (msg->axes.size() != (size_t)AXIS_COUNT) {
00152 ROS_ERROR("Expected %zu joy axis count, received %zu", (size_t)AXIS_COUNT, msg->axes.size());
00153 return;
00154 }
00155 if (msg->buttons.size() != (size_t)BTN_COUNT) {
00156 ROS_ERROR("Expected %zu joy button count, received %zu", (size_t)BTN_COUNT, msg->buttons.size());
00157 return;
00158 }
00159
00160
00161 if (msg->axes[AXIS_ACCELERATOR_PEDAL] != 0.0) {
00162 data_.joy_accelerator_pedal_valid = true;
00163 }
00164 if (msg->axes[AXIS_BRAKE] != 0.0) {
00165 data_.joy_brake_valid = true;
00166 }
00167
00168
00169 if (data_.joy_accelerator_pedal_valid) {
00170 data_.accelerator_pedal_joy = 0.5 - 0.5 * msg->axes[AXIS_ACCELERATOR_PEDAL];
00171 }
00172
00173
00174 if (data_.joy_brake_valid) {
00175 data_.brake_joy = 0.5 - 0.5 * msg->axes[AXIS_BRAKE];
00176 }
00177
00178
00179 if (msg->buttons[BTN_PARK]) {
00180 data_.gear_cmd = dbw_pacifica_msgs::Gear::PARK;
00181 } else if (msg->buttons[BTN_REVERSE]) {
00182 data_.gear_cmd = dbw_pacifica_msgs::Gear::REVERSE;
00183 } else if (msg->buttons[BTN_DRIVE]) {
00184 data_.gear_cmd = dbw_pacifica_msgs::Gear::DRIVE;
00185 } else if (msg->buttons[BTN_NEUTRAL]) {
00186 data_.gear_cmd = dbw_pacifica_msgs::Gear::NEUTRAL;
00187 } else {
00188 data_.gear_cmd = dbw_pacifica_msgs::Gear::NONE;
00189 }
00190
00191
00192 data_.steering_joy = 470.0 * M_PI / 180.0 * ((fabs(msg->axes[AXIS_STEER_1]) > fabs(msg->axes[AXIS_STEER_2])) ? msg->axes[AXIS_STEER_1] : msg->axes[AXIS_STEER_2]);
00193 data_.steering_mult = msg->buttons[BTN_STEER_MULT_1] || msg->buttons[BTN_STEER_MULT_2];
00194
00195
00196 if (msg->axes[AXIS_TURN_SIG] != joy_.axes[AXIS_TURN_SIG]) {
00197 switch (data_.turn_signal_cmd) {
00198 case dbw_pacifica_msgs::TurnSignal::NONE:
00199 if (msg->axes[AXIS_TURN_SIG] < -0.5) {
00200 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::RIGHT;
00201 } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
00202 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::LEFT;
00203 }
00204 break;
00205 case dbw_pacifica_msgs::TurnSignal::LEFT:
00206 if (msg->axes[AXIS_TURN_SIG] < -0.5) {
00207 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::RIGHT;
00208 } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
00209 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::NONE;
00210 }
00211 break;
00212 case dbw_pacifica_msgs::TurnSignal::RIGHT:
00213 if (msg->axes[AXIS_TURN_SIG] < -0.5) {
00214 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::NONE;
00215 } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
00216 data_.turn_signal_cmd = dbw_pacifica_msgs::TurnSignal::LEFT;
00217 }
00218 break;
00219 }
00220 }
00221
00222
00223 if (enable_) {
00224 const std_msgs::Empty empty;
00225 if (msg->buttons[BTN_ENABLE]) {
00226 pub_enable_.publish(empty);
00227 }
00228 if (msg->buttons[BTN_DISABLE]) {
00229 pub_disable_.publish(empty);
00230 }
00231 }
00232
00233 data_.stamp = ros::Time::now();
00234 joy_ = *msg;
00235 }
00236
00237 }