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 #include <ros/ros.h>
00036 #include <sensor_msgs/Joy.h>
00037 #include <geometry_msgs/Twist.h>
00038 #include <std_msgs/Int32.h>
00039 #include <unistd.h>
00040 #include <vector>
00041 #include <robotnik_msgs/enable_disable.h>
00042 #include <robotnik_msgs/set_digital_output.h>
00043 #include "diagnostic_msgs/DiagnosticStatus.h"
00044 #include "diagnostic_updater/diagnostic_updater.h"
00045 #include "diagnostic_updater/update_functions.h"
00046 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00047 #include "diagnostic_updater/publisher.h"
00048
00049
00050 #include "ackermann_msgs/AckermannDriveStamped.h"
00051 #include <std_srvs/Empty.h>
00052
00053 #define DEFAULT_MAX_SKID_LINEAR_SPEED 2.0 //m/s
00054 #define DEFAULT_MAX_ANGULAR_POSITION 2.0 // rads/s
00055
00056 #define MAX_NUM_OF_BUTTONS 16
00057 #define MAX_NUM_OF_AXES 8
00058 #define MAX_NUM_OF_BUTTONS_PS3 19
00059 #define MAX_NUM_OF_AXES_PS3 20
00060
00061 #define DEFAULT_NUM_OF_BUTTONS 16
00062 #define DEFAULT_NUM_OF_AXES 8
00063
00064 #define DEFAULT_AXIS_LINEAR_X 1
00065 #define DEFAULT_AXIS_ANGULAR 0
00066 #define DEFAULT_SCALE_LINEAR 1.0
00067 #define DEFAULT_SCALE_ANGULAR 1.0
00068
00069 #define DEFAULT_JOY_TIMEOUT 0.5
00070
00071
00072 #define DEFAULT_JOY "/joy"
00073
00074 #define DEFAULT_HZ 50.0
00075
00077 class Button{
00078 int iPressed;
00079 bool bReleased;
00080
00081 public:
00082
00083 Button(){
00084 iPressed = 0;
00085 bReleased = false;
00086 }
00088 void Press(int value){
00089 if(iPressed and !value){
00090 bReleased = true;
00091
00092 }else if(bReleased and value)
00093 bReleased = false;
00094
00095 iPressed = value;
00096
00097 }
00098
00099 int IsPressed(){
00100 return iPressed;
00101 }
00102
00103 bool IsReleased(){
00104 bool b = bReleased;
00105 bReleased = false;
00106 return b;
00107 }
00108 };
00109
00111
00113 class AgvsPad
00114 {
00115 public:
00116
00117 AgvsPad();
00118
00119 void ControlLoop();
00120 int SetStateMode(int state, int arm_mode, int platform_mode);
00121
00122 private:
00123
00124 void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
00125
00126 char * StateToString(int state);
00127 int SwitchToState(int new_state);
00128
00129 void PublishState();
00131 bool EnableDisable(robotnik_msgs::enable_disable::Request &req, robotnik_msgs::enable_disable::Response &res );
00132 void Update();
00133
00134 private:
00135
00136 ros::NodeHandle nh_;
00137
00138 int axis_linear_speed_, axis_angular_position_;
00139 double l_scale_, a_scale_;
00140 double current_speed_lvl;
00142 double max_linear_speed_, max_angular_position_;
00144 double desired_freq_;
00145
00146
00148 ros::Publisher vel_pub_;
00149
00151 ros::Subscriber joy_sub_;
00153 std::string joy_topic_;
00155 std::string cmd_topic_vel;
00157 std::string cmd_service_io_;
00159 std::string topic_state_;
00161 ros::Publisher state_pub_;
00162
00164 std::string service_raise_elevator_;
00166 std::string service_lower_elevator_;
00167
00168
00170 ros::ServiceServer enable_disable_srv_;
00171 ros::ServiceClient set_digital_outputs_client_;
00172 ros::ServiceClient raise_elevator_client_;
00173 ros::ServiceClient lower_elevator_client_;
00174
00175
00176
00178 int num_of_buttons_;
00179 int num_of_axes_;
00180
00182 std::vector<float> fAxes;
00184 std::vector<Button> vButtons;
00186 int button_dead_man_;
00188 int button_speed_up_, button_speed_down_;
00189 int button_up_car_, button_down_car_;
00190 int output_1_, output_2_;
00191 bool bOutput1, bOutput2;
00192
00193
00195 diagnostic_updater::HeaderlessTopicDiagnostic *pub_command_freq;
00197 diagnostic_updater::HeaderlessTopicDiagnostic *sus_joy_freq;
00199 diagnostic_updater::Updater updater_pad;
00201 double min_freq_command, min_freq_joy;
00203 double max_freq_command, max_freq_joy;
00205 bool bEnable;
00206 };
00207
00208
00209 AgvsPad::AgvsPad():
00210 axis_linear_speed_(1),
00211 axis_angular_position_(2), nh_("~")
00212 {
00213
00214 current_speed_lvl = 0.1;
00215
00216 nh_.param("num_of_buttons", num_of_buttons_, DEFAULT_NUM_OF_BUTTONS);
00217 nh_.param("num_of_axes", num_of_axes_, DEFAULT_NUM_OF_AXES);
00218 nh_.param("desired_freq", desired_freq_, DEFAULT_HZ);
00219
00220 if(num_of_axes_ > MAX_NUM_OF_AXES){
00221 num_of_axes_ = MAX_NUM_OF_AXES;
00222 ROS_INFO("AgvsPad::AgvsPad: Limiting the max number of axes to %d", MAX_NUM_OF_AXES);
00223 }
00224 if(num_of_buttons_ > MAX_NUM_OF_BUTTONS){
00225 num_of_buttons_ = MAX_NUM_OF_BUTTONS;
00226 ROS_INFO("AgvsPad::AgvsPad: Limiting the max number of buttons to %d", MAX_NUM_OF_BUTTONS);
00227 }
00228
00229 nh_.param("topic_joy", joy_topic_, std::string(DEFAULT_JOY));
00230
00231
00232 nh_.param("cmd_topic_vel", cmd_topic_vel, std::string("/agvs_controller/command"));
00233
00234 nh_.param("button_dead_man", button_dead_man_, button_dead_man_);
00235 nh_.param("button_speed_up", button_speed_up_, button_speed_up_);
00236 nh_.param("button_speed_down", button_speed_down_, button_speed_down_);
00237
00238 nh_.param("max_angular_position", max_angular_position_, DEFAULT_MAX_ANGULAR_POSITION);
00239 nh_.param("max_linear_speed_", max_linear_speed_, DEFAULT_MAX_SKID_LINEAR_SPEED);
00240 nh_.param("axis_linear_speed", axis_linear_speed_, DEFAULT_AXIS_LINEAR_X);
00241 nh_.param("axis_angular_position", axis_angular_position_, DEFAULT_AXIS_ANGULAR);
00242 ROS_INFO("axis_linear_speed_ = %d, axis_angular = %d", axis_linear_speed_, axis_angular_position_);
00243 ROS_INFO("max_linear_speed = %lf, max_angular_speed = %lf", max_linear_speed_, max_angular_position_);
00244
00245
00246 nh_.param("cmd_service_io", cmd_service_io_, cmd_service_io_);
00247 nh_.param("button_up_car", button_up_car_, button_up_car_);
00248 nh_.param("button_down_car", button_down_car_, button_down_car_);
00249 nh_.param("output_1", output_1_, output_1_);
00250 nh_.param("output_2", output_2_, output_2_);
00251
00252 nh_.param("topic_state", topic_state_, std::string("/agvs_pad/state"));
00253
00254 nh_.param("service_raise_elevator", service_raise_elevator_, std::string("/agvs_robot_control/raise_elevator"));
00255 nh_.param("service_lower_elevator", service_lower_elevator_, std::string("/agvs_robot_control/lower_elevator"));
00256
00257 ROS_INFO("AgvsPad num_of_buttons_ = %d, axes = %d, topic controller: %s, hz = %.2lf", num_of_buttons_, num_of_axes_, cmd_topic_vel.c_str(), desired_freq_);
00258
00259 for(int i = 0; i < MAX_NUM_OF_BUTTONS_PS3; i++){
00260 Button b;
00261 vButtons.push_back(b);
00262 }
00263
00264 for(int i = 0; i < MAX_NUM_OF_AXES_PS3; i++){
00265 fAxes.push_back(0.0);
00266 }
00267
00268
00269
00270
00271 this->vel_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(this->cmd_topic_vel, 1);
00272
00273
00274
00275
00276
00277
00278
00279 joy_sub_ = nh_.subscribe<sensor_msgs::Joy>(joy_topic_, 1, &AgvsPad::joyCallback, this);
00280
00281
00282 set_digital_outputs_client_ = nh_.serviceClient<robotnik_msgs::set_digital_output>(cmd_service_io_);
00283
00284
00285 raise_elevator_client_ = nh_.serviceClient<std_srvs::Empty>(service_raise_elevator_);
00286 lower_elevator_client_ = nh_.serviceClient<std_srvs::Empty>(service_lower_elevator_);
00287
00288
00289 bOutput1 = bOutput2 = false;
00290
00291
00292
00293 updater_pad.setHardwareID("AGVS-PAD");
00294
00295 min_freq_command = min_freq_joy = 5.0;
00296 max_freq_command = max_freq_joy = 50.0;
00297 sus_joy_freq = new diagnostic_updater::HeaderlessTopicDiagnostic("/joy", updater_pad,
00298 diagnostic_updater::FrequencyStatusParam(&min_freq_joy, &max_freq_joy, 0.1, 10));
00299
00300 pub_command_freq = new diagnostic_updater::HeaderlessTopicDiagnostic(cmd_topic_vel.c_str(), updater_pad,
00301 diagnostic_updater::FrequencyStatusParam(&min_freq_command, &max_freq_command, 0.1, 10));
00302
00303
00304 enable_disable_srv_ = nh_.advertiseService("/agvs_pad/enable_disable", &AgvsPad::EnableDisable, this);
00305
00306 bEnable = true;
00307
00308 }
00309
00310
00311
00312
00313
00314
00315
00316 void AgvsPad::Update(){
00317 PublishState();
00318 }
00319
00321 void AgvsPad::PublishState(){
00322
00323
00324 }
00325
00326
00327
00328
00329
00330 bool AgvsPad::EnableDisable(robotnik_msgs::enable_disable::Request &req, robotnik_msgs::enable_disable::Response &res )
00331 {
00332 bEnable = req.value;
00333
00334 ROS_INFO("AgvsPad::EnablaDisable: Setting to %d", req.value);
00335 res.ret = true;
00336 return true;
00337 }
00338
00339
00340 void AgvsPad::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00341 {
00342 double t_diff = (ros::Time::now() - joy->header.stamp).toSec();
00343 if( t_diff <= DEFAULT_JOY_TIMEOUT){
00344
00345 for(int i = 0; i < joy->axes.size(); i++){
00346 this->fAxes[i] = joy->axes[i];
00347 }
00348 for(int i = 0; i < joy->buttons.size(); i++){
00349 this->vButtons[i].Press(joy->buttons[i]);
00350 }
00351 }else
00352 ROS_WARN("AgvsPad::joyCallback: %.3lf secs of diff. Ignoring command", t_diff);
00353
00354
00355 }
00356
00357
00359 void AgvsPad::ControlLoop(){
00360 double desired_linear_speed = 0.0, desired_angular_position = 0.0;
00361
00362 ackermann_msgs::AckermannDriveStamped ref_msg;
00363
00364
00365 ros::Rate r(desired_freq_);
00366
00367 while(ros::ok()) {
00368
00369 Update();
00370
00371 if(bEnable){
00372
00373 if(vButtons[button_dead_man_].IsPressed()){
00374 ref_msg.header.stamp = ros::Time::now();
00375 ref_msg.drive.jerk = 0.0;
00376 ref_msg.drive.acceleration = 0.0;
00377 ref_msg.drive.steering_angle_velocity = 0.0;
00378
00379 desired_linear_speed = max_linear_speed_ * current_speed_lvl * fAxes[axis_linear_speed_];
00380 desired_angular_position = max_angular_position_ * fAxes[axis_angular_position_];
00381
00382 ref_msg.drive.steering_angle = desired_angular_position;
00383 ref_msg.drive.speed = desired_linear_speed;
00384
00385
00386 vel_pub_.publish(ref_msg);
00387
00388 if(vButtons[button_speed_up_].IsReleased()){
00389 current_speed_lvl += 0.1;
00390 if(current_speed_lvl > 1.0)
00391 current_speed_lvl = 1.0;
00392 }
00393 if(vButtons[button_speed_down_].IsReleased()){
00394 current_speed_lvl -= 0.1;
00395 if(current_speed_lvl < 0.0)
00396 current_speed_lvl = 0.0;
00397 }
00398 if (vButtons[button_up_car_].IsReleased()){
00399 std_srvs::Empty empty_srv;
00400 raise_elevator_client_.call( empty_srv );
00401 ROS_INFO("AgvsPad::ControlLoop: Raise elevator");
00402 }
00403 if (vButtons[button_down_car_].IsReleased()){
00404 std_srvs::Empty empty_srv;
00405 lower_elevator_client_.call( empty_srv );
00406 ROS_INFO("AgvsPad::ControlLoop: Lower elevator");
00407 }
00408
00409 }else if(vButtons[button_dead_man_].IsReleased()){
00410 ref_msg.header.stamp = ros::Time::now();
00411 ref_msg.drive.jerk = 0.0;
00412 ref_msg.drive.acceleration = 0.0;
00413 ref_msg.drive.steering_angle_velocity = 0.0;
00414
00415 ref_msg.drive.steering_angle = 0.0;
00416 ref_msg.drive.speed = 0.0;
00417
00418 vel_pub_.publish(ref_msg);
00419 }
00420
00421 }
00422
00423 ros::spinOnce();
00424 r.sleep();
00425 }
00426
00427 }
00428
00429
00431 int main(int argc, char** argv)
00432 {
00433 ros::init(argc, argv, "agvs_pad");
00434 AgvsPad pad;
00435
00436 pad.ControlLoop();
00437
00438 }
00439