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
00061 #include <ros/ros.h>
00062
00063
00064 #include <std_msgs/Float64.h>
00065 #include <sensor_msgs/JointState.h>
00066 #include <diagnostic_msgs/DiagnosticStatus.h>
00067 #include <diagnostic_msgs/DiagnosticArray.h>
00068 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00069 #include <pr2_controllers_msgs/JointControllerState.h>
00070
00071
00072 #include <cob_srvs/Trigger.h>
00073 #include <cob_base_drive_chain/ElmoRecorderReadout.h>
00074 #include <cob_base_drive_chain/ElmoRecorderConfig.h>
00075
00076
00077
00078 #include <cob_base_drive_chain/CanCtrlPltfCOb3.h>
00079 #include <cob_utilities/IniFile.h>
00080 #include <cob_utilities/MathSup.h>
00081
00082
00083
00087 class NodeClass
00088 {
00089 public:
00090
00091 ros::NodeHandle n;
00092
00093
00097 ros::Publisher topicPub_JointState;
00098
00099 ros::Publisher topicPub_ControllerState;
00100 ros::Publisher topicPub_DiagnosticGlobal_;
00101
00102
00106 ros::Publisher topicPub_Diagnostic;
00107
00108
00109
00113 ros::Subscriber topicSub_JointStateCmd;
00114
00115
00119 ros::ServiceServer srvServer_Init;
00120
00124 ros::ServiceServer srvServer_Recover;
00125
00129 ros::ServiceServer srvServer_Shutdown;
00130
00131 ros::ServiceServer srvServer_SetMotionType;
00132
00138 ros::ServiceServer srvServer_ElmoRecorderConfig;
00139
00154 ros::ServiceServer srvServer_ElmoRecorderReadout;
00155
00156
00157
00158 #ifdef __SIM__
00159 ros::Publisher br_steer_pub;
00160 ros::Publisher bl_steer_pub;
00161 ros::Publisher fr_steer_pub;
00162 ros::Publisher fl_steer_pub;
00163 ros::Publisher br_caster_pub;
00164 ros::Publisher bl_caster_pub;
00165 ros::Publisher fr_caster_pub;
00166 ros::Publisher fl_caster_pub;
00167
00168 std::vector<double> m_gazeboPos;
00169 std::vector<double> m_gazeboVel;
00170 ros::Time m_gazeboStamp;
00171
00172 ros::Subscriber topicSub_GazeboJointStates;
00173 #else
00174 CanCtrlPltfCOb3 *m_CanCtrlPltf;
00175 #endif
00176 bool m_bisInitialized;
00177 int m_iNumMotors;
00178 int m_iNumDrives;
00179
00180 struct ParamType
00181 {
00182 double dMaxDriveRateRadpS;
00183 double dMaxSteerRateRadpS;
00184
00185 std::vector<double> vdWheelNtrlPosRad;
00186 };
00187 ParamType m_Param;
00188
00189 std::string sIniDirectory;
00190 bool m_bPubEffort;
00191 bool m_bReadoutElmo;
00192
00193
00194 NodeClass()
00195 {
00197
00198 if (n.hasParam("IniDirectory"))
00199 {
00200 n.getParam("IniDirectory", sIniDirectory);
00201 ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
00202 }
00203 else
00204 {
00205 sIniDirectory = "Platform/IniFiles/";
00206 ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
00207 }
00208
00209 n.param<bool>("PublishEffort", m_bPubEffort, false);
00210 if(m_bPubEffort) ROS_INFO("You have choosen to publish effort of motors, that charges capacity of CAN");
00211
00212
00213 IniFile iniFile;
00214 iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
00215
00216
00217 iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
00218 iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
00219 if(m_iNumMotors < 2 || m_iNumMotors > 8) {
00220 m_iNumMotors = 8;
00221 m_iNumDrives = 4;
00222 }
00223
00224 #ifdef __SIM__
00225 bl_caster_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_r_wheel_controller/command", 1);
00226 br_caster_pub = n.advertise<std_msgs::Float64>("/base_br_caster_r_wheel_controller/command", 1);
00227 fl_caster_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_r_wheel_controller/command", 1);
00228 fr_caster_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_r_wheel_controller/command", 1);
00229 bl_steer_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_rotation_controller/command", 1);
00230 br_steer_pub = n.advertise<std_msgs::Float64>("/base_br_caster_rotation_controller/command", 1);
00231 fl_steer_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_rotation_controller/command", 1);
00232 fr_steer_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_rotation_controller/command", 1);
00233
00234 topicSub_GazeboJointStates = n.subscribe("/joint_states", 1, &NodeClass::gazebo_joint_states_Callback, this);
00235
00236 m_gazeboPos.resize(m_iNumMotors);
00237 m_gazeboVel.resize(m_iNumMotors);
00238 #else
00239 m_CanCtrlPltf = new CanCtrlPltfCOb3(sIniDirectory);
00240 #endif
00241
00242
00243
00244 topicPub_JointState = n.advertise<sensor_msgs::JointState>("/joint_states", 1);
00245 topicPub_ControllerState = n.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
00246 topicPub_DiagnosticGlobal_ = n.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00247
00248 topicPub_Diagnostic = n.advertise<diagnostic_msgs::DiagnosticStatus>("diagnostic", 1);
00249
00250 topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this);
00251
00252
00253 srvServer_Init = n.advertiseService("init", &NodeClass::srvCallback_Init, this);
00254 srvServer_ElmoRecorderConfig = n.advertiseService("ElmoRecorderConfig", &NodeClass::srvCallback_ElmoRecorderConfig, this);
00255 srvServer_ElmoRecorderReadout = n.advertiseService("ElmoRecorderReadout", &NodeClass::srvCallback_ElmoRecorderReadout, this);
00256 m_bReadoutElmo = false;
00257
00258 srvServer_Recover = n.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
00259 srvServer_Shutdown = n.advertiseService("shutdown", &NodeClass::srvCallback_Shutdown, this);
00260
00261
00262 #ifdef __SIM__
00263 m_bisInitialized = initDrives();
00264 #else
00265 m_bisInitialized = false;
00266 #endif
00267
00268 }
00269
00270
00271 ~NodeClass()
00272 {
00273 #ifdef __SIM__
00274
00275 #else
00276 m_CanCtrlPltf->shutdownPltf();
00277 #endif
00278 }
00279
00280
00281
00282 void topicCallback_JointStateCmd(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00283 {
00284 ROS_DEBUG("Topic Callback joint_command");
00285
00286 if(m_bisInitialized == true)
00287 {
00288 ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
00289 sensor_msgs::JointState JointStateCmd;
00290 JointStateCmd.position.resize(m_iNumMotors);
00291 JointStateCmd.velocity.resize(m_iNumMotors);
00292 JointStateCmd.effort.resize(m_iNumMotors);
00293
00294 for(unsigned int i = 0; i < msg->joint_names.size(); i++)
00295 {
00296
00297
00298
00299
00300
00301 if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
00302 {
00303 JointStateCmd.position[0] = msg->desired.positions[i];
00304 JointStateCmd.velocity[0] = msg->desired.velocities[i];
00305
00306 }
00307 else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_r_wheel_joint")
00308 {
00309 JointStateCmd.position[2] = msg->desired.positions[i];
00310 JointStateCmd.velocity[2] = msg->desired.velocities[i];
00311
00312 }
00313 else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_r_wheel_joint")
00314 {
00315 JointStateCmd.position[4] = msg->desired.positions[i];
00316 JointStateCmd.velocity[4] = msg->desired.velocities[i];
00317
00318 }
00319 else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_r_wheel_joint")
00320 {
00321 JointStateCmd.position[6] = msg->desired.positions[i];
00322 JointStateCmd.velocity[6] = msg->desired.velocities[i];
00323
00324 }
00325
00326 else if(msg->joint_names[i] == "fl_caster_rotation_joint")
00327 {
00328 JointStateCmd.position[1] = msg->desired.positions[i];
00329 JointStateCmd.velocity[1] = msg->desired.velocities[i];
00330
00331 }
00332 else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_rotation_joint")
00333 {
00334 JointStateCmd.position[3] = msg->desired.positions[i];
00335 JointStateCmd.velocity[3] = msg->desired.velocities[i];
00336
00337 }
00338 else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_rotation_joint")
00339 {
00340 JointStateCmd.position[5] = msg->desired.positions[i];
00341 JointStateCmd.velocity[5] = msg->desired.velocities[i];
00342
00343 }
00344 else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_rotation_joint")
00345 {
00346 JointStateCmd.position[7] = msg->desired.positions[i];
00347 JointStateCmd.velocity[7] = msg->desired.velocities[i];
00348
00349 }
00350 else
00351 {
00352 ROS_ERROR("Unkown joint name");
00353 }
00354 }
00355
00356
00357
00358 for(int i = 0; i < m_iNumMotors; i++)
00359 {
00360 #ifdef __SIM__
00361 #else
00362
00363 if( i == 1 || i == 3 || i == 5 || i == 7)
00364 {
00365 if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
00366 {
00367 JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
00368 }
00369 if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
00370 {
00371 JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
00372 }
00373 }
00374
00375 else
00376 {
00377 if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
00378 {
00379 JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
00380 }
00381 if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
00382 {
00383 JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS;
00384 }
00385 }
00386 #endif
00387 #ifdef __SIM__
00388 ROS_DEBUG("Send velocity data to gazebo");
00389 std_msgs::Float64 fl;
00390 fl.data = JointStateCmd.velocity[i];
00391 if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
00392 fl_caster_pub.publish(fl);
00393 if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
00394 fr_caster_pub.publish(fl);
00395 if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
00396 bl_caster_pub.publish(fl);
00397 if(msg->joint_names[i] == "br_caster_r_wheel_joint")
00398 br_caster_pub.publish(fl);
00399
00400 if(msg->joint_names[i] == "fl_caster_rotation_joint")
00401 fl_steer_pub.publish(fl);
00402 if(msg->joint_names[i] == "fr_caster_rotation_joint")
00403 fr_steer_pub.publish(fl);
00404 if(msg->joint_names[i] == "bl_caster_rotation_joint")
00405 bl_steer_pub.publish(fl);
00406 if(msg->joint_names[i] == "br_caster_rotation_joint")
00407 br_steer_pub.publish(fl);
00408 ROS_DEBUG("Successfully sent velicities to gazebo");
00409 #else
00410 ROS_DEBUG("Send velocity data to drives");
00411 m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]);
00412 ROS_DEBUG("Successfully sent velicities to drives");
00413 #endif
00414 }
00415
00416 #ifdef __SIM__
00417
00418 #else
00419 if(m_bPubEffort) {
00420 m_CanCtrlPltf->requestMotorTorque();
00421 }
00422 #endif
00423 }
00424 }
00425
00426
00427
00428
00429
00430 bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00431 cob_srvs::Trigger::Response &res )
00432 {
00433 ROS_DEBUG("Service Callback init");
00434 if(m_bisInitialized == false)
00435 {
00436 m_bisInitialized = initDrives();
00437
00438
00439 res.success.data = m_bisInitialized;
00440 if(m_bisInitialized)
00441 {
00442 ROS_INFO("base initialized");
00443 }
00444 else
00445 {
00446 res.error_message.data = "initialization of base failed";
00447 ROS_ERROR("Initializing base failed");
00448 }
00449 }
00450 else
00451 {
00452 ROS_WARN("...base already initialized...");
00453 res.success.data = true;
00454 res.error_message.data = "platform already initialized";
00455 }
00456 return true;
00457 }
00458
00459 bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req,
00460 cob_base_drive_chain::ElmoRecorderConfig::Response &res ){
00461 if(m_bisInitialized)
00462 {
00463 #ifdef __SIM__
00464 res.success = true;
00465 #else
00466 m_CanCtrlPltf->evalCanBuffer();
00467 res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
00468 #endif
00469 res.message = "Successfully configured all motors for instant record";
00470 }
00471
00472 return true;
00473 }
00474
00475 bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req,
00476 cob_base_drive_chain::ElmoRecorderReadout::Response &res ){
00477 if(m_bisInitialized) {
00478 #ifdef __SIM__
00479 res.success = true;
00480 #else
00481 m_CanCtrlPltf->evalCanBuffer();
00482 res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
00483 #endif
00484 if(res.success == 0) {
00485 res.message = "Successfully requested reading out of Recorded data";
00486 m_bReadoutElmo = true;
00487 ROS_WARN("CPU consuming evalCanBuffer used for ElmoReadout activated");
00488 } else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
00489 else if(res.success == 2) res.message = "A previous transmission is still in progress";
00490 }
00491
00492 return true;
00493 }
00494
00495
00496
00497
00498 bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
00499 cob_srvs::Trigger::Response &res )
00500 {
00501 if(m_bisInitialized)
00502 {
00503 ROS_DEBUG("Service callback reset");
00504 #ifdef __SIM__
00505 res.success.data = true;
00506 #else
00507 res.success.data = m_CanCtrlPltf->resetPltf();
00508 #endif
00509 if (res.success.data) {
00510 ROS_INFO("base resetted");
00511 } else {
00512 res.error_message.data = "reset of base failed";
00513 ROS_WARN("Resetting base failed");
00514 }
00515 }
00516 else
00517 {
00518 ROS_WARN("...base already recovered...");
00519 res.success.data = true;
00520 res.error_message.data = "base already recovered";
00521 }
00522
00523 return true;
00524 }
00525
00526
00527 bool srvCallback_Shutdown(cob_srvs::Trigger::Request &req,
00528 cob_srvs::Trigger::Response &res )
00529 {
00530 ROS_DEBUG("Service callback shutdown");
00531 #ifdef __SIM__
00532 res.success.data = true;
00533 #else
00534 res.success.data = m_CanCtrlPltf->shutdownPltf();
00535 #endif
00536 if (res.success.data)
00537 ROS_INFO("Drives shut down");
00538 else
00539 ROS_INFO("Shutdown of Drives FAILED");
00540
00541 return true;
00542 }
00543
00544
00545 bool publish_JointStates()
00546 {
00547
00548 int j, k;
00549 bool bIsError;
00550 std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
00551
00552
00553 vdAngGearRad.resize(m_iNumMotors, 0);
00554 vdVelGearRad.resize(m_iNumMotors, 0);
00555 vdEffortGearNM.resize(m_iNumMotors, 0);
00556
00557
00558 sensor_msgs::JointState jointstate;
00559 diagnostic_msgs::DiagnosticStatus diagnostics;
00560 pr2_controllers_msgs::JointTrajectoryControllerState controller_state;
00561
00562
00563 #ifdef __SIM__
00564 jointstate.header.stamp = m_gazeboStamp;
00565 controller_state.header.stamp = m_gazeboStamp;
00566 #else
00567 jointstate.header.stamp = ros::Time::now();
00568 controller_state.header.stamp = jointstate.header.stamp;
00569 #endif
00570
00571
00572
00573 jointstate.position.assign(m_iNumMotors, 0.0);
00574 jointstate.velocity.assign(m_iNumMotors, 0.0);
00575 jointstate.effort.assign(m_iNumMotors, 0.0);
00576
00577 if(m_bisInitialized == false)
00578 {
00579
00580 bIsError = false;
00581
00582 j = 0;
00583 k = 0;
00584
00585
00586 for(int i = 0; i<m_iNumMotors; i++)
00587 {
00588 jointstate.position[i] = 0.0;
00589 jointstate.velocity[i] = 0.0;
00590 jointstate.effort[i] = 0.0;
00591 }
00592 jointstate.name.push_back("fl_caster_r_wheel_joint");
00593 jointstate.name.push_back("fl_caster_rotation_joint");
00594 jointstate.name.push_back("bl_caster_r_wheel_joint");
00595 jointstate.name.push_back("bl_caster_rotation_joint");
00596 jointstate.name.push_back("br_caster_r_wheel_joint");
00597 jointstate.name.push_back("br_caster_rotation_joint");
00598 jointstate.name.push_back("fr_caster_r_wheel_joint");
00599 jointstate.name.push_back("fr_caster_rotation_joint");
00600
00601 }
00602 else
00603 {
00604
00605
00606 #ifdef __SIM__
00607
00608 #else
00609 ROS_DEBUG("Read CAN-Buffer");
00610 m_CanCtrlPltf->evalCanBuffer();
00611 ROS_DEBUG("Successfully read CAN-Buffer");
00612 #endif
00613 j = 0;
00614 k = 0;
00615 for(int i = 0; i<m_iNumMotors; i++)
00616 {
00617 #ifdef __SIM__
00618 vdAngGearRad[i] = m_gazeboPos[i];
00619 vdVelGearRad[i] = m_gazeboVel[i];
00620 #else
00621 m_CanCtrlPltf->getGearPosVelRadS(i, &vdAngGearRad[i], &vdVelGearRad[i]);
00622 #endif
00623
00624
00625 if(m_bPubEffort) {
00626 for(int i=0; i<m_iNumMotors; i++)
00627 {
00628 #ifdef __SIM__
00629
00630 #else
00631 m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]);
00632 #endif
00633 }
00634 }
00635
00636
00637 if( i == 1 || i == 3 || i == 5 || i == 7)
00638 {
00639
00640 vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
00641 MathSup::normalizePi(vdAngGearRad[i]);
00642 j = j+1;
00643 }
00644 }
00645
00646
00647 for(int i = 0; i<m_iNumMotors; i++)
00648 {
00649 jointstate.position[i] = vdAngGearRad[i];
00650 jointstate.velocity[i] = vdVelGearRad[i];
00651 jointstate.effort[i] = vdEffortGearNM[i];
00652 }
00653
00654 jointstate.name.push_back("fl_caster_r_wheel_joint");
00655 jointstate.name.push_back("fl_caster_rotation_joint");
00656 jointstate.name.push_back("bl_caster_r_wheel_joint");
00657 jointstate.name.push_back("bl_caster_rotation_joint");
00658 jointstate.name.push_back("br_caster_r_wheel_joint");
00659 jointstate.name.push_back("br_caster_rotation_joint");
00660 jointstate.name.push_back("fr_caster_r_wheel_joint");
00661 jointstate.name.push_back("fr_caster_rotation_joint");
00662
00663 }
00664
00665 controller_state.joint_names = jointstate.name;
00666 controller_state.actual.positions = jointstate.position;
00667 controller_state.actual.velocities = jointstate.velocity;
00668
00669
00670 topicPub_JointState.publish(jointstate);
00671 topicPub_ControllerState.publish(controller_state);
00672
00673 ROS_DEBUG("published new drive-chain configuration (JointState message)");
00674
00675
00676 if(m_bisInitialized)
00677 {
00678
00679 #ifdef __SIM__
00680 bIsError = false;
00681 #else
00682 bIsError = m_CanCtrlPltf->isPltfError();
00683 #endif
00684 }
00685
00686
00687 if(bIsError)
00688 {
00689 diagnostics.level = 2;
00690 diagnostics.name = "drive-chain can node";
00691 diagnostics.message = "one or more drives are in Error mode";
00692 }
00693 else
00694 {
00695 if (m_bisInitialized)
00696 {
00697 diagnostics.level = 0;
00698 diagnostics.name = "drive-chain can node";
00699 diagnostics.message = "drives operating normal";
00700 }
00701 else
00702 {
00703 diagnostics.level = 1;
00704 diagnostics.name = "drive-chain can node";
00705 diagnostics.message = "drives are initializing";
00706 }
00707 }
00708
00709
00710 topicPub_Diagnostic.publish(diagnostics);
00711 ROS_DEBUG("published new drive-chain configuration (JointState message)");
00712
00713 diagnostic_msgs::DiagnosticArray diagnostics_gl;
00714 diagnostics_gl.status.resize(1);
00715
00716 if(bIsError)
00717 {
00718 diagnostics_gl.status[0].level = 2;
00719 diagnostics_gl.status[0].name = n.getNamespace();;
00720
00721 }
00722 else
00723 {
00724 if (m_bisInitialized)
00725 {
00726 diagnostics_gl.status[0].level = 0;
00727 diagnostics_gl.status[0].name = n.getNamespace();
00728 diagnostics_gl.status[0].message = "base_drive_chain initialized and running";
00729 }
00730 else
00731 {
00732 diagnostics_gl.status[0].level = 1;
00733 diagnostics_gl.status[0].name = n.getNamespace();
00734 diagnostics_gl.status[0].message = "base_drive_chain not initialized";
00735 }
00736 }
00737
00738 topicPub_DiagnosticGlobal_.publish(diagnostics_gl);
00739
00740 return true;
00741 }
00742
00743
00744 bool initDrives();
00745
00746 #ifdef __SIM__
00747 void gazebo_joint_states_Callback(const sensor_msgs::JointState::ConstPtr& msg) {
00748 for (unsigned int i=0; i<msg->name.size(); i++) {
00749
00750 if(msg->name[i] == "fl_caster_r_wheel_joint") {
00751 m_gazeboPos[0] = msg->position[i];
00752 m_gazeboVel[0] = msg->velocity[i];
00753 m_gazeboStamp = msg->header.stamp;
00754 }
00755 else if(msg->name[i] == "bl_caster_r_wheel_joint") {
00756 m_gazeboPos[2] = msg->position[i];
00757 m_gazeboVel[2] = msg->velocity[i];
00758 }
00759 else if(msg->name[i] == "br_caster_r_wheel_joint") {
00760 m_gazeboPos[4] = msg->position[i];
00761 m_gazeboVel[4] = msg->velocity[i];
00762 }
00763 else if(msg->name[i] == "fr_caster_r_wheel_joint") {
00764 m_gazeboPos[6] = msg->position[i];
00765 m_gazeboVel[6] = msg->velocity[i];
00766 }
00767
00768
00769 else if(msg->name[i] == "fl_caster_rotation_joint") {
00770 m_gazeboPos[1] = msg->position[i];
00771 m_gazeboVel[1] = msg->velocity[i];
00772 }
00773 else if(msg->name[i] == "bl_caster_rotation_joint") {
00774 m_gazeboPos[3] = msg->position[i];
00775 m_gazeboVel[3] = msg->velocity[i];
00776 }
00777 else if(msg->name[i] == "br_caster_rotation_joint") {
00778 m_gazeboPos[5] = msg->position[i];
00779 m_gazeboVel[5] = msg->velocity[i];
00780 }
00781 else if(msg->name[i] == "fr_caster_rotation_joint") {
00782 m_gazeboPos[7] = msg->position[i];
00783 m_gazeboVel[7] = msg->velocity[i];
00784 }
00785 }
00786 }
00787 #else
00788
00789 #endif
00790 };
00791
00792
00793
00794 int main(int argc, char** argv)
00795 {
00796
00797 ros::init(argc, argv, "base_drive_chain");
00798
00799 NodeClass nodeClass;
00800
00801
00802 ros::Rate loop_rate(100);
00803
00804 while(nodeClass.n.ok())
00805 {
00806 #ifdef __SIM__
00807
00808 #else
00809
00810 if( nodeClass.m_bReadoutElmo )
00811 {
00812 if(nodeClass.m_bisInitialized) nodeClass.m_CanCtrlPltf->evalCanBuffer();
00813 if(nodeClass.m_CanCtrlPltf->ElmoRecordings(100, 0, "") == 0)
00814 {
00815 nodeClass.m_bReadoutElmo = false;
00816 ROS_INFO("CPU consuming evalCanBuffer used for ElmoReadout deactivated");
00817 }
00818 }
00819 #endif
00820
00821 nodeClass.publish_JointStates();
00822
00823 loop_rate.sleep();
00824 ros::spinOnce();
00825 }
00826
00827 return 0;
00828 }
00829
00830
00831
00832 bool NodeClass::initDrives()
00833 {
00834 ROS_INFO("Initializing Base Drive Chain");
00835
00836
00837 m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0);
00838
00839
00840
00841 IniFile iniFile;
00842
00843
00844 iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
00845
00846
00847 iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true);
00848 iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true);
00849
00850 #ifdef __SIM__
00851
00852 if(m_iNumDrives >=1)
00853 m_Param.vdWheelNtrlPosRad[0] = 0.0f;
00854 if(m_iNumDrives >=2)
00855 m_Param.vdWheelNtrlPosRad[1] = 0.0f;
00856 if(m_iNumDrives >=3)
00857 m_Param.vdWheelNtrlPosRad[2] = 0.0f;
00858 if(m_iNumDrives >=4)
00859 m_Param.vdWheelNtrlPosRad[3] = 0.0f;
00860 #else
00861
00862 if(m_iNumDrives >=1)
00863 iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_Param.vdWheelNtrlPosRad[0], true);
00864 if(m_iNumDrives >=2)
00865 iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_Param.vdWheelNtrlPosRad[1], true);
00866 if(m_iNumDrives >=3)
00867 iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_Param.vdWheelNtrlPosRad[2], true);
00868 if(m_iNumDrives >=4)
00869 iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_Param.vdWheelNtrlPosRad[3], true);
00870
00871
00872 for(int i=0; i<m_iNumDrives; i++)
00873 {
00874 m_Param.vdWheelNtrlPosRad[i] = MathSup::convDegToRad(m_Param.vdWheelNtrlPosRad[i]);
00875 }
00876 #endif
00877
00878
00879 ROS_INFO("Initializing CanCtrlItf");
00880 bool bTemp1;
00881 #ifdef __SIM__
00882 bTemp1 = true;
00883 #else
00884 bTemp1 = m_CanCtrlPltf->initPltf();
00885 #endif
00886
00887 ROS_INFO("Initializing done");
00888
00889
00890 return bTemp1;
00891 }