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/JointState.h>
00037 #include <sensor_msgs/Imu.h>
00038 #include <geometry_msgs/Twist.h>
00039 #include <std_msgs/Float64.h>
00040 #include <tf/transform_broadcaster.h>
00041 #include <nav_msgs/Odometry.h>
00042 #include <robotnik_msgs/set_mode.h>
00043 #include <robotnik_msgs/get_mode.h>
00044 #include <robotnik_msgs/set_odometry.h>
00045 #include <robotnik_msgs/ptz.h>
00046
00047
00048
00049 #include "diagnostic_msgs/DiagnosticStatus.h"
00050 #include "diagnostic_updater/diagnostic_updater.h"
00051 #include "diagnostic_updater/update_functions.h"
00052 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00053 #include "diagnostic_updater/publisher.h"
00054
00055
00056 #define PI 3.1415926535
00057 #define SUMMIT_XL_MIN_COMMAND_REC_FREQ 5.0
00058 #define SUMMIT_XL_MAX_COMMAND_REC_FREQ 150.0
00059
00060 #define SKID_STEERING 1
00061 #define MECANUM_STEERING 2
00062
00063 #define SUMMIT_XL_WHEEL_DIAMETER 0.25 // Default wheel diameter
00064 #define SUMMIT_XL_D_TRACKS_M 1.0 // default equivalent W distance (difference is due to slippage of skid steering)
00065 #define SUMMIT_XL_WHEELBASE 0.446 // default real L distance forward to rear axis
00066 #define SUMMIT_XL_TRACKWIDTH 0.408 // default real W distance from left wheels to right wheels
00067
00068 using namespace std;
00069
00070 class SummitXLControllerClass {
00071
00072 public:
00073
00074 ros::NodeHandle node_handle_;
00075 ros::NodeHandle private_node_handle_;
00076 double desired_freq_;
00077
00078
00079 diagnostic_updater::Updater diagnostic_;
00080 diagnostic_updater::FrequencyStatus freq_diag_;
00081 diagnostic_updater::HeaderlessTopicDiagnostic *subs_command_freq;
00082 ros::Time last_command_time_;
00083 diagnostic_updater::FunctionDiagnosticTask command_freq_;
00084
00085
00086 std::string robot_model_;
00087
00088
00089 ros::Publisher ref_vel_flw_;
00090 ros::Publisher ref_vel_frw_;
00091 ros::Publisher ref_vel_blw_;
00092 ros::Publisher ref_vel_brw_;
00093 ros::Publisher ref_pos_flw_;
00094 ros::Publisher ref_pos_frw_;
00095 ros::Publisher ref_pos_blw_;
00096 ros::Publisher ref_pos_brw_;
00097 ros::Publisher ref_pos_scissor_;
00098 ros::Publisher ref_pos_pan_;
00099 ros::Publisher ref_pos_tilt_;
00100
00101
00102 ros::Subscriber joint_state_sub_;
00103
00104
00105 ros::Subscriber cmd_sub_;
00106
00107
00108 ros::Subscriber ptz_sub_;
00109
00110
00111
00112
00113 ros::ServiceServer srv_SetOdometry_;
00114 ros::ServiceServer srv_SetMode_;
00115 ros::ServiceServer srv_GetMode_;
00116
00117
00118 std::string frw_vel_topic_;
00119 std::string flw_vel_topic_;
00120 std::string brw_vel_topic_;
00121 std::string blw_vel_topic_;
00122
00123
00124 std::string joint_front_right_wheel;
00125 std::string joint_front_left_wheel;
00126 std::string joint_back_left_wheel;
00127 std::string joint_back_right_wheel;
00128
00129
00130 std::string frw_pos_topic_;
00131 std::string flw_pos_topic_;
00132 std::string brw_pos_topic_;
00133 std::string blw_pos_topic_;
00134
00135
00136 std::string joint_front_right_steer;
00137 std::string joint_front_left_steer;
00138 std::string joint_back_left_steer;
00139 std::string joint_back_right_steer;
00140
00141
00142 std::string scissor_pos_topic_;
00143
00144
00145 std::string joint_camera_pan;
00146 std::string joint_camera_tilt;
00147
00148
00149 std::string pan_pos_topic_;
00150 std::string tilt_pos_topic_;
00151
00152
00153 std::string scissor_prismatic_joint;
00154
00155
00156 int kinematic_modes_;
00157 int active_kinematic_mode_;
00158
00159
00160 int frw_vel_, flw_vel_, blw_vel_, brw_vel_;
00161 int frw_pos_, flw_pos_, blw_pos_, brw_pos_;
00162 int scissor_pos_;
00163 int pan_pos_, tilt_pos_;
00164
00165
00166
00167 double linearSpeedXMps_;
00168 double linearSpeedYMps_;
00169 double angularSpeedRads_;
00170
00171
00172 double robot_pose_px_;
00173 double robot_pose_py_;
00174 double robot_pose_pa_;
00175 double robot_pose_vx_;
00176 double robot_pose_vy_;
00177
00178
00179 sensor_msgs::JointState joint_state_;
00180
00181
00182 geometry_msgs::Twist base_vel_msg_;
00183
00184
00185 double v_ref_x_;
00186 double v_ref_y_;
00187 double w_ref_;
00188 double v_ref_z_;
00189 double pos_ref_pan_;
00190 double pos_ref_tilt_;
00191
00192
00193 bool read_state_;
00194
00195
00196 double summit_xl_wheel_diameter_;
00197 double summit_xl_d_tracks_m_;
00198 double summit_xl_wheelbase_;
00199 double summit_xl_trackwidth_;
00200
00201
00202 double ang_vel_x_;
00203 double ang_vel_y_;
00204 double ang_vel_z_;
00205
00206 double lin_acc_x_;
00207 double lin_acc_y_;
00208 double lin_acc_z_;
00209
00210 double orientation_x_;
00211 double orientation_y_;
00212 double orientation_z_;
00213 double orientation_w_;
00214
00215
00216 bool publish_odom_tf_;
00217
00218 ros::Subscriber imu_sub_;
00219
00220
00221 ros::Publisher odom_pub_;
00222
00223
00224 tf::TransformBroadcaster odom_broadcaster;
00225
00226
00230 SummitXLControllerClass(ros::NodeHandle h) : diagnostic_(),
00231 node_handle_(h), private_node_handle_("~"),
00232 desired_freq_(100),
00233 freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05) ),
00234 command_freq_("Command frequency check", boost::bind(&SummitXLControllerClass::check_command_subscriber, this, _1))
00235 {
00236
00237
00238 ROS_INFO("summit_xl_robot_control_node - Init ");
00239
00240
00241
00242 kinematic_modes_ = 1;
00243
00244 ros::NodeHandle summit_xl_robot_control_node_handle(node_handle_, "summit_xl_robot_control");
00245
00246
00247 if (!private_node_handle_.getParam("model", robot_model_)) {
00248 ROS_ERROR("Robot model not defined.");
00249 exit(-1);
00250 }
00251 else ROS_INFO("Robot Model : %s", robot_model_.c_str());
00252
00253
00254 private_node_handle_.param<std::string>("frw_vel_topic", frw_vel_topic_, "/summit_xl/joint_frw_velocity_controller/command");
00255 private_node_handle_.param<std::string>("flw_vel_topic", flw_vel_topic_, "/summit_xl/joint_flw_velocity_controller/command");
00256 private_node_handle_.param<std::string>("blw_vel_topic", blw_vel_topic_, "/summit_xl/joint_blw_velocity_controller/command");
00257 private_node_handle_.param<std::string>("brw_vel_topic", brw_vel_topic_, "/summit_xl/joint_brw_velocity_controller/command");
00258
00259
00260 private_node_handle_.param<std::string>("joint_front_right_wheel", joint_front_right_wheel, "joint_front_right_wheel");
00261 private_node_handle_.param<std::string>("joint_front_left_wheel", joint_front_left_wheel, "joint_front_left_wheel");
00262 private_node_handle_.param<std::string>("joint_back_left_wheel", joint_back_left_wheel, "joint_back_left_wheel");
00263 private_node_handle_.param<std::string>("joint_back_right_wheel", joint_back_right_wheel, "joint_back_right_wheel");
00264
00265
00266 if ((robot_model_=="summit_xl_omni") || (robot_model_=="x_wam")) {
00267 kinematic_modes_ = 2;
00268 private_node_handle_.param<std::string>("frw_pos_topic", frw_pos_topic_, "/summit_xl/joint_frw_velocity_controller/command");
00269 private_node_handle_.param<std::string>("flw_pos_topic", flw_pos_topic_, "/summit_xl/joint_flw_velocity_controller/command");
00270 private_node_handle_.param<std::string>("blw_pos_topic", blw_pos_topic_, "/summit_xl/joint_blw_velocity_controller/command");
00271 private_node_handle_.param<std::string>("brw_pos_topic", brw_pos_topic_, "/summit_xl/joint_brw_velocity_controller/command");
00272
00273 private_node_handle_.param<std::string>("joint_front_right_steer", joint_front_right_steer, "joint_front_right_steer");
00274 private_node_handle_.param<std::string>("joint_front_left_steer", joint_front_left_steer, "joint_front_left_steer");
00275 private_node_handle_.param<std::string>("joint_back_left_steer", joint_back_left_steer, "joint_back_left_steer");
00276 private_node_handle_.param<std::string>("joint_back_right_steer", joint_back_right_steer, "joint_back_right_steer");
00277
00278 if (!private_node_handle_.getParam("summit_xl_wheelbase", summit_xl_wheelbase_))
00279 summit_xl_wheelbase_ = SUMMIT_XL_WHEELBASE;
00280 if (!private_node_handle_.getParam("summit_xl_trackwidth", summit_xl_trackwidth_))
00281 summit_xl_trackwidth_ = SUMMIT_XL_TRACKWIDTH;
00282
00283
00284 if (robot_model_=="x_wam") {
00285 private_node_handle_.param<std::string>("scissor_pos_topic", scissor_pos_topic_, "/summit_xl/joint_scissor_position_controller/command");
00286 private_node_handle_.param<std::string>("scissor_prismatic_joint", scissor_prismatic_joint, "scissor_prismatic_joint");
00287 }
00288 }
00289
00290
00291 private_node_handle_.param<std::string>("pan_pos_topic", pan_pos_topic_, "/summit_xl/joint_pan_position_controller/command");
00292 private_node_handle_.param<std::string>("tilt_pos_topic", tilt_pos_topic_, "/summit_xl/joint_tilt_position_controller/command");
00293 private_node_handle_.param<std::string>("joint_camera_pan", joint_camera_pan, "joint_camera_pan");
00294 private_node_handle_.param<std::string>("joint_camera_tilt", joint_camera_tilt, "joint_camera_tilt");
00295
00296
00297 if (!private_node_handle_.getParam("summit_xl_wheel_diameter", summit_xl_wheel_diameter_))
00298 summit_xl_wheel_diameter_ = SUMMIT_XL_WHEEL_DIAMETER;
00299 if (!private_node_handle_.getParam("summit_xl_d_tracks_m", summit_xl_d_tracks_m_))
00300 summit_xl_d_tracks_m_ = SUMMIT_XL_D_TRACKS_M;
00301 ROS_INFO("summit_xl_wheel_diameter_ = %5.2f", summit_xl_wheel_diameter_);
00302 ROS_INFO("summit_xl_d_tracks_m_ = %5.2f", summit_xl_d_tracks_m_);
00303
00304 private_node_handle_.param("publish_odom_tf", publish_odom_tf_, true);
00305 if (publish_odom_tf_) ROS_INFO("PUBLISHING odom->base_footprin tf");
00306 else ROS_INFO("NOT PUBLISHING odom->base_footprint tf");
00307
00308
00309 linearSpeedXMps_ = 0.0;
00310 linearSpeedYMps_ = 0.0;
00311 angularSpeedRads_ = 0.0;
00312
00313
00314 robot_pose_px_ = 0.0;
00315 robot_pose_py_ = 0.0;
00316 robot_pose_pa_ = 0.0;
00317 robot_pose_vx_ = 0.0;
00318 robot_pose_vy_ = 0.0;
00319
00320
00321 v_ref_x_ = 0.0;
00322 v_ref_y_ = 0.0;
00323 w_ref_ = 0.0;
00324 v_ref_z_ = 0.0;
00325 pos_ref_pan_ = 0.0;
00326 pos_ref_tilt_= 0.0;
00327
00328
00329 ang_vel_x_ = 0.0; ang_vel_y_ = 0.0; ang_vel_z_ = 0.0;
00330 lin_acc_x_ = 0.0; lin_acc_y_ = 0.0; lin_acc_z_ = 0.0;
00331 orientation_x_ = 0.0; orientation_y_ = 0.0; orientation_z_ = 0.0; orientation_w_ = 0.0;
00332
00333
00334 active_kinematic_mode_ = SKID_STEERING;
00335
00336
00337 srv_SetMode_ = summit_xl_robot_control_node_handle.advertiseService("set_mode", &SummitXLControllerClass::srvCallback_SetMode, this);
00338 srv_GetMode_ = summit_xl_robot_control_node_handle.advertiseService("get_mode", &SummitXLControllerClass::srvCallback_GetMode, this);
00339 srv_SetOdometry_ = summit_xl_robot_control_node_handle.advertiseService("set_odometry", &SummitXLControllerClass::srvCallback_SetOdometry, this);
00340
00341
00342 joint_state_sub_ = summit_xl_robot_control_node_handle.subscribe<sensor_msgs::JointState>("/summit_xl/joint_states", 1, &SummitXLControllerClass::jointStateCallback, this);
00343
00344
00345 imu_sub_ = summit_xl_robot_control_node_handle.subscribe("/summit_xl/imu_data", 1, &SummitXLControllerClass::imuCallback, this);
00346
00347
00348 ref_vel_frw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( frw_vel_topic_, 50);
00349 ref_vel_flw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( flw_vel_topic_, 50);
00350 ref_vel_blw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( blw_vel_topic_, 50);
00351 ref_vel_brw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( brw_vel_topic_, 50);
00352
00353 if ((robot_model_=="summit_xl_omni")||(robot_model_=="x_wam")) {
00354 ref_pos_frw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( frw_pos_topic_, 50);
00355 ref_pos_flw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( flw_pos_topic_, 50);
00356 ref_pos_blw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( blw_pos_topic_, 50);
00357 ref_pos_brw_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( brw_pos_topic_, 50);
00358
00359 if (robot_model_=="x_wam")
00360 ref_pos_scissor_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( scissor_pos_topic_, 50);
00361 }
00362
00363 ref_pos_pan_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( pan_pos_topic_, 50);
00364 ref_pos_tilt_ = summit_xl_robot_control_node_handle.advertise<std_msgs::Float64>( tilt_pos_topic_, 50);
00365
00366
00367 cmd_sub_ = summit_xl_robot_control_node_handle.subscribe<geometry_msgs::Twist>("command", 1, &SummitXLControllerClass::commandCallback, this);
00368
00369
00370 ptz_sub_ = summit_xl_robot_control_node_handle.subscribe<robotnik_msgs::ptz>("command_ptz", 1, &SummitXLControllerClass::command_ptzCallback, this);
00371
00372
00373
00374
00375 odom_pub_ = summit_xl_robot_control_node_handle.advertise<nav_msgs::Odometry>("/summit_xl/odom", 1000);
00376
00377
00378 diagnostic_.setHardwareID("summit_xl_robot_control - simulation");
00379 diagnostic_.add( freq_diag_ );
00380 diagnostic_.add( command_freq_ );
00381
00382
00383
00384 double min_freq = SUMMIT_XL_MIN_COMMAND_REC_FREQ;
00385 double max_freq = SUMMIT_XL_MAX_COMMAND_REC_FREQ;
00386 subs_command_freq = new diagnostic_updater::HeaderlessTopicDiagnostic("/summit_xl_robot_control/command", diagnostic_,
00387 diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10));
00388 subs_command_freq->addTask(&command_freq_);
00389
00390
00391 read_state_ = false;
00392 }
00393
00395 int starting()
00396 {
00397
00398 ROS_INFO("SummitXLControllerClass::starting");
00399
00400
00401
00402
00403
00404
00405 if (read_state_) {
00406 vector<string> joint_names = joint_state_.name;
00407 frw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_right_wheel)) - joint_names.begin();
00408 flw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_front_left_wheel)) - joint_names.begin();
00409 blw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_left_wheel)) - joint_names.begin();
00410 brw_vel_ = find (joint_names.begin(),joint_names.end(), string(joint_back_right_wheel)) - joint_names.begin();
00411 if ((robot_model_=="summit_xl_omni")||(robot_model_=="x_wam")) {
00412 frw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_right_steer)) - joint_names.begin();
00413 flw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_front_left_steer)) - joint_names.begin();
00414 blw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_left_steer)) - joint_names.begin();
00415 brw_pos_ = find (joint_names.begin(),joint_names.end(), string(joint_back_right_steer)) - joint_names.begin();
00416 }
00417 if (robot_model_=="x_wam") {
00418 scissor_pos_ = find (joint_names.begin(),joint_names.end(), string(scissor_prismatic_joint)) - joint_names.begin();
00419 }
00420
00421
00422
00423 return 0;
00424 }
00425 else return -1;
00426 }
00427
00428
00430 void UpdateControl()
00431 {
00432
00433
00434 if (active_kinematic_mode_ == SKID_STEERING) {
00435
00436
00437 double dUl = v_ref_x_ - 0.5 * summit_xl_d_tracks_m_ * w_ref_;
00438
00439 double dUr = v_ref_x_ + 0.5 * summit_xl_d_tracks_m_ * w_ref_;
00440
00441
00442 double limit = 40.0;
00443
00444
00445
00446
00447 std_msgs::Float64 frw_ref_msg;
00448 std_msgs::Float64 flw_ref_msg;
00449 std_msgs::Float64 blw_ref_msg;
00450 std_msgs::Float64 brw_ref_msg;
00451
00452 double k1 = 5.0;
00453 frw_ref_msg.data = -saturation( k1 * dUr, -limit, limit);
00454 flw_ref_msg.data = saturation( k1 * dUl, -limit, limit);
00455 blw_ref_msg.data = saturation( k1 * dUl, -limit, limit);
00456 brw_ref_msg.data = -saturation( k1 * dUr, -limit, limit);
00457
00458 ref_vel_frw_.publish( frw_ref_msg );
00459 ref_vel_flw_.publish( flw_ref_msg );
00460 ref_vel_blw_.publish( blw_ref_msg );
00461 ref_vel_brw_.publish( brw_ref_msg );
00462 }
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631 }
00632
00633
00634 void UpdateOdometry()
00635 {
00636
00637 if (active_kinematic_mode_ == SKID_STEERING) {
00638
00639
00640 robot_pose_vx_ = linearSpeedXMps_ * cos(robot_pose_pa_);
00641 robot_pose_vy_ = linearSpeedXMps_ * sin(robot_pose_pa_);
00642 }
00643
00644
00645 if (active_kinematic_mode_ == MECANUM_STEERING) {
00646
00647
00648 double v1, v2, v3, v4;
00649 v1 = joint_state_.velocity[frw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
00650 v2 = joint_state_.velocity[flw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
00651 v3 = joint_state_.velocity[blw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
00652 v4 = joint_state_.velocity[brw_vel_] * (summit_xl_wheel_diameter_ / 2.0);
00653 double a1, a2, a3, a4;
00654 a1 = radnorm2( joint_state_.position[frw_pos_] );
00655 a2 = radnorm2( joint_state_.position[flw_pos_] );
00656 a3 = radnorm2( joint_state_.position[blw_pos_] );
00657 a4 = radnorm2( joint_state_.position[brw_pos_] );
00658 double v1x = -v1 * cos( a1 ); double v1y = -v1 * sin( a1 );
00659 double v2x = v2 * cos( a2 ); double v2y = v2 * sin( a2 );
00660 double v3x = v3 * cos( a3 ); double v3y = v3 * sin( a3 );
00661 double v4x = -v4 * cos( a4 ); double v4y = -v4 * sin( a4 );
00662 double C = (v4y + v1y) / 2.0;
00663 double B = (v2x + v1x) / 2.0;
00664 double D = (v2y + v3y) / 2.0;
00665 double A = (v3x + v4x) / 2.0;
00666
00667 robot_pose_vx_ = (A+B) / 2.0;
00668 robot_pose_vy_ = (C+D) / 2.0;
00669 }
00670
00671
00672 double fSamplePeriod = 1.0 / desired_freq_;
00673 robot_pose_pa_ += ang_vel_z_ * fSamplePeriod;
00674 robot_pose_px_ += robot_pose_vx_ * fSamplePeriod;
00675 robot_pose_py_ += robot_pose_vy_ * fSamplePeriod;
00676
00677 }
00678
00679
00680 void PublishOdometry()
00681 {
00682 ros::Time current_time = ros::Time::now();
00683
00684
00685 geometry_msgs::TransformStamped odom_trans;
00686 odom_trans.header.stamp = current_time;
00687 odom_trans.header.frame_id = "odom";
00688 odom_trans.child_frame_id = "base_footprint";
00689
00690 odom_trans.transform.translation.x = robot_pose_px_;
00691 odom_trans.transform.translation.y = robot_pose_py_;
00692 odom_trans.transform.translation.z = 0.0;
00693 odom_trans.transform.rotation.x = orientation_x_;
00694 odom_trans.transform.rotation.y = orientation_y_;
00695 odom_trans.transform.rotation.z = orientation_z_;
00696 odom_trans.transform.rotation.w = orientation_w_;
00697
00698
00699
00700
00701 if (publish_odom_tf_) odom_broadcaster.sendTransform(odom_trans);
00702
00703
00704 nav_msgs::Odometry odom;
00705 odom.header.stamp = current_time;
00706 odom.header.frame_id = "odom";
00707
00708
00709
00710 odom.pose.pose.position.x = robot_pose_px_;
00711 odom.pose.pose.position.y = robot_pose_py_;
00712 odom.pose.pose.position.z = 0.0;
00713
00714 odom.pose.pose.orientation.x = orientation_x_;
00715 odom.pose.pose.orientation.y = orientation_y_;
00716 odom.pose.pose.orientation.z = orientation_z_;
00717 odom.pose.pose.orientation.w = orientation_w_;
00718
00719 for(int i = 0; i < 6; i++)
00720 odom.pose.covariance[i*6+i] = 0.1;
00721
00722
00723 odom.child_frame_id = "base_footprint";
00724
00725 odom.twist.twist.linear.x = robot_pose_vx_;
00726 odom.twist.twist.linear.y = robot_pose_vy_;
00727 odom.twist.twist.linear.z = 0.0;
00728
00729 odom.twist.twist.angular.x = ang_vel_x_;
00730 odom.twist.twist.angular.y = ang_vel_y_;
00731 odom.twist.twist.angular.z = ang_vel_z_;
00732
00733 for(int i = 0; i < 6; i++)
00734 odom.twist.covariance[6*i+i] = 0.1;
00735
00736
00737 odom_pub_.publish(odom);
00738 }
00739
00741 void stopping()
00742 {}
00743
00744
00745
00746
00747
00748
00749 void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat)
00750 {
00751 ros::Time current_time = ros::Time::now();
00752
00753 double diff = (current_time - last_command_time_).toSec();
00754
00755 if(diff > 1.0){
00756 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Topic is not receiving commands");
00757
00758
00759 }else{
00760 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Topic receiving commands");
00761 }
00762 }
00763
00764
00765
00766 void setCommand(const geometry_msgs::Twist &cmd_vel)
00767 {
00768 v_ref_x_ = saturation(cmd_vel.linear.x, -10.0, 10.0);
00769 v_ref_y_ = saturation(cmd_vel.linear.y, -10.0, 10.0);
00770 w_ref_ = saturation(cmd_vel.angular.z, -10.0, 10.0);
00771 v_ref_z_ = saturation(cmd_vel.linear.z, -10.0, 10.0);
00772 }
00773
00774
00775
00776 bool srvCallback_SetMode(robotnik_msgs::set_mode::Request& request, robotnik_msgs::set_mode::Response& response)
00777 {
00778
00779
00780
00781
00782
00783 ROS_INFO("SummitXLControllerClass::srvCallback_SetMode request.mode= %d", request.mode);
00784 if (request.mode == 1) {
00785 active_kinematic_mode_ = request.mode;
00786 return true;
00787 }
00788 if (request.mode == 2) {
00789 if (kinematic_modes_ == 2) {
00790 active_kinematic_mode_ = request.mode;
00791 return true;
00792 }
00793 else return false;
00794 }
00795 return false;
00796 }
00797
00798
00799 bool srvCallback_GetMode(robotnik_msgs::get_mode::Request& request, robotnik_msgs::get_mode::Response& response)
00800 {
00801 response.mode = active_kinematic_mode_;
00802 return true;
00803 }
00804
00805
00806
00807 bool srvCallback_SetOdometry(robotnik_msgs::set_odometry::Request &request, robotnik_msgs::set_odometry::Response &response )
00808 {
00809
00810
00811
00812
00813
00814 response.ret = true;
00815 return true;
00816 }
00817
00818
00819
00820 void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg)
00821 {
00822 joint_state_ = *msg;
00823 read_state_ = true;
00824 }
00825
00826
00827 void commandCallback(const geometry_msgs::TwistConstPtr& msg)
00828 {
00829
00830 last_command_time_ = ros::Time::now();
00831 subs_command_freq->tick();
00832
00833 base_vel_msg_ = *msg;
00834 this->setCommand(base_vel_msg_);
00835 }
00836
00837
00838 void command_ptzCallback(const robotnik_msgs::ptzConstPtr& msg)
00839 {
00840 pos_ref_pan_ += msg->pan / 180.0 * PI;
00841 pos_ref_tilt_ += msg->tilt / 180.0 * PI;
00842 }
00843
00844
00845 void imuCallback( const sensor_msgs::Imu& imu_msg){
00846
00847 orientation_x_ = imu_msg.orientation.x;
00848 orientation_y_ = imu_msg.orientation.y;
00849 orientation_z_ = imu_msg.orientation.z;
00850 orientation_w_ = imu_msg.orientation.w;
00851
00852 ang_vel_x_ = imu_msg.angular_velocity.x;
00853 ang_vel_y_ = imu_msg.angular_velocity.y;
00854 ang_vel_z_ = imu_msg.angular_velocity.z;
00855
00856 lin_acc_x_ = imu_msg.linear_acceleration.x;
00857 lin_acc_y_ = imu_msg.linear_acceleration.y;
00858 lin_acc_z_ = imu_msg.linear_acceleration.z;
00859 }
00860
00861 double saturation(double u, double min, double max)
00862 {
00863 if (u>max) u=max;
00864 if (u<min) u=min;
00865 return u;
00866 }
00867
00868 double radnorm( double value )
00869 {
00870 while (value > PI) value -= PI;
00871 while (value < -PI) value += PI;
00872 return value;
00873 }
00874
00875 double radnorm2( double value )
00876 {
00877 while (value > 2.0*PI) value -= 2.0*PI;
00878 while (value < -2.0*PI) value += 2.0*PI;
00879 return value;
00880 }
00881
00882 bool spin()
00883 {
00884 ROS_INFO("summit_xl_robot_control::spin()");
00885 ros::Rate r(desired_freq_);
00886
00887 while (!ros::isShuttingDown())
00888 {
00889 if (starting() == 0)
00890 {
00891 while(ros::ok() && node_handle_.ok()) {
00892 UpdateControl();
00893 UpdateOdometry();
00894 PublishOdometry();
00895 diagnostic_.update();
00896 ros::spinOnce();
00897 r.sleep();
00898 }
00899 ROS_INFO("END OF ros::ok() !!!");
00900 } else {
00901
00902
00903 usleep(1000000);
00904 ros::spinOnce();
00905 }
00906 }
00907
00908 ROS_INFO("summit_xl_robot_control::spin() - end");
00909 return true;
00910 }
00911
00912 };
00913
00914 int main(int argc, char** argv)
00915 {
00916 ros::init(argc, argv, "summit_xl_robot_control");
00917
00918 ros::NodeHandle n;
00919 SummitXLControllerClass sxlrc(n);
00920
00921
00922
00923 sxlrc.spin();
00924
00925 return (0);
00926
00927
00928 }
00929