rb1_base_pad.cpp
Go to the documentation of this file.
1 /*
2  * rb1_base_pad
3  * Copyright (c) 2012, Robotnik Automation, SLL
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the Robotnik Automation, SLL. nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  *
30  * \author Robotnik Automation, SLL
31  * \brief Allows to use a pad with the roboy controller, sending the messages received from the joystick device
32  */
33 
34 
35 #include <ros/ros.h>
36 #include <sensor_msgs/Joy.h>
37 #include <geometry_msgs/Twist.h>
38 #include <robotnik_msgs/ptz.h>
39 // Not yet catkinized 9/2013
40 // #include <sound_play/sound_play.h>
41 #include <unistd.h>
42 #include <robotnik_msgs/set_mode.h>
43 #include <rb1_base_pad/enable_disable_pad.h>
44 #include <robotnik_msgs/set_digital_output.h>
45 #include <robotnik_msgs/ptz.h>
46 #include <robotnik_msgs/home.h>
49 #include <robotnik_msgs/SetElevator.h>
50 #include <robotnik_msgs/ElevatorAction.h>
51 
52 #define DEFAULT_NUM_OF_BUTTONS 16
53 #define DEFAULT_AXIS_LINEAR_X 1
54 #define DEFAULT_AXIS_LINEAR_Y 0
55 #define DEFAULT_AXIS_ANGULAR 2
56 #define DEFAULT_AXIS_LINEAR_Z 3
57 #define DEFAULT_SCALE_LINEAR 1.0
58 #define DEFAULT_SCALE_ANGULAR 2.0
59 #define DEFAULT_SCALE_LINEAR_Z 1.0
60 
61 #define ITERATIONS_AFTER_DEADMAN 3.0
62 
63 #define JOY_ERROR_TIME 1.0
64 
71 
72 
73 class RB1BasePad
74 {
75  public:
76  RB1BasePad();
77  void Update();
78 
79  private:
80  void padCallback(const sensor_msgs::Joy::ConstPtr& joy);
81  bool EnableDisablePad(rb1_base_pad::enable_disable_pad::Request &req, rb1_base_pad::enable_disable_pad::Response &res );
82 
84 
92  std::string cmd_topic_vel_;
94  std::string cmd_service_io_;
98  std::string cmd_topic_ptz_;
101  double current_vel;
117  std::string cmd_set_mode_;
123  std::string cmd_home_;
127  // ros::ServiceServer enable_disable_srv_;
128 
137  // DIAGNOSTICS
149  // bool bEnable;
151  // sound_play::SoundClient sc;
154 };
155 
156 
157 RB1BasePad::RB1BasePad():
158  linear_x_(1),
159  linear_y_(0),
160  angular_(2),
161  linear_z_(3)
162 {
163  current_vel = 0.1;
164  //
165  nh_.param("num_of_buttons", num_of_buttons_, DEFAULT_NUM_OF_BUTTONS);
166 
167  // MOTION CONF
168  nh_.param("axis_linear_x", linear_x_, DEFAULT_AXIS_LINEAR_X);
169  nh_.param("axis_linear_y", linear_y_, DEFAULT_AXIS_LINEAR_Y);
170  nh_.param("axis_linear_z", linear_z_, DEFAULT_AXIS_LINEAR_Z);
171  nh_.param("axis_angular", angular_, DEFAULT_AXIS_ANGULAR);
172  nh_.param("scale_angular", a_scale_, DEFAULT_SCALE_ANGULAR);
173  nh_.param("scale_linear", l_scale_, DEFAULT_SCALE_LINEAR);
174  nh_.param("scale_linear_z", l_scale_z_, DEFAULT_SCALE_LINEAR_Z);
175  nh_.param("cmd_topic_vel", cmd_topic_vel_, cmd_topic_vel_);
176  nh_.param("button_dead_man", dead_man_button_, dead_man_button_);
177  nh_.param("button_speed_up", speed_up_button_, speed_up_button_); //4 Thrustmaster
178  nh_.param("button_speed_down", speed_down_button_, speed_down_button_); //5 Thrustmaster
179 
180  // DIGITAL OUTPUTS CONF
181  nh_.param("cmd_service_io", cmd_service_io_, cmd_service_io_);
182  nh_.param("button_output_1", button_output_1_, button_output_1_);
183  nh_.param("button_output_2", button_output_2_, button_output_2_);
184  nh_.param("output_1", output_1_, output_1_);
185  nh_.param("output_2", output_2_, output_2_);
186  // PANTILT CONF
187  nh_.param("cmd_topic_ptz", cmd_topic_ptz_, cmd_topic_ptz_);
188  nh_.param("button_ptz_tilt_up", ptz_tilt_up_, ptz_tilt_up_);
189  nh_.param("button_ptz_tilt_down", ptz_tilt_down_, ptz_tilt_down_);
190  nh_.param("button_ptz_pan_right", ptz_pan_right_, ptz_pan_right_);
191  nh_.param("button_ptz_pan_left", ptz_pan_left_, ptz_pan_left_);
192  nh_.param("button_home", button_home_, button_home_);
193  nh_.param("pan_increment", pan_increment_, 1);
194  nh_.param("tilt_increment",tilt_increment_, 1);
195  nh_.param("button_lower_elevator",button_lower_elevator_, 6);
196  nh_.param("button_raise_elevator",button_raise_elevator_, 4);
197  nh_.param("button_stop_elevator",button_stop_elevator_, 16);
198  nh_.param("axis_elevator",axis_elevator_, 1);
199  nh_.param("cmd_service_home", cmd_home_, cmd_home_);
200  nh_.param("check_message_timeout", check_message_timeout_, check_message_timeout_);
201  nh_.param<std::string>("elevator_service_name", elevator_service_name_, "set_elevator");
202 
203  ROS_INFO("RB1BasePad num_of_buttons_ = %d", num_of_buttons_);
204  for(int i = 0; i < num_of_buttons_; i++){
205  bRegisteredButtonEvent[i] = false;
206  ROS_INFO("bREG %d", i);
207  }
208 
209  // Publish through the node handle Twist type messages to the guardian_controller/command topic
210  vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_topic_vel_, 1);
211  // Publishes msgs for the pant-tilt cam
212  ptz_pub_ = nh_.advertise<robotnik_msgs::ptz>(cmd_topic_ptz_, 1);
213 
214  // Listen through the node handle sensor_msgs::Joy messages from joystick
215  // (these are the references that we will sent to cmd_vel)
216  pad_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &RB1BasePad::padCallback, this);
217 
218  // Request service to activate / deactivate digital I/O
219  set_digital_outputs_client_ = nh_.serviceClient<robotnik_msgs::set_digital_output>(cmd_service_io_);
220  set_elevator_client_ = nh_.serviceClient<robotnik_msgs::SetElevator>(elevator_service_name_);
221 
222  bOutput1 = bOutput2 = false;
223 
224  // Request service to start homing
225  doHome = nh_.serviceClient<robotnik_msgs::home>(cmd_home_);
226 
227  // Diagnostics
228  updater_pad.setHardwareID("None");
229  // Topics freq control
234 
237 
238  // Advertises new service to enable/disable the pad
239  // enable_disable_srv_ = nh_.advertiseService("/rb1_base_pad/enable_disable_pad", &RB1BasePad::EnableDisablePad, this);
240  // bEnable = true; // Communication flag enabled by default
241 
242 }
243 
244 
245 /*
246  * \brief Updates the diagnostic component. Diagnostics
247  *
248  */
251 }
252 
253 /*
254  * \brief Enables/Disables the pad
255  *
256  */
257 /*
258 bool RB1BasePad::EnableDisablePad(rb1_base_pad::enable_disable_pad::Request &req, rb1_base_pad::enable_disable_pad::Response &res )
259 {
260  bEnable = req.value;
261 
262  ROS_INFO("RB1BasePad::EnablaDisablePad: Setting to %d", req.value);
263  res.ret = true;
264  return true;
265 }
266 */
267 
268 void RB1BasePad::padCallback(const sensor_msgs::Joy::ConstPtr& joy)
269 {
270  geometry_msgs::Twist vel;
271  robotnik_msgs::ptz ptz;
272  bool ptzEvent = false;
273  static int send_iterations_after_dead_man = 0;
274 
275  // Checks the ROS time to avoid noise in the pad
276  if(check_message_timeout_ && ((ros::Time::now() - joy->header.stamp).toSec() > JOY_ERROR_TIME))
277  return;
278 
279  vel.angular.x = 0.0; vel.angular.y = 0.0; vel.angular.z = 0.0;
280  vel.linear.x = 0.0; vel.linear.y = 0.0; vel.linear.z = 0.0;
281 
282  // Actions dependant on dead-man button
283  if (joy->buttons[dead_man_button_] == 1) {
284  //ROS_ERROR("RB1BasePad::padCallback: DEADMAN button %d", dead_man_button_);
285  // Set the current velocity level
286  if ( joy->buttons[speed_down_button_] == 1 ){
288  if(current_vel > 0.1){
289  current_vel = current_vel - 0.1;
291  ROS_INFO("Velocity: %f%%", current_vel*100.0);
292  char buf[50]="\0";
293  int percent = (int) (current_vel*100.0);
294  sprintf(buf," %d percent", percent);
295  // sc.say(buf);
296  }
297  }else{
299  }
300 
301  if (joy->buttons[speed_up_button_] == 1){
303  if(current_vel < 0.9){
304  current_vel = current_vel + 0.1;
306  ROS_INFO("Velocity: %f%%", current_vel*100.0);
307  char buf[50]="\0";
308  int percent = (int) (current_vel*100.0);
309  sprintf(buf," %d percent", percent);
310  // sc.say(buf);
311  }
312 
313  }else{
315  }
316 
317  vel.angular.x = current_vel*(a_scale_*joy->axes[angular_]);
318  vel.angular.y = current_vel*(a_scale_*joy->axes[angular_]);
319  vel.angular.z = current_vel*(a_scale_*joy->axes[angular_]);
320  vel.linear.x = current_vel*l_scale_*joy->axes[linear_x_];
321  vel.linear.y = current_vel*l_scale_*joy->axes[linear_y_];
322  vel.linear.z = current_vel*l_scale_z_*joy->axes[linear_z_];
323 
324  // ELEVATOR
325  if (joy->axes[axis_elevator_]>0.99){
326  //ROS_INFO("RB1BasePad::padCallback: button %d calling service:%s RAISE", button_stop_elevator_,elevator_service_name_.c_str());
327  robotnik_msgs::SetElevator elevator_msg_srv;
328 
329  elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::RAISE;
330  set_elevator_client_.call( elevator_msg_srv );
331 
332  }
333 
334  if (joy->axes[axis_elevator_]<-0.99){
335  //ROS_INFO("RB1BasePad::padCallback: button %d calling service:%s LOWER", button_stop_elevator_,elevator_service_name_.c_str());
336  robotnik_msgs::SetElevator elevator_msg_srv;
337 
338  elevator_msg_srv.request.action.action = robotnik_msgs::ElevatorAction::LOWER;
339  set_elevator_client_.call( elevator_msg_srv );
340  }
341  }
342  else {
343  vel.angular.x = 0.0; vel.angular.y = 0.0; vel.angular.z = 0.0;
344  vel.linear.x = 0.0; vel.linear.y = 0.0; vel.linear.z = 0.0;
345  }
346 
347  sus_joy_freq->tick(); // Ticks the reception of joy events
348 
349  // Publish only with deadman button pushed for twist use
350  if (joy->buttons[dead_man_button_] == 1) {
351  send_iterations_after_dead_man = ITERATIONS_AFTER_DEADMAN;
352  if (ptzEvent) ptz_pub_.publish(ptz);
353  vel_pub_.publish(vel);
355  } else { // send some 0 if deadman is released
356  if (send_iterations_after_dead_man >0) {
357  send_iterations_after_dead_man--;
358  vel_pub_.publish(vel);
360  }
361  }
362 }
363 
364 
365 int main(int argc, char** argv)
366 {
367  ros::init(argc, argv, "rb1_base_pad");
368  RB1BasePad rb1_base_pad;
369 
370  ros::Rate r(50.0);
371 
372  while( ros::ok() ){
373  // UPDATING DIAGNOSTICS
374  rb1_base_pad.Update();
375  ros::spinOnce();
376  r.sleep();
377  }
378 }
379 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
int main(int argc, char **argv)
int output_1_
double current_vel
bool bOutput2
int linear_y_
int kinematic_mode_
kinematic mode
int pan_increment_
Flag to enable/disable the communication with the publishers topics.
int ptz_tilt_up_
buttons to the pan-tilt-zoom camera
double a_scale_
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber pad_sub_
It will be suscribed to the joystick.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int linear_x_
diagnostic_updater::HeaderlessTopicDiagnostic * pub_command_freq
Diagnostic to control the frequency of the published command velocity topic.
void setHardwareID(const std::string &hwid)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
diagnostic_updater::Updater updater_pad
General status diagnostic updater.
bool call(MReq &req, MRes &res)
#define DEFAULT_SCALE_LINEAR
double max_freq_joy
diagnostic_updater::HeaderlessTopicDiagnostic * sus_joy_freq
Diagnostic to control the reception frequency of the subscribed joy topic.
int button_output_2_
ros::NodeHandle nh_
ros::ServiceClient set_elevator_client_
Service to activate the elevator.
double l_scale_
int ptz_pan_left_
void Update()
int axis_elevator_
int button_stop_elevator_
int speed_up_button_
Number of the button for increase or decrease the speed max of the joystick.
int num_of_buttons_
Number of buttons of the joystick.
ros::ServiceClient set_digital_outputs_client_
Enables/disables the pad.
int ptz_tilt_down_
std::string cmd_topic_ptz_
Name of the topic where it will be publishing the pant-tilt values.
bool bOutput1
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define DEFAULT_NUM_OF_BUTTONS
ROSCPP_DECL bool ok()
ros::Publisher vel_pub_
It will publish into command velocity (for the robot) and the ptz_state (for the pantilt) ...
std::string cmd_topic_vel_
Name of the topic where it will be publishing the velocity.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string elevator_service_name_
Name of the service where to actuate the elevator.
int ptz_pan_right_
int button_home_
button to start the homing service
bool bRegisteredButtonEvent[DEFAULT_NUM_OF_BUTTONS]
Pointer to a vector for controlling the event when pushing the buttons.
double l_scale_z_
ros::Publisher ptz_pub_
bool check_message_timeout_
If it is True, it will check the timeout message.
std::string cmd_service_io_
Name of the service where it will be modifying the digital outputs.
int angular_
bool sleep()
std::string cmd_home_
Name of the service to do the homing.
int button_raise_elevator_
void padCallback(const sensor_msgs::Joy::ConstPtr &joy)
ros::ServiceClient doHome
Service to start homing.
#define DEFAULT_AXIS_LINEAR_X
int linear_z_
#define ITERATIONS_AFTER_DEADMAN
std::string cmd_set_mode_
Name of the service to change the mode.
static Time now()
int speed_down_button_
bool EnableDisablePad(rb1_base_pad::enable_disable_pad::Request &req, rb1_base_pad::enable_disable_pad::Response &res)
#define DEFAULT_SCALE_ANGULAR
#define DEFAULT_AXIS_LINEAR_Z
int button_output_1_
double min_freq_command
Diagnostics min freq.
#define DEFAULT_AXIS_LINEAR_Y
int tilt_increment_
int button_kinematic_mode_
button to change kinematic mode
ROSCPP_DECL void spinOnce()
double min_freq_joy
#define DEFAULT_SCALE_LINEAR_Z
#define DEFAULT_AXIS_ANGULAR
double max_freq_command
Diagnostics max freq.
ros::ServiceClient setKinematicMode
Service to modify the kinematic mode.
int button_lower_elevator_
#define JOY_ERROR_TIME
int output_2_
int dead_man_button_
Number of the DEADMAN button.


rb1_base_pad
Author(s): Roberto Guzmán
autogenerated on Sun Dec 6 2020 03:58:49