cob_base_drive_chain.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 //##################
19 //#### includes ####
20 
21 // standard includes
22 //--
23 
24 // ROS includes
25 #include <ros/ros.h>
26 
27 // ROS message includes
28 #include <std_msgs/Float64.h>
29 #include <sensor_msgs/JointState.h>
30 #include <diagnostic_msgs/DiagnosticStatus.h>
31 #include <diagnostic_msgs/DiagnosticArray.h>
32 #include <control_msgs/JointTrajectoryControllerState.h>
33 #include <control_msgs/JointControllerState.h>
34 
35 // ROS service includes
36 #include <std_srvs/Trigger.h>
37 #include <cob_base_drive_chain/ElmoRecorderReadout.h>
38 #include <cob_base_drive_chain/ElmoRecorderConfig.h>
39 
40 
41 // external includes
42 #include <cob_base_drive_chain/CanCtrlPltfCOb3.h>
43 #include <cob_utilities/IniFile.h>
44 #include <cob_utilities/MathSup.h>
45 
46 //####################
47 //#### node class ####
51 class NodeClass
52 {
53  public:
54  // create a handle for this node, initialize node
55  ros::NodeHandle n;
56 
57  // topics to publish
61  ros::Publisher topicPub_JointState;
62 
63  ros::Publisher topicPub_ControllerState;
64  ros::Publisher topicPub_DiagnosticGlobal_;
65 
66 
70  ros::Publisher topicPub_Diagnostic;
71 
75  ros::Timer glDiagnostics_timer;
76 
77  // topics to subscribe, callback is called for new messages arriving
81  ros::Subscriber topicSub_JointStateCmd;
82 
83  // service servers
87  ros::ServiceServer srvServer_Init;
88 
92  ros::ServiceServer srvServer_Recover;
93 
97  ros::ServiceServer srvServer_Shutdown;
98 
99  ros::ServiceServer srvServer_SetMotionType;
100 
106  ros::ServiceServer srvServer_ElmoRecorderConfig;
107 
122  ros::ServiceServer srvServer_ElmoRecorderReadout;
123 
124 
125  // global variables
126  // generate can-node handle
127 #ifdef __SIM__
128  ros::Publisher br_steer_pub;
129  ros::Publisher bl_steer_pub;
130  ros::Publisher fr_steer_pub;
131  ros::Publisher fl_steer_pub;
132  ros::Publisher br_caster_pub;
133  ros::Publisher bl_caster_pub;
134  ros::Publisher fr_caster_pub;
135  ros::Publisher fl_caster_pub;
136 
137  std::vector<double> m_gazeboPos;
138  std::vector<double> m_gazeboVel;
139  ros::Time m_gazeboStamp;
140 
141  ros::Subscriber topicSub_GazeboJointStates;
142 #else
143  CanCtrlPltfCOb3 *m_CanCtrlPltf;
144 #endif
145  bool m_bisInitialized;
146  int m_iNumMotors;
147  int m_iNumDrives;
148 
149  struct ParamType
150  {
151  double dMaxDriveRateRadpS;
152  double dMaxSteerRateRadpS;
153 
154  std::vector<double> vdWheelNtrlPosRad;
155  };
156  ParamType m_Param;
157 
158  std::string sIniDirectory;
159  bool m_bPubEffort;
160  bool m_bReadoutElmo;
161 
162  // Constructor
163  NodeClass()
164  {
166  // Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
167  if (n.hasParam("IniDirectory"))
168  {
169  n.getParam("IniDirectory", sIniDirectory);
170  ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
171  }
172  else
173  {
174  sIniDirectory = "Platform/IniFiles/";
175  ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
176  }
177 
178  n.param<bool>("PublishEffort", m_bPubEffort, false);
179  if(m_bPubEffort) ROS_INFO("You have choosen to publish effort of motors, that charges capacity of CAN");
180 
181 
182  IniFile iniFile;
183  iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
184 
185  // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
186  iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
187  iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
188  if(m_iNumMotors < 2 || m_iNumMotors > 8) {
189  m_iNumMotors = 8;
190  m_iNumDrives = 4;
191  }
192 
193 #ifdef __SIM__
194  bl_caster_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_r_wheel_controller/command", 1);
195  br_caster_pub = n.advertise<std_msgs::Float64>("/base_br_caster_r_wheel_controller/command", 1);
196  fl_caster_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_r_wheel_controller/command", 1);
197  fr_caster_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_r_wheel_controller/command", 1);
198  bl_steer_pub = n.advertise<std_msgs::Float64>("/base_bl_caster_rotation_controller/command", 1);
199  br_steer_pub = n.advertise<std_msgs::Float64>("/base_br_caster_rotation_controller/command", 1);
200  fl_steer_pub = n.advertise<std_msgs::Float64>("/base_fl_caster_rotation_controller/command", 1);
201  fr_steer_pub = n.advertise<std_msgs::Float64>("/base_fr_caster_rotation_controller/command", 1);
202 
203  topicSub_GazeboJointStates = n.subscribe("/joint_states", 1, &NodeClass::gazebo_joint_states_Callback, this);
204 
205  m_gazeboPos.resize(m_iNumMotors);
206  m_gazeboVel.resize(m_iNumMotors);
207 #else
208  topicPub_JointState = n.advertise<sensor_msgs::JointState>("/joint_states", 1);
209  m_CanCtrlPltf = new CanCtrlPltfCOb3(sIniDirectory);
210 #endif
211 
212  // implementation of topics
213  // published topics
214  topicPub_ControllerState = n.advertise<control_msgs::JointTrajectoryControllerState>("state", 1);
215  topicPub_DiagnosticGlobal_ = n.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
216 
217  topicPub_Diagnostic = n.advertise<diagnostic_msgs::DiagnosticStatus>("diagnostic", 1);
218  // subscribed topics
219  topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this);
220 
221  // implementation of service servers
222  srvServer_Init = n.advertiseService("init", &NodeClass::srvCallback_Init, this);
223  srvServer_ElmoRecorderConfig = n.advertiseService("ElmoRecorderConfig", &NodeClass::srvCallback_ElmoRecorderConfig, this);
224  srvServer_ElmoRecorderReadout = n.advertiseService("ElmoRecorderReadout", &NodeClass::srvCallback_ElmoRecorderReadout, this);
225  m_bReadoutElmo = false;
226 
227  srvServer_Recover = n.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
228  srvServer_Shutdown = n.advertiseService("shutdown", &NodeClass::srvCallback_Shutdown, this);
229 
230  //Timer for publishing global diagnostics
231  glDiagnostics_timer = n.createTimer(ros::Duration(1), &NodeClass::publish_globalDiagnostics, this);
232 
233  // initialization of variables
234 #ifdef __SIM__
235  m_bisInitialized = initDrives();
236 #else
237  m_bisInitialized = false;
238 #endif
239 
240  }
241 
242  // Destructor
243  ~NodeClass()
244  {
245 #ifdef __SIM__
246 
247 #else
248  m_CanCtrlPltf->shutdownPltf();
249 #endif
250  }
251 
252  // topic callback functions
253  // function will be called when a new message arrives on a topic
254  void topicCallback_JointStateCmd(const control_msgs::JointTrajectoryControllerState::ConstPtr& msg)
255  {
256  ROS_DEBUG("Topic Callback joint_command");
257  // only process cmds when system is initialized
258  if(m_bisInitialized == true)
259  {
260  ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
261  sensor_msgs::JointState JointStateCmd;
262  JointStateCmd.position.resize(m_iNumMotors);
263  JointStateCmd.velocity.resize(m_iNumMotors);
264  JointStateCmd.effort.resize(m_iNumMotors);
265 
266  for(unsigned int i = 0; i < msg->joint_names.size(); i++)
267  {
268  // associate inputs to according steer and drive joints
269  // ToDo: specify this globally (Prms-File or config-File or via msg-def.)
270  // check if velocities lie inside allowed boundaries
271 
272  //DRIVES
273  if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
274  {
275  JointStateCmd.position[0] = msg->desired.positions[i];
276  JointStateCmd.velocity[0] = msg->desired.velocities[i];
277  //JointStateCmd.effort[0] = msg->effort[i];
278  }
279  else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_r_wheel_joint")
280  {
281  JointStateCmd.position[2] = msg->desired.positions[i];
282  JointStateCmd.velocity[2] = msg->desired.velocities[i];
283  //JointStateCmd.effort[2] = msg->effort[i];
284  }
285  else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_r_wheel_joint")
286  {
287  JointStateCmd.position[4] = msg->desired.positions[i];
288  JointStateCmd.velocity[4] = msg->desired.velocities[i];
289  //JointStateCmd.effort[4] = msg->effort[i];
290  }
291  else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_r_wheel_joint")
292  {
293  JointStateCmd.position[6] = msg->desired.positions[i];
294  JointStateCmd.velocity[6] = msg->desired.velocities[i];
295  //JointStateCmd.effort[6] = msg->effort[i];
296  }
297  //STEERS
298  else if(msg->joint_names[i] == "fl_caster_rotation_joint")
299  {
300  JointStateCmd.position[1] = msg->desired.positions[i];
301  JointStateCmd.velocity[1] = msg->desired.velocities[i];
302  //JointStateCmd.effort[1] = msg->effort[i];
303  }
304  else if(m_iNumDrives>=2 && msg->joint_names[i] == "bl_caster_rotation_joint")
305  {
306  JointStateCmd.position[3] = msg->desired.positions[i];
307  JointStateCmd.velocity[3] = msg->desired.velocities[i];
308  //JointStateCmd.effort[3] = msg->effort[i];
309  }
310  else if(m_iNumDrives>=3 && msg->joint_names[i] == "br_caster_rotation_joint")
311  {
312  JointStateCmd.position[5] = msg->desired.positions[i];
313  JointStateCmd.velocity[5] = msg->desired.velocities[i];
314  //JointStateCmd.effort[5] = msg->effort[i];
315  }
316  else if(m_iNumDrives>=4 && msg->joint_names[i] == "fr_caster_rotation_joint")
317  {
318  JointStateCmd.position[7] = msg->desired.positions[i];
319  JointStateCmd.velocity[7] = msg->desired.velocities[i];
320  //JointStateCmd.effort[7] = msg->effort[i];
321  }
322  else
323  {
324  ROS_ERROR("Unkown joint name %s", (msg->joint_names[i]).c_str());
325  }
326  }
327 
328 
329  // check if velocities lie inside allowed boundaries
330  for(int i = 0; i < m_iNumMotors; i++)
331  {
332 #ifdef __SIM__
333 #else
334  // for steering motors
335  if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
336  {
337  if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
338  {
339  JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
340  }
341  if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
342  {
343  JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
344  }
345  }
346  // for driving motors
347  else
348  {
349  if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
350  {
351  JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
352  }
353  if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
354  {
355  JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS;
356  }
357  }
358 #endif
359 #ifdef __SIM__
360  ROS_DEBUG("Send velocity data to gazebo");
361  std_msgs::Float64 fl;
362  fl.data = JointStateCmd.velocity[i];
363  if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
364  fl_caster_pub.publish(fl);
365  if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
366  fr_caster_pub.publish(fl);
367  if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
368  bl_caster_pub.publish(fl);
369  if(msg->joint_names[i] == "br_caster_r_wheel_joint")
370  br_caster_pub.publish(fl);
371 
372  if(msg->joint_names[i] == "fl_caster_rotation_joint")
373  fl_steer_pub.publish(fl);
374  if(msg->joint_names[i] == "fr_caster_rotation_joint")
375  fr_steer_pub.publish(fl);
376  if(msg->joint_names[i] == "bl_caster_rotation_joint")
377  bl_steer_pub.publish(fl);
378  if(msg->joint_names[i] == "br_caster_rotation_joint")
379  br_steer_pub.publish(fl);
380  ROS_DEBUG("Successfully sent velicities to gazebo");
381 #else
382  ROS_DEBUG("Send velocity data to drives");
383  m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]);
384  ROS_DEBUG("Successfully sent velicities to drives");
385 #endif
386  }
387 
388 #ifdef __SIM__
389 
390 #else
391  if(m_bPubEffort) {
392  m_CanCtrlPltf->requestMotorTorque();
393  }
394 #endif
395  }
396  }
397 
398  // service callback functions
399  // function will be called when a service is querried
400 
401  // Init Can-Configuration
402  bool srvCallback_Init(std_srvs::Trigger::Request &req,
403  std_srvs::Trigger::Response &res )
404  {
405  ROS_DEBUG("Service callback init");
406  if(m_bisInitialized == false)
407  {
408  m_bisInitialized = initDrives();
409  //ROS_INFO("...initializing can-nodes...");
410  //m_bisInitialized = m_CanCtrlPltf->initPltf();
411  res.success = m_bisInitialized;
412  if(m_bisInitialized)
413  {
414  ROS_INFO("base initialized");
415  }
416  else
417  {
418  res.message = "initialization of base failed";
419  ROS_ERROR("Initializing base failed");
420  }
421  }
422  else
423  {
424  ROS_WARN("...base already initialized...");
425  res.success = true;
426  res.message = "platform already initialized";
427  }
428  return true;
429  }
430 
431  bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req,
432  cob_base_drive_chain::ElmoRecorderConfig::Response &res ){
433  if(m_bisInitialized)
434  {
435 #ifdef __SIM__
436  res.success = true;
437 #else
438  m_CanCtrlPltf->evalCanBuffer();
439  res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
440 #endif
441  res.message = "Successfully configured all motors for instant record";
442  }
443 
444  return true;
445  }
446 
447  bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req,
448  cob_base_drive_chain::ElmoRecorderReadout::Response &res ){
449  if(m_bisInitialized) {
450 #ifdef __SIM__
451  res.success = true;
452 #else
453  m_CanCtrlPltf->evalCanBuffer();
454  res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
455 #endif
456  if(res.success == 0) {
457  res.message = "Successfully requested reading out of Recorded data";
458  m_bReadoutElmo = true;
459  ROS_WARN("CPU consuming evalCanBuffer used for ElmoReadout activated");
460  } else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
461  else if(res.success == 2) res.message = "A previous transmission is still in progress";
462  }
463 
464  return true;
465  }
466 
467 
468 
469  // reset Can-Configuration
470  bool srvCallback_Recover(std_srvs::Trigger::Request &req,
471  std_srvs::Trigger::Response &res )
472  {
473  if(m_bisInitialized)
474  {
475  ROS_DEBUG("Service callback recover");
476 #ifdef __SIM__
477  res.success = true;
478 #else
479  res.success = m_CanCtrlPltf->resetPltf();
480 #endif
481  if (res.success) {
482  ROS_INFO("base resetted");
483  } else {
484  res.message = "reset of base failed";
485  ROS_WARN("Resetting base failed");
486  }
487  }
488  else
489  {
490  ROS_WARN("Base not yet initialized.");
491  res.success = false;
492  res.message = "Base not yet initialized.";
493  }
494  return true;
495  }
496 
497  // shutdown Drivers and Can-Node
498  bool srvCallback_Shutdown(std_srvs::Trigger::Request &req,
499  std_srvs::Trigger::Response &res )
500  {
501  ROS_DEBUG("Service callback shutdown");
502 #ifdef __SIM__
503  res.success = true;
504 #else
505  res.success = m_CanCtrlPltf->shutdownPltf();
506 #endif
507  if (res.success)
508  ROS_INFO("Drives shut down");
509  else
510  ROS_INFO("Shutdown of Drives FAILED");
511 
512  return true;
513  }
514 
515  //publish JointStates cyclical instead of service callback
516  bool publish_JointStates()
517  {
518  // init local variables
519  int j, k;
520  bool bIsError;
521  std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
522 
523  // set default values
524  vdAngGearRad.resize(m_iNumMotors, 0);
525  vdVelGearRad.resize(m_iNumMotors, 0);
526  vdEffortGearNM.resize(m_iNumMotors, 0);
527 
528  // create temporary (local) JointState/Diagnostics Data-Container
529  sensor_msgs::JointState jointstate;
530  diagnostic_msgs::DiagnosticStatus diagnostics;
531  control_msgs::JointTrajectoryControllerState controller_state;
532 
533  // get time stamp for header
534 #ifdef __SIM__
535  jointstate.header.stamp = m_gazeboStamp;
536  controller_state.header.stamp = m_gazeboStamp;
537 #else
538  jointstate.header.stamp = ros::Time::now();
539  controller_state.header.stamp = jointstate.header.stamp;
540 #endif
541 
542  // assign right size to JointState
543  //jointstate.name.resize(m_iNumMotors);
544  jointstate.position.assign(m_iNumMotors, 0.0);
545  jointstate.velocity.assign(m_iNumMotors, 0.0);
546  jointstate.effort.assign(m_iNumMotors, 0.0);
547 
548  if(m_bisInitialized == false)
549  {
550  // as long as system is not initialized
551  bIsError = false;
552 
553  j = 0;
554  k = 0;
555 
556  // set data to jointstate
557  for(int i = 0; i<m_iNumMotors; i++)
558  {
559  jointstate.position[i] = 0.0;
560  jointstate.velocity[i] = 0.0;
561  jointstate.effort[i] = 0.0;
562  }
563  jointstate.name.push_back("fl_caster_r_wheel_joint");
564  jointstate.name.push_back("fl_caster_rotation_joint");
565  jointstate.name.push_back("bl_caster_r_wheel_joint");
566  jointstate.name.push_back("bl_caster_rotation_joint");
567  jointstate.name.push_back("br_caster_r_wheel_joint");
568  jointstate.name.push_back("br_caster_rotation_joint");
569  jointstate.name.push_back("fr_caster_r_wheel_joint");
570  jointstate.name.push_back("fr_caster_rotation_joint");
571  jointstate.name.resize(m_iNumMotors);
572 
573  }
574  else
575  {
576  // as soon as drive chain is initialized
577  // read Can-Buffer
578 #ifdef __SIM__
579 
580 #else
581  ROS_DEBUG("Read CAN-Buffer");
582  m_CanCtrlPltf->evalCanBuffer();
583  ROS_DEBUG("Successfully read CAN-Buffer");
584 #endif
585  j = 0;
586  k = 0;
587  for(int i = 0; i<m_iNumMotors; i++)
588  {
589 #ifdef __SIM__
590  vdAngGearRad[i] = m_gazeboPos[i];
591  vdVelGearRad[i] = m_gazeboVel[i];
592 #else
593  m_CanCtrlPltf->getGearPosVelRadS(i, &vdAngGearRad[i], &vdVelGearRad[i]);
594 #endif
595 
596  //Get motor torque
597  if(m_bPubEffort) {
598  for(int i=0; i<m_iNumMotors; i++)
599  {
600 #ifdef __SIM__
601  //vdEffortGearNM[i] = m_gazeboEff[i];
602 #else
603  m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
604 #endif
605  }
606  }
607 
608  // if a steering motor was read -> correct for offset
609  if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
610  {
611  // correct for initial offset of steering angle (arbitrary homing position)
612  vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
613  MathSup::normalizePi(vdAngGearRad[i]);
614  j = j+1;
615  }
616  }
617 
618  // set data to jointstate
619  for(int i = 0; i<m_iNumMotors; i++)
620  {
621  jointstate.position[i] = vdAngGearRad[i];
622  jointstate.velocity[i] = vdVelGearRad[i];
623  jointstate.effort[i] = vdEffortGearNM[i];
624  }
625 
626  jointstate.name.push_back("fl_caster_r_wheel_joint");
627  jointstate.name.push_back("fl_caster_rotation_joint");
628  jointstate.name.push_back("bl_caster_r_wheel_joint");
629  jointstate.name.push_back("bl_caster_rotation_joint");
630  jointstate.name.push_back("br_caster_r_wheel_joint");
631  jointstate.name.push_back("br_caster_rotation_joint");
632  jointstate.name.push_back("fr_caster_r_wheel_joint");
633  jointstate.name.push_back("fr_caster_rotation_joint");
634  jointstate.name.resize(m_iNumMotors);
635 
636  }
637 
638  controller_state.joint_names = jointstate.name;
639  controller_state.actual.positions = jointstate.position;
640  controller_state.actual.velocities = jointstate.velocity;
641 
642  // publish jointstate message
643 #ifdef __SIM__
644  //do not publish
645 #else
646  topicPub_JointState.publish(jointstate);
647 #endif
648  topicPub_ControllerState.publish(controller_state);
649 
650  ROS_DEBUG("published new drive-chain configuration (JointState message)");
651 
652 
653  if(m_bisInitialized)
654  {
655  // read Can only after initialization
656 #ifdef __SIM__
657  bIsError = false;
658 #else
659  bIsError = m_CanCtrlPltf->isPltfError();
660 #endif
661  }
662 
663  // set data to diagnostics
664  if(bIsError)
665  {
666  diagnostics.level = 2;
667  diagnostics.name = "drive-chain can node";
668  diagnostics.message = "one or more drives are in Error mode";
669  }
670  else
671  {
672  if (m_bisInitialized)
673  {
674  diagnostics.level = 0;
675  diagnostics.name = "drive-chain can node";
676  diagnostics.message = "drives operating normal";
677  }
678  else
679  {
680  diagnostics.level = 1;
681  diagnostics.name = "drive-chain can node";
682  diagnostics.message = "drives are initializing";
683  }
684  }
685 
686  // publish diagnostic message
687  topicPub_Diagnostic.publish(diagnostics);
688  ROS_DEBUG("published new drive-chain configuration (JointState message)");
689 
690 
691  return true;
692  }
693 
694  void publish_globalDiagnostics(const ros::TimerEvent& event)
695  {
696  //publish global diagnostic messages
697  diagnostic_msgs::DiagnosticArray diagnostics_gl;
698  diagnostics_gl.header.stamp = ros::Time::now();
699  diagnostics_gl.status.resize(1);
700  // set data to diagnostics
701 #ifdef __SIM__
702  if (false)
703 #else
704  if(m_bisInitialized && m_CanCtrlPltf->isPltfError())
705 #endif
706 
707  {
708  diagnostics_gl.status[0].level = 2;
709  diagnostics_gl.status[0].name = ros::this_node::getName();
710  diagnostics_gl.status[0].message = "Base not initialized or in error";
711  }
712  else
713  {
714  if (m_bisInitialized)
715  {
716  diagnostics_gl.status[0].level = 0;
717  diagnostics_gl.status[0].name = ros::this_node::getName();
718  diagnostics_gl.status[0].message = "base_drive_chain initialized and running";
719  }
720  else
721  {
722  diagnostics_gl.status[0].level = 1;
723  diagnostics_gl.status[0].name = ros::this_node::getName();
724  diagnostics_gl.status[0].message = "base_drive_chain not initialized";
725  }
726  }
727  // publish diagnostic message
728  topicPub_DiagnosticGlobal_.publish(diagnostics_gl);
729  }
730 
731  // other function declarations
732  bool initDrives();
733 
734 #ifdef __SIM__
735  void gazebo_joint_states_Callback(const sensor_msgs::JointState::ConstPtr& msg) {
736  for (unsigned int i=0; i<msg->name.size(); i++) {
737  //Drives
738  if(msg->name[i] == "fl_caster_r_wheel_joint") {
739  m_gazeboPos[0] = msg->position[i];
740  m_gazeboVel[0] = msg->velocity[i];
741  m_gazeboStamp = msg->header.stamp;
742  }
743  else if(msg->name[i] == "bl_caster_r_wheel_joint") {
744  m_gazeboPos[2] = msg->position[i];
745  m_gazeboVel[2] = msg->velocity[i];
746  }
747  else if(msg->name[i] == "br_caster_r_wheel_joint") {
748  m_gazeboPos[4] = msg->position[i];
749  m_gazeboVel[4] = msg->velocity[i];
750  }
751  else if(msg->name[i] == "fr_caster_r_wheel_joint") {
752  m_gazeboPos[6] = msg->position[i];
753  m_gazeboVel[6] = msg->velocity[i];
754  }
755 
756  //Steers
757  else if(msg->name[i] == "fl_caster_rotation_joint") {
758  m_gazeboPos[1] = msg->position[i];
759  m_gazeboVel[1] = msg->velocity[i];
760  }
761  else if(msg->name[i] == "bl_caster_rotation_joint") {
762  m_gazeboPos[3] = msg->position[i];
763  m_gazeboVel[3] = msg->velocity[i];
764  }
765  else if(msg->name[i] == "br_caster_rotation_joint") {
766  m_gazeboPos[5] = msg->position[i];
767  m_gazeboVel[5] = msg->velocity[i];
768  }
769  else if(msg->name[i] == "fr_caster_rotation_joint") {
770  m_gazeboPos[7] = msg->position[i];
771  m_gazeboVel[7] = msg->velocity[i];
772  }
773  }
774  }
775 #else
776 
777 #endif
778 };
779 
780 //#######################
781 //#### main programm ####
782 int main(int argc, char** argv)
783 {
784  // initialize ROS, spezify name of node
785  ros::init(argc, argv, "base_drive_chain");
786 
787  NodeClass nodeClass;
788 
789  // specify looprate of control-cycle
790  ros::Rate loop_rate(100); // Hz
791 
792  while(nodeClass.n.ok())
793  {
794 #ifdef __SIM__
795 
796 #else
797  //Read-out of CAN buffer is only necessary during read-out of Elmo Recorder
798  if( nodeClass.m_bReadoutElmo )
799  {
800  if(nodeClass.m_bisInitialized) nodeClass.m_CanCtrlPltf->evalCanBuffer();
801  if(nodeClass.m_CanCtrlPltf->ElmoRecordings(100, 0, "") == 0)
802  {
803  nodeClass.m_bReadoutElmo = false;
804  ROS_INFO("CPU consuming evalCanBuffer used for ElmoReadout deactivated");
805  }
806  }
807 #endif
808 
809  nodeClass.publish_JointStates();
810 
811  loop_rate.sleep();
812  ros::spinOnce();
813  }
814 
815  return 0;
816 }
817 
818 //##################################
819 //#### function implementations ####
820 bool NodeClass::initDrives()
821 {
822  ROS_INFO("Initializing Base Drive Chain");
823 
824  // init member vectors
825  m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0);
826 // m_Param.vdWheelNtrlPosRad.assign(4,0);
827  // ToDo: replace the following steps by ROS configuration files
828  // create Inifile class and set target inifile (from which data shall be read)
829  IniFile iniFile;
830 
831  //n.param<std::string>("PltfIniLoc", sIniFileName, "Platform/IniFiles/Platform.ini");
832  iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
833 
834  // get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
835  iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true);
836  iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true);
837 
838 #ifdef __SIM__
839  // get Offset from Zero-Position of Steering
840  if(m_iNumDrives >=1)
841  m_Param.vdWheelNtrlPosRad[0] = 0.0f;
842  if(m_iNumDrives >=2)
843  m_Param.vdWheelNtrlPosRad[1] = 0.0f;
844  if(m_iNumDrives >=3)
845  m_Param.vdWheelNtrlPosRad[2] = 0.0f;
846  if(m_iNumDrives >=4)
847  m_Param.vdWheelNtrlPosRad[3] = 0.0f;
848 #else
849  // get Offset from Zero-Position of Steering
850  if(m_iNumDrives >=1)
851  iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_Param.vdWheelNtrlPosRad[0], true);
852  if(m_iNumDrives >=2)
853  iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_Param.vdWheelNtrlPosRad[1], true);
854  if(m_iNumDrives >=3)
855  iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_Param.vdWheelNtrlPosRad[2], true);
856  if(m_iNumDrives >=4)
857  iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_Param.vdWheelNtrlPosRad[3], true);
858 
859  //Convert Degree-Value from ini-File into Radian:
860  for(int i=0; i<m_iNumDrives; i++)
861  {
862  m_Param.vdWheelNtrlPosRad[i] = MathSup::convDegToRad(m_Param.vdWheelNtrlPosRad[i]);
863  }
864 #endif
865 
866  // debug log
867  ROS_INFO("Initializing CanCtrlItf");
868  bool bTemp1;
869 #ifdef __SIM__
870  bTemp1 = true;
871 #else
872  bTemp1 = m_CanCtrlPltf->initPltf();
873 #endif
874  // debug log
875  ROS_INFO("Initializing done");
876 
877 
878  return bTemp1;
879 }
NodeClass::m_CanCtrlPltf
CanCtrlPltfCOb3 * m_CanCtrlPltf
Definition: cob_base_drive_chain.cpp:143
NodeClass::srvCallback_Recover
bool srvCallback_Recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: cob_base_drive_chain.cpp:470
NodeClass::sIniDirectory
std::string sIniDirectory
Definition: cob_base_drive_chain.cpp:158
ros::NodeHandle
CanCtrlPltfCOb3::setVelGearRadS
int setVelGearRadS(int iCanIdent, double dVelGearRadS)
Definition: CanCtrlPltfCOb3.cpp:1289
NodeClass::publish_globalDiagnostics
void publish_globalDiagnostics(const ros::TimerEvent &event)
Definition: cob_base_drive_chain.cpp:694
NodeClass::topicPub_JointState
ros::Publisher topicPub_JointState
Definition: cob_base_drive_chain.cpp:61
NodeClass::topicPub_ControllerState
ros::Publisher topicPub_ControllerState
Definition: cob_base_drive_chain.cpp:63
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
NodeClass::srvServer_ElmoRecorderConfig
ros::ServiceServer srvServer_ElmoRecorderConfig
Definition: cob_base_drive_chain.cpp:106
NodeClass::srvCallback_Init
bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: cob_base_drive_chain.cpp:402
NodeClass::topicPub_DiagnosticGlobal_
ros::Publisher topicPub_DiagnosticGlobal_
Definition: cob_base_drive_chain.cpp:64
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
NodeClass::srvServer_Init
ros::ServiceServer srvServer_Init
Definition: cob_base_drive_chain.cpp:87
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
NodeClass::topicPub_Diagnostic
ros::Publisher topicPub_Diagnostic
Definition: cob_base_drive_chain.cpp:70
NodeClass::srvServer_Shutdown
ros::ServiceServer srvServer_Shutdown
Definition: cob_base_drive_chain.cpp:97
NodeClass::srvServer_SetMotionType
ros::ServiceServer srvServer_SetMotionType
Definition: cob_base_drive_chain.cpp:99
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ROS_WARN
#define ROS_WARN(...)
MathSup::normalizePi
static void normalizePi(double &angle)
NodeClass::topicSub_JointStateCmd
ros::Subscriber topicSub_JointStateCmd
Definition: cob_base_drive_chain.cpp:81
main
int main(int argc, char **argv)
Definition: cob_base_drive_chain.cpp:782
CanCtrlPltfCOb3::getMotorTorque
void getMotorTorque(int iCanIdent, double *pdTorqueNm)
Definition: CanCtrlPltfCOb3.cpp:1419
IniFile::GetKeyInt
int GetKeyInt(const char *pSect, const char *pKey, int *pValue, bool bWarnIfNotfound=true)
NodeClass::topicCallback_JointStateCmd
void topicCallback_JointStateCmd(const control_msgs::JointTrajectoryControllerState::ConstPtr &msg)
Definition: cob_base_drive_chain.cpp:254
ROS_INFO
#define ROS_INFO(...)
NodeClass::srvCallback_Shutdown
bool srvCallback_Shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: cob_base_drive_chain.cpp:498
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
NodeClass::glDiagnostics_timer
ros::Timer glDiagnostics_timer
Definition: cob_base_drive_chain.cpp:75
NodeClass::srvCallback_ElmoRecorderConfig
bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req, cob_base_drive_chain::ElmoRecorderConfig::Response &res)
Definition: cob_base_drive_chain.cpp:431
CanCtrlPltfCOb3::getGearPosVelRadS
int getGearPosVelRadS(int iCanIdent, double *pdAngleGearRad, double *pdVelGearRadS)
Definition: CanCtrlPltfCOb3.cpp:1348
IniFile::GetKeyDouble
int GetKeyDouble(const char *pSect, const char *pKey, double *pValue, bool bWarnIfNotfound=true)
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Rate::sleep
bool sleep()
IniFile::SetFileName
int SetFileName(std::string fileName, std::string strIniFileUsedBy="", bool bCreate=false)
NodeClass::srvCallback_ElmoRecorderReadout
bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req, cob_base_drive_chain::ElmoRecorderReadout::Response &res)
Definition: cob_base_drive_chain.cpp:447
NodeClass::n
ros::NodeHandle n
Definition: cob_base_drive_chain.cpp:55
NodeClass::srvServer_ElmoRecorderReadout
ros::ServiceServer srvServer_ElmoRecorderReadout
Definition: cob_base_drive_chain.cpp:122
NodeClass::ParamType::vdWheelNtrlPosRad
std::vector< double > vdWheelNtrlPosRad
Definition: cob_base_drive_chain.cpp:154
MathSup::convDegToRad
static double convDegToRad(const double &dAngDeg)
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
ros::Time::now
static Time now()
ros::NodeHandle::ok
bool ok() const
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
ros::spinOnce
ROSCPP_DECL void spinOnce()
CanCtrlPltfCOb3::ElmoRecordings
int ElmoRecordings(int iFlag, int iParam, std::string sString)
Definition: CanCtrlPltfCOb3.cpp:1456
ROS_ERROR
#define ROS_ERROR(...)
NodeClass::srvServer_Recover
ros::ServiceServer srvServer_Recover
Definition: cob_base_drive_chain.cpp:92
ROS_DEBUG
#define ROS_DEBUG(...)

cob_base_drive_chain
Author(s): Christian Connette
autogenerated on Wed Apr 7 2021 02:11:54