publish_control.cpp
Go to the documentation of this file.
1 /*
2 * Unpublished Copyright (c) 2009-2018 AutonomouStuff, LLC, All Rights Reserved.
3 *
4 * This file is part of the PACMod ROS 1.0 driver which is released under the MIT license.
5 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6 */
7 
8 #include "publish_control.h"
9 
10 using namespace AS::Joystick;
11 
21 std::unordered_map<JoyAxis, int, EnumHash> PublishControl::axes;
22 std::unordered_map<JoyButton, int, EnumHash> PublishControl::btns;
23 pacmod_msgs::VehicleSpeedRpt::ConstPtr PublishControl::last_speed_rpt = NULL;
25 bool PublishControl::prev_enable = false;
26 bool PublishControl::local_enable = false;
28 bool PublishControl::accel_0_rcvd = false;
29 bool PublishControl::brake_0_rcvd = false;
33 
35 {
36  // Subscribe to messages
37  joy_sub = n.subscribe("joy", 1000, &PublishControl::callback_control, this);
38  speed_sub = n.subscribe("/pacmod/parsed_tx/vehicle_speed_rpt", 20, &PublishControl::callback_veh_speed);
39 
40  // Advertise published messages
41  enable_pub = n.advertise<std_msgs::Bool>("/pacmod/as_rx/enable", 20);
42 }
43 
44 /*
45  * Called when a game controller message is received
46  */
47 void PublishControl::callback_control(const sensor_msgs::Joy::ConstPtr& msg)
48 {
49  try
50  {
51  // Only send messages when enabled, or when the state changes between enabled/disabled
52  check_is_enabled(msg);
53 
54  if (local_enable == true || local_enable != prev_enable)
55  {
56  // Steering
58 
59  // Turn signals
61 
62  // Shifting
64 
65  // Accelerator
67 
68  // Brake
70 
71  // Lights and horn
73  }
74 
76  }
77  catch (const std::out_of_range& oor)
78  {
79  ROS_ERROR("An out-of-range exception was caught. This probably means you selected the wrong controller type.");
80  }
81 
82  last_axes.clear();
83  last_axes.insert(last_axes.end(), msg->axes.begin(), msg->axes.end());
84 }
85 
86 /*
87  * Called when the node receives a message from the enable topic
88  */
89 void PublishControl::callback_pacmod_enable(const std_msgs::Bool::ConstPtr& msg)
90 {
91  if (msg->data == false &&
93  prev_enable = false;
94 
95  enable_mutex.lock();
96  pacmod_enable = msg->data;
97  enable_mutex.unlock();
98 
100 }
101 
102 /*
103  * Called when the node receives a message from the vehicle speed topic
104  */
105 void PublishControl::callback_veh_speed(const pacmod_msgs::VehicleSpeedRpt::ConstPtr& msg)
106 {
107  speed_mutex.lock();
108  last_speed_rpt = msg;
109  speed_mutex.unlock();
110 }
111 
112 void PublishControl::check_is_enabled(const sensor_msgs::Joy::ConstPtr& msg)
113 {
114  bool state_changed = false;
115 
116  enable_mutex.lock();
118  enable_mutex.unlock();
119 
121  {
122  // Enable
123  if (msg->axes[axes[DPAD_UD]] >= 0.9 && !local_enable)
124  {
125  std_msgs::Bool bool_pub_msg;
126  bool_pub_msg.data = true;
127  local_enable = true;
128  enable_pub.publish(bool_pub_msg);
129 
130  state_changed = true;
131  }
132 
133  // Disable
134  if (msg->axes[axes[DPAD_UD]] <= -0.9 && local_enable)
135  {
136  std_msgs::Bool bool_pub_msg;
137  bool_pub_msg.data = false;
138  local_enable = false;
139  enable_pub.publish(bool_pub_msg);
140 
141  state_changed = true;
142  }
143  }
144  else
145  {
146  // Enable
147  if (msg->buttons[btns[START_PLUS]] == BUTTON_DOWN && msg->buttons[btns[BACK_SELECT_MINUS]] == BUTTON_DOWN && !local_enable)
148  {
149  std_msgs::Bool bool_pub_msg;
150  bool_pub_msg.data = true;
151  local_enable = true;
152  enable_pub.publish(bool_pub_msg);
153 
154  state_changed = true;
155  }
156 
157  // Disable
158  if (msg->buttons[btns[BACK_SELECT_MINUS]] == BUTTON_DOWN && msg->buttons[btns[START_PLUS]] != BUTTON_DOWN && local_enable)
159  {
160  std_msgs::Bool bool_pub_msg;
161  bool_pub_msg.data = false;
162  local_enable = false;
163  enable_pub.publish(bool_pub_msg);
164 
165  state_changed = true;
166  }
167  }
168 
169  if (state_changed)
170  {
171  enable_mutex.lock();
173  enable_mutex.unlock();
174  }
175 }
static GamepadType controller
virtual void publish_steering_message(const sensor_msgs::Joy::ConstPtr &msg)=0
virtual void check_is_enabled(const sensor_msgs::Joy::ConstPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
static pacmod_msgs::VehicleSpeedRpt::ConstPtr last_speed_rpt
virtual void callback_control(const sensor_msgs::Joy::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static std::mutex speed_mutex
Definition: globals.h:122
static const uint16_t BUTTON_DOWN
Definition: globals.h:118
static void callback_pacmod_enable(const std_msgs::Bool::ConstPtr &msg)
static void callback_veh_speed(const pacmod_msgs::VehicleSpeedRpt::ConstPtr &msg)
static std::unordered_map< JoyButton, int, EnumHash > btns
std::vector< float > last_axes
virtual void publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr &msg)=0
static const uint16_t INVALID
Definition: globals.h:117
static std::mutex enable_mutex
Definition: globals.h:121
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void publish_brake_message(const sensor_msgs::Joy::ConstPtr &msg)=0
virtual void publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr &msg)=0
virtual void publish_shifting_message(const sensor_msgs::Joy::ConstPtr &msg)=0
static const float MAX_ROT_RAD_DEFAULT
Definition: globals.h:110
static std::unordered_map< JoyAxis, int, EnumHash > axes
virtual void publish_accelerator_message(const sensor_msgs::Joy::ConstPtr &msg)=0
#define ROS_ERROR(...)


pacmod_game_control
Author(s): Joe Driscoll
autogenerated on Mon Jun 10 2019 14:09:15