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 <diagnostic_msgs/DiagnosticStatus.h>
00067 #include <diagnostic_updater/diagnostic_updater.h>
00068 #include <geometry_msgs/Twist.h>
00069 #include <nav_msgs/Odometry.h>
00070 #include <tf/transform_broadcaster.h>
00071 #include <cob_msgs/EmergencyStopState.h>
00072 #include <control_msgs/JointTrajectoryControllerState.h>
00073
00074
00075 #include <cob_omni_drive_controller/UndercarriageCtrlGeomROS.h>
00076 #include <cob_omni_drive_controller/OdometryTracker.h>
00077 #include <vector>
00078 #include <angles/angles.h>
00079
00080
00081
00082 class NodeClass
00083 {
00084
00085 public:
00086
00087 ros::NodeHandle n;
00088
00089
00090 ros::Publisher topic_pub_joint_state_cmd_;
00091 ros::Publisher topic_pub_controller_joint_command_;
00092 ros::Publisher topic_pub_odometry_;
00093 tf::TransformBroadcaster tf_broadcast_odometry_;
00094
00095
00096 ros::Subscriber topic_sub_CMD_pltf_twist_;
00097 ros::Subscriber topic_sub_EM_stop_state_;
00098 ros::Subscriber topic_sub_drive_diagnostic_;
00099
00100
00101
00102 ros::Subscriber topic_sub_joint_controller_states_;
00103
00104
00105 diagnostic_updater::Updater updater_;
00106
00107
00108 ros::Timer timer_ctrl_step_;
00109
00110
00111 UndercarriageCtrl * ucar_ctrl_;
00112 bool is_initialized_bool_;
00113 bool is_ucarr_geom_initialized_bool_;
00114 bool broadcast_tf_;
00115 int drive_chain_diagnostic_;
00116 ros::Time last_time_;
00117 ros::Time joint_state_odom_stamp_;
00118 double sample_time_, timeout_;
00119 int iwatchdog_;
00120 double max_vel_trans_, max_vel_rot_;
00121
00122 OdometryTracker* odom_tracker_;
00123
00124 int m_iNumJoints;
00125 int m_iNumWheels;
00126 bool m_bEMStopActive;
00127
00128 bool has_target;
00129
00130 diagnostic_msgs::DiagnosticStatus diagnostic_status_lookup_;
00131
00132
00133 NodeClass()
00134 : ucar_ctrl_(0)
00135 {
00136
00137 is_initialized_bool_ = false;
00138 is_ucarr_geom_initialized_bool_ = false;
00139 broadcast_tf_ = true;
00140 iwatchdog_ = 0;
00141 last_time_ = ros::Time::now();
00142 sample_time_ = 0.020;
00143
00144 drive_chain_diagnostic_ = diagnostic_status_lookup_.OK;
00145 has_target = false;
00146
00147
00148
00149 if (n.hasParam("timeout"))
00150 {
00151 n.getParam("timeout", timeout_);
00152 ROS_INFO("Timeout loaded from Parameter-Server is: %fs", timeout_);
00153 }
00154 else
00155 {
00156 ROS_WARN("No parameter timeout on Parameter-Server. Using default: 1.0s");
00157 timeout_ = 1.0;
00158 }
00159 if ( timeout_ < sample_time_ )
00160 {
00161 ROS_WARN("Specified timeout < sample_time. Setting timeout to sample_time = %fs", sample_time_);
00162 timeout_ = sample_time_;
00163 }
00164
00165 if (n.hasParam("max_trans_velocity"))
00166 {
00167 n.getParam("max_trans_velocity", max_vel_trans_);
00168 ROS_INFO("Max translational velocity loaded from Parameter-Server is: %fs", max_vel_trans_);
00169 }
00170 else
00171 {
00172 ROS_WARN("No parameter max_trans_velocity on Parameter-Server. Using default: 1.1 m/s");
00173 max_vel_trans_ = 1.1;
00174 }
00175 if (n.hasParam("max_rot_velocity"))
00176 {
00177 n.getParam("max_rot_velocity", max_vel_rot_);
00178 ROS_INFO("Max rotational velocity loaded from Parameter-Server is: %fs", max_vel_rot_);
00179 }
00180 else
00181 {
00182 ROS_WARN("No parameter max_rot_velocity on Parameter-Server. Using default: 1.8 rad/s");
00183 max_vel_rot_ = 1.8;
00184 }
00185 if (n.hasParam("broadcast_tf"))
00186 {
00187 n.getParam("broadcast_tf", broadcast_tf_);
00188 }
00189
00190 const std::string frame_id = n.param("frame_id", std::string("odom"));
00191 const std::string child_frame_id = n.param("child_frame_id", std::string("base_footprint"));
00192 const double cov_pose = n.param("cov_pose", 0.1);
00193 const double cov_twist = n.param("cov_twist", 0.1);
00194
00195 odom_tracker_ = new OdometryTracker(frame_id, child_frame_id, cov_pose, cov_twist);
00196
00197
00198 std::vector<UndercarriageCtrl::WheelParams> wps;
00199
00200
00201 if(cob_omni_drive_controller::parseWheelParams(wps,n)){
00202
00203 m_iNumWheels = wps.size();
00204 m_iNumJoints = m_iNumWheels * 2;
00205
00206 ucar_ctrl_ = new UndercarriageCtrl(wps);
00207 is_ucarr_geom_initialized_bool_ = true;
00208 }
00209
00210
00211
00212
00213 topic_pub_controller_joint_command_ = n.advertise<control_msgs::JointTrajectoryControllerState> ("joint_command", 1);
00214
00215 topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("odometry", 1);
00216
00217
00218 topic_sub_CMD_pltf_twist_ = n.subscribe("command", 1, &NodeClass::topicCallbackTwistCmd, this);
00219 topic_sub_EM_stop_state_ = n.subscribe("/emergency_stop_state", 1, &NodeClass::topicCallbackEMStop, this);
00220 topic_sub_drive_diagnostic_ = n.subscribe("diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
00221
00222
00223
00224
00225 topic_sub_joint_controller_states_ = n.subscribe("state", 1, &NodeClass::topicCallbackJointControllerStates, this);
00226
00227
00228 updater_.setHardwareID(ros::this_node::getName());
00229 updater_.add("initialization", this, &NodeClass::diag_init);
00230
00231
00232 timer_ctrl_step_ = n.createTimer(ros::Duration(sample_time_), &NodeClass::timerCallbackCtrlStep, this);
00233
00234 if (is_ucarr_geom_initialized_bool_)
00235 is_initialized_bool_ = true;
00236 }
00237
00238
00239
00240 ~NodeClass()
00241 {
00242 topic_sub_CMD_pltf_twist_.shutdown();
00243 topic_sub_EM_stop_state_.shutdown();
00244 topic_sub_drive_diagnostic_.shutdown();
00245 topic_sub_joint_controller_states_.shutdown();
00246
00247 timer_ctrl_step_.stop();
00248 delete ucar_ctrl_;
00249 delete odom_tracker_;
00250 }
00251
00252 void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
00253 {
00254 if(is_initialized_bool_)
00255 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
00256 else
00257 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
00258 stat.add("Initialized", is_initialized_bool_);
00259 }
00260
00261
00262 void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg)
00263 {
00264
00265 PlatformState pltState;
00266
00267 if( (fabs(msg->linear.x) > max_vel_trans_) || (fabs(msg->linear.y) > max_vel_trans_) || (fabs(msg->angular.z) > max_vel_rot_)){
00268 if(fabs(msg->linear.x) > max_vel_trans_){
00269 ROS_DEBUG_STREAM("Recevied cmdVelX: " << msg->linear.x <<
00270 ", which is bigger than the maximal allowed translational velocity: " << max_vel_trans_ << " so stop the robot");
00271 }
00272 if(fabs(msg->linear.y) > max_vel_trans_){
00273 ROS_DEBUG_STREAM("Recevied cmdVelY: " << msg->linear.x <<
00274 ", which is bigger than the maximal allowed translational velocity: " << max_vel_trans_ << " so stop the robot");
00275 }
00276 if(fabs(msg->angular.z) > max_vel_rot_){
00277 ROS_DEBUG_STREAM("Recevied cmdVelTh: " << msg->angular.z <<
00278 ", which is bigger than the maximal allowed rotational velocity: " << max_vel_rot_ << " so stop the robot");
00279 }
00280
00281
00282
00283 }
00284 else{
00285
00286 pltState.setVelX(msg->linear.x);
00287 pltState.setVelY(msg->linear.y);
00288 pltState.dRotRobRadS = msg->angular.z;
00289 }
00290
00291 iwatchdog_ = 0;
00292
00293
00294 if (is_initialized_bool_ && drive_chain_diagnostic_ == diagnostic_status_lookup_.OK){
00295 ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]",
00296 msg->linear.x, msg->linear.y, msg->angular.z);
00297
00298
00299
00300 has_target = msg->linear.x!= 0 || msg->linear.y!=0 || msg->angular.z != 0;
00301 ucar_ctrl_->setTarget(pltState);
00302 }
00303 else{
00304
00305
00306
00307 pltState = PlatformState();
00308 ucar_ctrl_->setTarget(pltState);
00309
00310 ROS_DEBUG("Forced platform-velocity cmds to zero");
00311 }
00312 }
00313
00314
00315 void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg)
00316 {
00317 int current_EM_state = msg->emergency_state;
00318
00319 if (current_EM_state == msg->EMFREE){
00320
00321 if (is_initialized_bool_){
00322 setEMStopActive(false);
00323 ROS_DEBUG("Undercarriage Controller EM-Stop released");
00324 }
00325 }
00326 else{
00327 ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop");
00328
00329
00330
00331 PlatformState pltState;
00332 ucar_ctrl_->setTarget(pltState);
00333
00334 ROS_DEBUG("Forced platform-velocity cmds to zero");
00335
00336
00337 setEMStopActive(true);
00338 }
00339 }
00340
00341
00342 void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
00343 {
00344 control_msgs::JointTrajectoryControllerState joint_state_cmd;
00345
00346
00347 joint_state_cmd.header.stamp = ros::Time::now();
00348
00349
00350
00351
00352 joint_state_cmd.desired.positions.resize(m_iNumJoints);
00353 joint_state_cmd.desired.velocities.resize(m_iNumJoints);
00354
00355
00356
00357 joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint");
00358 joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint");
00359 joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint");
00360 joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint");
00361 joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint");
00362 joint_state_cmd.joint_names.push_back("br_caster_rotation_joint");
00363 joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint");
00364 joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint");
00365 joint_state_cmd.joint_names.resize(m_iNumJoints);
00366
00367
00368 for(int i=0; i<m_iNumJoints; i++)
00369 {
00370 joint_state_cmd.desired.positions[i] = 0.0;
00371 joint_state_cmd.desired.velocities[i] = 0.0;
00372
00373 }
00374
00375
00376 drive_chain_diagnostic_ = msg->level;
00377
00378
00379 if (is_initialized_bool_){
00380
00381 if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK){
00382 has_target = false;
00383
00384 ROS_DEBUG("drive chain not availlable: halt Controller");
00385
00386
00387 setEMStopActive(true);
00388
00389
00390
00391 PlatformState pltState;
00392 ucar_ctrl_->setTarget(pltState);
00393
00394 ROS_DEBUG("Forced platform-velocity cmds to zero");
00395 }
00396 }
00397
00398 else
00399 {
00400
00401 if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
00402 {
00403
00404 topic_pub_controller_joint_command_.publish(joint_state_cmd);
00405 }
00406 }
00407 }
00408
00409 void topicCallbackJointControllerStates(const control_msgs::JointTrajectoryControllerState::ConstPtr& msg) {
00410 int num_joints = msg->joint_names.size();
00411
00412
00413 std::vector<WheelState> wStates;
00414 wStates.assign(m_iNumWheels, WheelState());
00415
00416 joint_state_odom_stamp_ = msg->header.stamp;
00417
00418 for(int i = 0; i < num_joints; i++)
00419 {
00420
00421
00422 if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
00423 {
00424 wStates[0].dVelGearDriveRadS = msg->actual.velocities[i];
00425 }
00426 if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
00427 {
00428 wStates[1].dVelGearDriveRadS = msg->actual.velocities[i];
00429 }
00430 if(msg->joint_names[i] == "br_caster_r_wheel_joint")
00431 {
00432 wStates[2].dVelGearDriveRadS = msg->actual.velocities[i];
00433 }
00434 if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
00435 {
00436 wStates[3].dVelGearDriveRadS = msg->actual.velocities[i];
00437 }
00438 if(msg->joint_names[i] == "fl_caster_rotation_joint")
00439 {
00440 wStates[0].dAngGearSteerRad = msg->actual.positions[i];
00441 wStates[0].dVelGearSteerRadS = msg->actual.velocities[i];
00442 }
00443 if(msg->joint_names[i] == "bl_caster_rotation_joint")
00444 {
00445 wStates[1].dAngGearSteerRad = msg->actual.positions[i];
00446 wStates[1].dVelGearSteerRadS = msg->actual.velocities[i];
00447 }
00448 if(msg->joint_names[i] == "br_caster_rotation_joint")
00449 {
00450 wStates[2].dAngGearSteerRad = msg->actual.positions[i];
00451 wStates[2].dVelGearSteerRadS = msg->actual.velocities[i];
00452 }
00453 if(msg->joint_names[i] == "fr_caster_rotation_joint")
00454 {
00455 wStates[3].dAngGearSteerRad = msg->actual.positions[i];
00456 wStates[3].dVelGearSteerRadS = msg->actual.velocities[i];
00457 }
00458 }
00459
00460
00461 ucar_ctrl_->updateWheelStates(wStates);
00462
00463
00464 UpdateOdometry();
00465 }
00466
00467 void timerCallbackCtrlStep(const ros::TimerEvent& e) {
00468 CalcCtrlStep();
00469 }
00470
00471
00472
00473 bool InitCtrl();
00474
00475 void CalcCtrlStep();
00476
00477
00478 void UpdateOdometry();
00479
00480
00481 void setEMStopActive(bool bEMStopActive);
00482
00483 };
00484
00485
00486
00487 int main(int argc, char** argv)
00488 {
00489
00490 ros::init(argc, argv, "undercarriage_ctrl");
00491
00492
00493
00494 NodeClass nodeClass;
00495
00496
00497 if( nodeClass.is_initialized_bool_ ) {
00498 nodeClass.last_time_ = ros::Time::now();
00499 ROS_INFO("New Undercarriage control successfully initialized.");
00500 } else {
00501 ROS_FATAL("Undercarriage control initialization failed!");
00502 throw std::runtime_error("Undercarriage control initialization failed, check yaml-Files!");
00503 }
00504
00505
00506
00507
00508
00509
00510
00511 ros::spin();
00512
00513 return 0;
00514 }
00515
00516
00517
00518
00519
00520 void NodeClass::CalcCtrlStep()
00521 {
00522
00523 std::vector<WheelCommand> wStates;
00524 wStates.assign(m_iNumWheels, WheelCommand());
00525
00526
00527 control_msgs::JointTrajectoryControllerState joint_state_cmd;
00528
00529 int j = 0, k = 0;
00530 iwatchdog_ += 1;
00531
00532
00533 if (is_initialized_bool_)
00534 {
00535
00536
00537
00538
00539
00540
00541
00542 ucar_ctrl_->calcControlStep(wStates, sample_time_, false);
00543
00544
00545 if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK){
00546 for(int i = 0; i < wStates.size(); i++){
00547 wStates[i].dAngGearSteerRad = 0.0;
00548 wStates[i].dVelGearSteerRadS = 0.0;
00549 }
00550 }
00551
00552
00553
00554
00555 joint_state_cmd.header.stamp = ros::Time::now();
00556
00557
00558
00559
00560
00561
00562
00563
00564 joint_state_cmd.desired.positions.resize(m_iNumJoints);
00565 joint_state_cmd.desired.velocities.resize(m_iNumJoints);
00566
00567 joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint");
00568 joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint");
00569 joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint");
00570 joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint");
00571 joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint");
00572 joint_state_cmd.joint_names.push_back("br_caster_rotation_joint");
00573 joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint");
00574 joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint");
00575 joint_state_cmd.joint_names.resize(m_iNumJoints);
00576
00577
00578 for(int i = 0; i<m_iNumJoints; i++)
00579 {
00580 if(iwatchdog_ < (int) std::floor(timeout_/sample_time_) && has_target)
00581 {
00582
00583 if( i == 1 || i == 3 || i == 5 || i == 7)
00584 {
00585 joint_state_cmd.desired.positions[i] = wStates[j].dAngGearSteerRad;
00586 joint_state_cmd.desired.velocities[i] = wStates[j].dVelGearSteerRadS;
00587 j = j + 1;
00588 }
00589 else
00590 {
00591 joint_state_cmd.desired.positions[i] = 0.0;
00592 joint_state_cmd.desired.velocities[i] = wStates[k].dVelGearDriveRadS;
00593 k = k + 1;
00594 }
00595 }
00596 else
00597 {
00598 joint_state_cmd.desired.positions[i] = 0.0;
00599 joint_state_cmd.desired.velocities[i] = 0.0;
00600 }
00601 }
00602
00603
00604 topic_pub_controller_joint_command_.publish(joint_state_cmd);
00605 }
00606
00607 }
00608
00609
00610
00611 void NodeClass::UpdateOdometry()
00612 {
00613
00614
00615 PlatformState pltState;
00616 if (is_initialized_bool_)
00617 {
00618
00619
00620
00621
00622 ucar_ctrl_->calcDirect(pltState);
00623 }
00624 odom_tracker_->track(joint_state_odom_stamp_, (joint_state_odom_stamp_-last_time_).toSec(), pltState.getVelX(), pltState.getVelY(), pltState.dRotRobRadS);
00625 last_time_ = joint_state_odom_stamp_;
00626
00627 nav_msgs::Odometry odom_top = odom_tracker_->getOdometry();
00628
00629 if (broadcast_tf_ == true)
00630 {
00631
00632 geometry_msgs::TransformStamped odom_tf;
00633
00634 odom_tf.header.stamp = odom_top.header.stamp;
00635 odom_tf.header.frame_id = "/odom_combined";
00636 odom_tf.child_frame_id = "/base_footprint";
00637
00638 odom_tf.transform.translation.x = odom_top.pose.pose.position.x;
00639 odom_tf.transform.translation.y = odom_top.pose.pose.position.y;
00640 odom_tf.transform.rotation = odom_top.pose.pose.orientation;
00641
00642
00643 tf_broadcast_odometry_.sendTransform(odom_tf);
00644 }
00645
00646
00647 topic_pub_odometry_.publish(odom_top);
00648 }
00649
00650
00651 void NodeClass::setEMStopActive(bool bEMStopActive)
00652 {
00653 m_bEMStopActive = bEMStopActive;
00654
00655 std::vector<WheelState> wStates;
00656 wStates.assign(m_iNumWheels, WheelState());
00657
00658 std::vector<WheelCommand> wCommands;
00659 wCommands.assign(m_iNumWheels, WheelCommand());
00660
00661
00662 if(m_bEMStopActive)
00663 {
00664
00665 has_target = false;
00666
00667 ucar_ctrl_->calcControlStep(wCommands, sample_time_, true);
00668
00669
00670 ucar_ctrl_->updateWheelStates(wStates);
00671 }
00672
00673
00674 }
00675
00676
00677