cob_base_drive_chain: cob_base_drive_chain: cob_base_drive

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_base_drive_chain
00013  * Description: This node provides control of the care-o-bot platform drives to the ROS-"network". For this purpose it offers several services and publishes data on different topics.
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Christian Connette, email:christian.connette@ipa.fhg.de
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: Feb 2010:
00021  * ToDo: Doesn't this node has to take care about the Watchdogs?
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *       * Redistributions of source code must retain the above copyright
00029  *         notice, this list of conditions and the following disclaimer.
00030  *       * Redistributions in binary form must reproduce the above copyright
00031  *         notice, this list of conditions and the following disclaimer in the
00032  *         documentation and/or other materials provided with the distribution.
00033  *       * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *         Engineering and Automation (IPA) nor the names of its
00035  *         contributors may be used to endorse or promote products derived from
00036  *         this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 //##################
00055 //#### includes ####
00056 
00057 // standard includes
00058 //--
00059 
00060 // ROS includes
00061 #include <ros/ros.h>
00062 
00063 // ROS message includes
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 // ROS service includes
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 // external includes
00078 #include <cob_base_drive_chain/CanCtrlPltfCOb3.h>
00079 #include <cob_utilities/IniFile.h>
00080 #include <cob_utilities/MathSup.h>
00081 
00082 //####################
00083 //#### node class ####
00087 class NodeClass
00088 {
00089         public:
00090                 // create a handle for this node, initialize node
00091                 ros::NodeHandle n;
00092 
00093                 // topics to publish
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                 // topics to subscribe, callback is called for new messages arriving
00113                 ros::Subscriber topicSub_JointStateCmd;
00114 
00115                 // service servers
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                 // global variables
00157                 // generate can-node handle
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                 // Constructor
00194                 NodeClass()
00195                 {
00197                         // Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
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                         // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
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                         // implementation of topics
00243                         // published topics
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                         // subscribed topics
00250                         topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this);
00251 
00252                         // implementation of service servers
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                         // initialization of variables
00262 #ifdef __SIM__
00263                         m_bisInitialized = initDrives();
00264 #else
00265                         m_bisInitialized = false;
00266 #endif
00267 
00268                 }
00269 
00270                 // Destructor
00271                 ~NodeClass() 
00272                 {
00273 #ifdef __SIM__
00274 
00275 #else
00276                         m_CanCtrlPltf->shutdownPltf();
00277 #endif
00278                 }
00279 
00280                 // topic callback functions 
00281                 // function will be called when a new message arrives on a topic
00282                 void topicCallback_JointStateCmd(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00283                 {
00284                         ROS_DEBUG("Topic Callback joint_command");
00285                         // only process cmds when system is initialized
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                                         // associate inputs to according steer and drive joints
00297                                         // ToDo: specify this globally (Prms-File or config-File or via msg-def.)
00298                                         // check if velocities lie inside allowed boundaries
00299                                         
00300                                         //DRIVES
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                                                         //JointStateCmd.effort[0] = msg->effort[i];
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                                                         //JointStateCmd.effort[2] = msg->effort[i];
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                                                         //JointStateCmd.effort[4] = msg->effort[i];
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                                                         //JointStateCmd.effort[6] = msg->effort[i];
00324                                         }
00325                                         //STEERS
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                                                         //JointStateCmd.effort[1] = msg->effort[i];
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                                                         //JointStateCmd.effort[3] = msg->effort[i];
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                                                         //JointStateCmd.effort[5] = msg->effort[i];
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                                                         //JointStateCmd.effort[7] = msg->effort[i];
00349                                         }
00350                                         else
00351                                         {
00352                                                 ROS_ERROR("Unkown joint name");
00353                                         }
00354                                 }
00355                                 
00356                         
00357                                 // check if velocities lie inside allowed boundaries
00358                                 for(int i = 0; i < m_iNumMotors; i++)
00359                                 {
00360 #ifdef __SIM__
00361 #else   
00362                                         // for steering motors
00363                                         if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
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                                         // for driving motors
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                 // service callback functions
00427                 // function will be called when a service is querried
00428 
00429                 // Init Can-Configuration
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                                 //ROS_INFO("...initializing can-nodes...");
00438                                 //m_bisInitialized = m_CanCtrlPltf->initPltf();
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                 // reset Can-Configuration
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                 // shutdown Drivers and Can-Node
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                 //publish JointStates cyclical instead of service callback
00545                 bool publish_JointStates()
00546                 {
00547                         // init local variables
00548                         int j, k;
00549                         bool bIsError;
00550                         std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
00551 
00552                         // set default values
00553                         vdAngGearRad.resize(m_iNumMotors, 0);
00554                         vdVelGearRad.resize(m_iNumMotors, 0);
00555                         vdEffortGearNM.resize(m_iNumMotors, 0);
00556 
00557                         // create temporary (local) JointState/Diagnostics Data-Container
00558                         sensor_msgs::JointState jointstate;
00559                         diagnostic_msgs::DiagnosticStatus diagnostics;
00560                         pr2_controllers_msgs::JointTrajectoryControllerState controller_state;
00561                         
00562                         // get time stamp for header
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                         // assign right size to JointState
00572                         //jointstate.name.resize(m_iNumMotors);
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                                 // as long as system is not initialized
00580                                 bIsError = false;
00581 
00582                                 j = 0;
00583                                 k = 0;
00584 
00585                                 // set data to jointstate                       
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                                 // as soon as drive chain is initialized
00605                                 // read Can-Buffer
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                                         //Get motor torque
00625                                         if(m_bPubEffort) {
00626                                                 for(int i=0; i<m_iNumMotors; i++) 
00627                                                 {
00628 #ifdef __SIM__
00629                                                         //vdEffortGearNM[i] = m_gazeboEff[i];
00630 #else
00631                                                         m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
00632 #endif
00633                                                 }
00634                                         }
00635 
00636                                         // if a steering motor was read -> correct for offset
00637                                         if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
00638                                         {
00639                                                 // correct for initial offset of steering angle (arbitrary homing position)
00640                                                 vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
00641                                                 MathSup::normalizePi(vdAngGearRad[i]);
00642                                                 j = j+1;
00643                                         }
00644                                 }
00645 
00646                                 // set data to jointstate
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                         // publish jointstate message
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                                 // read Can only after initialization
00679 #ifdef __SIM__
00680                                 bIsError = false;
00681 #else
00682                                 bIsError = m_CanCtrlPltf->isPltfError();
00683 #endif
00684                         }
00685 
00686                         // set data to diagnostics
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                         // publish diagnostic message
00710                         topicPub_Diagnostic.publish(diagnostics);
00711                         ROS_DEBUG("published new drive-chain configuration (JointState message)");
00712                         //publish global diagnostic messages
00713                         diagnostic_msgs::DiagnosticArray diagnostics_gl;
00714                     diagnostics_gl.status.resize(1);
00715                     // set data to diagnostics
00716                     if(bIsError)
00717                     {
00718                       diagnostics_gl.status[0].level = 2;
00719                       diagnostics_gl.status[0].name = n.getNamespace();;
00720                       //TODOdiagnostics.status[0].message = error_msg_;
00721                     }
00722                     else
00723                     {
00724                       if (m_bisInitialized)
00725                       {
00726                         diagnostics_gl.status[0].level = 0;
00727                         diagnostics_gl.status[0].name = n.getNamespace(); //"schunk_powercube_chain";
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(); //"schunk_powercube_chain";
00734                         diagnostics_gl.status[0].message = "base_drive_chain not initialized";
00735                       }
00736                     }
00737                     // publish diagnostic message
00738                     topicPub_DiagnosticGlobal_.publish(diagnostics_gl);
00739 
00740                         return true;
00741                 }
00742                 
00743                 // other function declarations
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                                 //Drives
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                                 //Steers
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 //#### main programm ####
00794 int main(int argc, char** argv)
00795 {
00796         // initialize ROS, spezify name of node
00797         ros::init(argc, argv, "base_drive_chain");
00798 
00799         NodeClass nodeClass;
00800 
00801         // specify looprate of control-cycle
00802         ros::Rate loop_rate(100); // Hz 
00803 
00804         while(nodeClass.n.ok())
00805         {
00806 #ifdef __SIM__
00807 
00808 #else
00809                 //Read-out of CAN buffer is only necessary during read-out of Elmo Recorder             
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 //#### function implementations ####
00832 bool NodeClass::initDrives()
00833 {
00834         ROS_INFO("Initializing Base Drive Chain");
00835 
00836         // init member vectors
00837         m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0);
00838 //      m_Param.vdWheelNtrlPosRad.assign(4,0);
00839         // ToDo: replace the following steps by ROS configuration files
00840         // create Inifile class and set target inifile (from which data shall be read)
00841         IniFile iniFile;
00842 
00843         //n.param<std::string>("PltfIniLoc", sIniFileName, "Platform/IniFiles/Platform.ini");
00844         iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
00845 
00846         // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
00847         iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true);
00848         iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true);
00849 
00850 #ifdef __SIM__
00851         // get Offset from Zero-Position of Steering
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         // get Offset from Zero-Position of Steering
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         //Convert Degree-Value from ini-File into Radian:
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         // debug log
00879         ROS_INFO("Initializing CanCtrlItf");
00880         bool bTemp1;
00881 #ifdef __SIM__
00882         bTemp1 = true;
00883 #else
00884         bTemp1 =  m_CanCtrlPltf->initPltf();
00885 #endif
00886         // debug log
00887         ROS_INFO("Initializing done");
00888 
00889 
00890         return bTemp1;
00891 }
cob_base_drive_chain.cpp
