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
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 #include <math.h>
00061
00062
00063 #include <ros/ros.h>
00064
00065
00066 #include <sensor_msgs/JointState.h>
00067 #include <diagnostic_msgs/DiagnosticStatus.h>
00068 #include <diagnostic_updater/diagnostic_updater.h>
00069 #include <geometry_msgs/Twist.h>
00070 #include <nav_msgs/Odometry.h>
00071 #include <tf/transform_broadcaster.h>
00072 #include <cob_relayboard/EmergencyStopState.h>
00073 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00074
00075
00076
00077 #include <cob_srvs/Trigger.h>
00078 #include <cob_base_drive_chain/GetJointState.h>
00079
00080
00081 #include <cob_undercarriage_ctrl/UndercarriageCtrlGeom.h>
00082 #include <cob_utilities/IniFile.h>
00083
00084
00085
00086
00087 class NodeClass
00088 {
00089
00090 public:
00091
00092 ros::NodeHandle n;
00093
00094
00095 ros::Publisher topic_pub_joint_state_cmd_;
00096 ros::Publisher topic_pub_controller_joint_command_;
00097 ros::Publisher topic_pub_odometry_;
00098 tf::TransformBroadcaster tf_broadcast_odometry_;
00099
00100
00101 ros::Subscriber topic_sub_CMD_pltf_twist_;
00102 ros::Subscriber topic_sub_EM_stop_state_;
00103 ros::Subscriber topic_sub_drive_diagnostic_;
00104
00105
00106
00107 ros::Subscriber topic_sub_joint_controller_states_;
00108
00109
00110 ros::ServiceServer srvServer_IsMoving;
00111
00112
00113 diagnostic_updater::Updater updater_;
00114
00115
00116 ros::ServiceClient srv_client_get_joint_state_;
00117
00118
00119 UndercarriageCtrlGeom * ucar_ctrl_;
00120 std::string sIniDirectory;
00121 bool is_initialized_bool_;
00122 bool is_moving_;
00123 int drive_chain_diagnostic_;
00124 ros::Time last_time_;
00125 double x_rob_m_, y_rob_m_, theta_rob_rad_;
00126 int iwatchdog_;
00127
00128 int m_iNumJoints;
00129
00130 diagnostic_msgs::DiagnosticStatus diagnostic_status_lookup_;
00131
00132
00133 NodeClass()
00134 {
00135
00136 is_initialized_bool_ = false;
00137 is_moving_ = false;
00138 iwatchdog_ = 0;
00139 last_time_ = ros::Time::now();
00140 x_rob_m_ = 0.0;
00141 y_rob_m_ = 0.0;
00142 theta_rob_rad_ = 0.0;
00143
00144 drive_chain_diagnostic_ = diagnostic_status_lookup_.OK;
00145
00146
00147
00148 if (n.hasParam("IniDirectory"))
00149 {
00150 n.getParam("IniDirectory", sIniDirectory);
00151 ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
00152 }
00153 else
00154 {
00155 sIniDirectory = "Platform/IniFiles/";
00156 ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
00157 }
00158
00159 IniFile iniFile;
00160 iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
00161 iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumJoints, true);
00162
00163 ucar_ctrl_ = new UndercarriageCtrlGeom(sIniDirectory);
00164
00165
00166
00167
00168
00169 topic_pub_controller_joint_command_ = n.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("joint_command", 1);
00170
00171 topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("odometry", 50);
00172
00173
00174 topic_sub_CMD_pltf_twist_ = n.subscribe("command", 1, &NodeClass::topicCallbackTwistCmd, this);
00175 topic_sub_EM_stop_state_ = n.subscribe("/emergency_stop_state", 1, &NodeClass::topicCallbackEMStop, this);
00176 topic_sub_drive_diagnostic_ = n.subscribe("diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
00177
00178
00179
00180
00181 topic_sub_joint_controller_states_ = n.subscribe("state", 1, &NodeClass::topicCallbackJointControllerStates, this);
00182
00183
00184 updater_.setHardwareID(ros::this_node::getName());
00185 updater_.add("initialization", this, &NodeClass::diag_init);
00186
00187
00188 srvServer_IsMoving = n.advertiseService("is_moving", &NodeClass::srvCallback_IsMoving, this);
00189
00190
00191 srv_client_get_joint_state_ = n.serviceClient<cob_base_drive_chain::GetJointState>("GetJointState");
00192 }
00193
00194
00195 ~NodeClass()
00196 {
00197 }
00198
00199 void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
00200 {
00201 if(is_initialized_bool_)
00202 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
00203 else
00204 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
00205 stat.add("Initialized", is_initialized_bool_);
00206 }
00207
00208
00209
00210
00211
00212
00213 void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg)
00214 {
00215 double vx_cmd_mms, vy_cmd_mms, w_cmd_rads;
00216
00217 iwatchdog_ = 0;
00218
00219
00220
00221 vx_cmd_mms = msg->linear.x*1000.0;
00222 vy_cmd_mms = msg->linear.y*1000.0;
00223 w_cmd_rads = msg->angular.z;
00224
00225
00226 if (is_initialized_bool_)
00227 {
00228 ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]",
00229 msg->linear.x, msg->linear.y, msg->angular.z);
00230
00231
00232 ucar_ctrl_->SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0);
00233
00234 }
00235 else
00236 {
00237
00238 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00239
00240 ROS_DEBUG("Forced platform-velocity cmds to zero");
00241 }
00242 }
00243
00244
00245 void topicCallbackEMStop(const cob_relayboard::EmergencyStopState::ConstPtr& msg)
00246 {
00247 int EM_state;
00248 EM_state = msg->emergency_state;
00249
00250 if (EM_state == msg->EMFREE)
00251 {
00252
00253 if (is_initialized_bool_)
00254 {
00255 ucar_ctrl_->setEMStopActive(false);
00256 ROS_DEBUG("Undercarriage Controller EM-Stop released");
00257
00258
00259 }
00260 }
00261 else
00262 {
00263 ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop");
00264
00265
00266 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00267
00268 ROS_DEBUG("Forced platform-velocity cmds to zero");
00269
00270
00271 ucar_ctrl_->setEMStopActive(true);
00272 }
00273 }
00274
00275
00276 void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
00277 {
00278 pr2_controllers_msgs::JointTrajectoryControllerState joint_state_cmd;
00279
00280
00281 joint_state_cmd.header.stamp = ros::Time::now();
00282
00283
00284
00285
00286 joint_state_cmd.desired.positions.resize(m_iNumJoints);
00287 joint_state_cmd.desired.velocities.resize(m_iNumJoints);
00288
00289 joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint");
00290 joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint");
00291 joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint");
00292 joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint");
00293 joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint");
00294 joint_state_cmd.joint_names.push_back("br_caster_rotation_joint");
00295 joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint");
00296 joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint");
00297
00298 for(int i=0; i<m_iNumJoints; i++)
00299 {
00300 joint_state_cmd.desired.positions[i] = 0.0;
00301 joint_state_cmd.desired.velocities[i] = 0.0;
00302
00303 }
00304
00305
00306 drive_chain_diagnostic_ = msg->level;
00307
00308
00309 if (is_initialized_bool_)
00310 {
00311
00312 if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
00313 {
00314
00315 ROS_DEBUG("drive chain not availlable: halt Controller");
00316
00317
00318 ucar_ctrl_->setEMStopActive(true);
00319
00320
00321 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00322
00323 ROS_DEBUG("Forced platform-velocity cmds to zero");
00324
00325
00326 if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
00327 {
00328
00329 topic_pub_controller_joint_command_.publish(joint_state_cmd);
00330 }
00331 }
00332 }
00333
00334 else
00335 {
00336
00337 if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
00338 {
00339
00340 topic_pub_controller_joint_command_.publish(joint_state_cmd);
00341 }
00342 }
00343 }
00344
00345
00346
00347
00348
00349 bool srvCallback_IsMoving(cob_srvs::Trigger::Request &req,
00350 cob_srvs::Trigger::Response &res )
00351 {
00352 ROS_DEBUG("Service Callback is_moving");
00353 res.success.data = is_moving_;
00354 return true;
00355 }
00356
00357
00358 bool srvCallbackInit(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00359 {
00360 if(is_initialized_bool_ == false)
00361 {
00362 is_initialized_bool_ = InitCtrl();
00363
00364 res.success.data = is_initialized_bool_;
00365
00366 if (is_initialized_bool_)
00367 {
00368 ROS_INFO("Undercarriage-Ctrl initialized");
00369
00370 last_time_ = ros::Time::now();
00371 }
00372 else
00373 {
00374 ROS_INFO("Undercarriage-Ctrl initialization failed");
00375 res.error_message.data = "initialization of undercarriage controller failed";
00376 }
00377 }
00378 else
00379 {
00380 ROS_ERROR("... undercarriage controller already initialized...");
00381 res.success.data = false;
00382 res.error_message.data = "undercarriage controller already initialized";
00383 }
00384 return true;
00385 }
00386
00387
00388 bool srvCallbackReset(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00389 {
00390 bool ctrlr_reset;
00391
00392 if (is_initialized_bool_)
00393 {
00394
00395
00396 is_initialized_bool_ = false;
00397
00398
00399
00400 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00401
00402 ROS_DEBUG("Forced platform-velocity cmds to zero");
00403
00404 ucar_ctrl_->setEMStopActive(true);
00405
00406
00407 ctrlr_reset = InitCtrl();
00408
00409 if (ctrlr_reset)
00410 {
00411
00412
00413 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00414
00415 ROS_DEBUG("Forced platform-velocity cmds to zero");
00416
00417 ucar_ctrl_->setEMStopActive(false);
00418
00419
00420 last_time_ = ros::Time::now();
00421
00422 x_rob_m_ = 0.0;
00423 y_rob_m_ = 0.0;
00424 theta_rob_rad_ = 0.0;
00425
00426
00427 is_initialized_bool_ = true;
00428
00429
00430 ROS_INFO("Undercarriage Controller resetted");
00431 res.success.data = true;
00432 }
00433 else
00434 {
00435
00436 ROS_INFO("Re-Init after Reset of Undercarriage Controller failed");
00437 res.success.data = false;
00438 res.error_message.data = "reinit after reset of undercarriage controller failed";
00439 }
00440 }
00441 else
00442 {
00443
00444 ROS_ERROR("... undercarriage controller not yet initialized, reset not possible ...");
00445
00446 res.success.data = false;
00447 res.error_message.data = "undercarriage controller not yet initialized";
00448 }
00449
00450 return true;
00451 }
00452
00453
00454 bool srvCallbackShutdown(cob_srvs::Trigger::Request &req,
00455 cob_srvs::Trigger::Response &res )
00456 {
00457 if (is_initialized_bool_)
00458 {
00459
00460
00461 ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
00462 ROS_DEBUG("Forced platform-velocity cmds to zero");
00463
00464
00465 is_initialized_bool_ = false;
00466
00467 res.success.data = true;
00468 }
00469 else
00470 {
00471
00472 ROS_ERROR("...platform not initialized...");
00473 res.success.data = false;
00474 res.error_message.data = "platform already or still down";
00475 }
00476 return true;
00477 }
00478
00479 void topicCallbackJointControllerStates(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg) {
00480 int num_joints;
00481 int iter_k, iter_j;
00482 std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM;
00483 std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM;
00484 cob_base_drive_chain::GetJointState srv_get_joint;
00485
00486
00487 num_joints = msg->joint_names.size();
00488
00489 drive_joint_ang_rad.assign(m_iNumJoints, 0.0);
00490 drive_joint_vel_rads.assign(m_iNumJoints, 0.0);
00491 drive_joint_effort_NM.assign(m_iNumJoints, 0.0);
00492
00493 steer_joint_ang_rad.assign(m_iNumJoints, 0.0);
00494 steer_joint_vel_rads.assign(m_iNumJoints, 0.0);
00495 steer_joint_effort_NM.assign(m_iNumJoints, 0.0);
00496
00497
00498 iter_k = 0;
00499 iter_j = 0;
00500
00501 for(int i = 0; i < num_joints; i++)
00502 {
00503
00504
00505 if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
00506 {
00507 drive_joint_ang_rad[0] = msg->actual.positions[i];
00508 drive_joint_vel_rads[0] = msg->actual.velocities[i];
00509
00510 }
00511 if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
00512 {
00513 drive_joint_ang_rad[1] = msg->actual.positions[i];
00514 drive_joint_vel_rads[1] = msg->actual.velocities[i];
00515
00516 }
00517 if(msg->joint_names[i] == "br_caster_r_wheel_joint")
00518 {
00519 drive_joint_ang_rad[2] = msg->actual.positions[i];
00520 drive_joint_vel_rads[2] = msg->actual.velocities[i];
00521
00522 }
00523 if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
00524 {
00525 drive_joint_ang_rad[3] = msg->actual.positions[i];
00526 drive_joint_vel_rads[3] = msg->actual.velocities[i];
00527
00528 }
00529 if(msg->joint_names[i] == "fl_caster_rotation_joint")
00530 {
00531 steer_joint_ang_rad[0] = msg->actual.positions[i];
00532 steer_joint_vel_rads[0] = msg->actual.velocities[i];
00533
00534 }
00535 if(msg->joint_names[i] == "bl_caster_rotation_joint")
00536 {
00537 steer_joint_ang_rad[1] = msg->actual.positions[i];
00538 steer_joint_vel_rads[1] = msg->actual.velocities[i];
00539
00540 }
00541 if(msg->joint_names[i] == "br_caster_rotation_joint")
00542 {
00543 steer_joint_ang_rad[2] = msg->actual.positions[i];
00544 steer_joint_vel_rads[2] = msg->actual.velocities[i];
00545
00546 }
00547 if(msg->joint_names[i] == "fr_caster_rotation_joint")
00548 {
00549 steer_joint_ang_rad[3] = msg->actual.positions[i];
00550 steer_joint_vel_rads[3] = msg->actual.velocities[i];
00551
00552 }
00553
00554 }
00555
00556
00557 ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads,
00558 drive_joint_ang_rad, steer_joint_ang_rad);
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
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693 bool InitCtrl();
00694
00695 void CalcCtrlStep();
00696
00697 void GetJointState();
00698
00699 void UpdateOdometry();
00700 };
00701
00702
00703
00704 int main(int argc, char** argv)
00705 {
00706
00707 ros::init(argc, argv, "undercarriage_ctrl");
00708
00709
00710 NodeClass nodeClass;
00711
00712
00713 if( nodeClass.is_initialized_bool_ = nodeClass.InitCtrl() ) {
00714 nodeClass.last_time_ = ros::Time::now();
00715 ROS_INFO("Undercarriage control successfully initialized.");
00716 } else
00717 ROS_WARN("Undercarriage control initialization failed! Try manually.");
00718
00719
00720 ros::Rate loop_rate(50);
00721
00722 while(nodeClass.n.ok())
00723 {
00724
00725 ros::spinOnce();
00726
00727
00728
00729
00730
00731
00732
00733 nodeClass.UpdateOdometry();
00734
00735
00736
00737 nodeClass.CalcCtrlStep();
00738
00739
00740 loop_rate.sleep();
00741 }
00742
00743 return 0;
00744 }
00745
00746
00747
00748 bool NodeClass::InitCtrl()
00749 {
00750 ROS_INFO("Initializing Undercarriage Controller");
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764 ucar_ctrl_->InitUndercarriageCtrl();
00765 ROS_INFO("Initializing Undercarriage Controller done");
00766
00767 return true;
00768 }
00769
00770
00771 void NodeClass::CalcCtrlStep()
00772 {
00773 double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy;
00774 std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad;
00775 pr2_controllers_msgs::JointTrajectoryControllerState joint_state_cmd;
00776 int j, k;
00777 iwatchdog_ += 1;
00778
00779
00780 if (is_initialized_bool_)
00781 {
00782
00783
00784
00785
00786
00787
00788
00789 ucar_ctrl_->GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy);
00790
00791
00792
00793 if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
00794 {
00795 steer_jointang_cmds_rad.assign(m_iNumJoints, 0.0);
00796 steer_jointvel_cmds_rads.assign(m_iNumJoints, 0.0);
00797
00798 }
00799
00800
00801 vx_cmd_ms = vx_cmd_ms/1000.0;
00802 vy_cmd_ms = vy_cmd_ms/1000.0;
00803
00804
00805
00806 joint_state_cmd.header.stamp = ros::Time::now();
00807
00808
00809
00810
00811 joint_state_cmd.desired.positions.resize(m_iNumJoints);
00812 joint_state_cmd.desired.velocities.resize(m_iNumJoints);
00813
00814 joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint");
00815 joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint");
00816 joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint");
00817 joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint");
00818 joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint");
00819 joint_state_cmd.joint_names.push_back("br_caster_rotation_joint");
00820 joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint");
00821 joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint");
00822
00823
00824 j = 0;
00825 k = 0;
00826 for(int i = 0; i<m_iNumJoints; i++)
00827 {
00828 if(iwatchdog_ < 50)
00829 {
00830
00831 if( i == 1 || i == 3 || i == 5 || i == 7)
00832 {
00833 joint_state_cmd.desired.positions[i] = steer_jointang_cmds_rad[j];
00834 joint_state_cmd.desired.velocities[i] = steer_jointvel_cmds_rads[j];
00835
00836 j = j + 1;
00837 }
00838 else
00839 {
00840 joint_state_cmd.desired.positions[i] = 0.0;
00841 joint_state_cmd.desired.velocities[i] = drive_jointvel_cmds_rads[k];
00842
00843 k = k + 1;
00844 }
00845 }
00846 else
00847 {
00848 joint_state_cmd.desired.positions[i] = 0.0;
00849 joint_state_cmd.desired.velocities[i] = 0.0;
00850
00851 }
00852 }
00853
00854
00855 topic_pub_controller_joint_command_.publish(joint_state_cmd);
00856 }
00857
00858 }
00859
00860
00861
00862 void NodeClass::GetJointState()
00863 {
00864 int num_joints;
00865 int iter_k, iter_j;
00866 std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM;
00867 std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM;
00868 cob_base_drive_chain::GetJointState srv_get_joint;
00869
00870
00871 srv_client_get_joint_state_.call(srv_get_joint);
00872
00873
00874 num_joints = srv_get_joint.response.jointstate.position.size();
00875
00876 drive_joint_ang_rad.assign(num_joints, 0.0);
00877 drive_joint_vel_rads.assign(num_joints, 0.0);
00878 drive_joint_effort_NM.assign(num_joints, 0.0);
00879
00880 steer_joint_ang_rad.assign(num_joints, 0.0);
00881 steer_joint_vel_rads.assign(num_joints, 0.0);
00882 steer_joint_effort_NM.assign(num_joints, 0.0);
00883
00884
00885 iter_k = 0;
00886 iter_j = 0;
00887
00888 for(int i = 0; i < num_joints; i++)
00889 {
00890
00891
00892
00893 if( i == 1 || i == 3 || i == 5 || i == 7)
00894 {
00895 steer_joint_ang_rad[iter_k] = srv_get_joint.response.jointstate.position[i];
00896 steer_joint_vel_rads[iter_k] = srv_get_joint.response.jointstate.velocity[i];
00897 steer_joint_effort_NM[iter_k] = srv_get_joint.response.jointstate.effort[i];
00898 iter_k = iter_k + 1;
00899 }
00900 else
00901 {
00902 drive_joint_ang_rad[iter_j] = srv_get_joint.response.jointstate.position[i];
00903 drive_joint_vel_rads[iter_j] = srv_get_joint.response.jointstate.velocity[i];
00904 drive_joint_effort_NM[iter_j] = srv_get_joint.response.jointstate.effort[i];
00905 iter_j = iter_j + 1;
00906 }
00907 }
00908
00909
00910 ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads,
00911 drive_joint_ang_rad, steer_joint_ang_rad);
00912
00913 }
00914
00915
00916
00917
00918 void NodeClass::UpdateOdometry()
00919 {
00920 double vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
00921 double dummy1, dummy2;
00922 double dt;
00923 ros::Time current_time;
00924
00925
00926
00927 if (is_initialized_bool_)
00928 {
00929
00930
00931
00932
00933 ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
00934 vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);
00935
00936
00937 vel_x_rob_ms = vel_x_rob_ms/1000.0;
00938 vel_y_rob_ms = vel_y_rob_ms/1000.0;
00939 delta_x_rob_m = delta_x_rob_m/1000.0;
00940 delta_y_rob_m = delta_y_rob_m/1000.0;
00941 }
00942 else
00943 {
00944
00945 vel_x_rob_ms = 0.0;
00946 vel_y_rob_ms = 0.0;
00947 delta_x_rob_m = 0.0;
00948 delta_y_rob_m = 0.0;
00949 }
00950
00951
00952
00953 current_time = ros::Time::now();
00954 dt = current_time.toSec() - last_time_.toSec();
00955 last_time_ = current_time;
00956
00957 x_rob_m_ = x_rob_m_ + (vel_x_rob_ms * cos(theta_rob_rad_) - vel_y_rob_ms * sin(theta_rob_rad_)) * dt;
00958 y_rob_m_ = y_rob_m_ + (vel_x_rob_ms * sin(theta_rob_rad_) + vel_y_rob_ms * cos(theta_rob_rad_)) * dt;
00959 theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt;
00960
00961
00962
00963 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);
00964
00965
00966 geometry_msgs::TransformStamped odom_tf;
00967
00968 odom_tf.header.stamp = current_time;
00969 odom_tf.header.frame_id = "/wheelodom";
00970 odom_tf.child_frame_id = "/base_footprint";
00971
00972 odom_tf.transform.translation.x = x_rob_m_;
00973 odom_tf.transform.translation.y = y_rob_m_;
00974 odom_tf.transform.translation.z = 0.0;
00975 odom_tf.transform.rotation = odom_quat;
00976
00977
00978 nav_msgs::Odometry odom_top;
00979
00980 odom_top.header.stamp = current_time;
00981 odom_top.header.frame_id = "/wheelodom";
00982 odom_top.child_frame_id = "/base_footprint";
00983
00984 odom_top.pose.pose.position.x = x_rob_m_;
00985 odom_top.pose.pose.position.y = y_rob_m_;
00986 odom_top.pose.pose.position.z = 0.0;
00987 odom_top.pose.pose.orientation = odom_quat;
00988 for(int i = 0; i < 6; i++)
00989 odom_top.pose.covariance[i*6+i] = 0.1;
00990
00991
00992 odom_top.twist.twist.linear.x = vel_x_rob_ms;
00993 odom_top.twist.twist.linear.y = vel_y_rob_ms;
00994 odom_top.twist.twist.linear.z = 0.0;
00995 odom_top.twist.twist.angular.x = 0.0;
00996 odom_top.twist.twist.angular.y = 0.0;
00997 odom_top.twist.twist.angular.z = rot_rob_rads;
00998 for(int i = 0; i < 6; i++)
00999 odom_top.twist.covariance[6*i+i] = 0.1;
01000
01001
01002
01003
01004 if (fabs(vel_x_rob_ms) > 0.005 or fabs(vel_y_rob_ms) > 0.005 or fabs(rot_rob_rads) > 0.005)
01005 {
01006 is_moving_ = true;
01007 }
01008 else
01009 {
01010 is_moving_ = false;
01011 }
01012
01013
01014 topic_pub_odometry_.publish(odom_top);
01015 }
01016
01017
01018
01019