Feature/pm 203 new imc initialization (#191)

* Removed the reset IMC until they work function for a single reset of all the IMC's.

* Added try catch to the update of the hardware_interface_node to be certain that the ethercat master gets deconstructed.

* Added boundaries to the encode reset to check not only on an angle-value of 0.

* fixed clang8 format fail.

* Updated format to match the cpp style guide

* Removed space after public declaration of class.

* Added include string.

Co-authored-by: martijnvandermarel <martijn.vander.marel@hetnet.nl>
Co-authored-by: Roelemans <47626418+Roelemans@users.noreply.github.com>
Co-authored-by: Olav de Haas <olavh@protonmail.com>

Author: 4 people

march_hardware/include/march_hardware/MarchRobot.h

@@ -19,10 +19,9 @@ class MarchRobot
 private:
   std::unique_ptr<EthercatMaster> ethercatMaster;
   std::unique_ptr<PowerDistributionBoard> powerDistributionBoard;
-
-public:
   ::std::vector<Joint> jointList;
 
+public:
   MarchRobot(::std::vector<Joint> jointList, ::std::string ifName, int ecatCycleTime);
 
   MarchRobot(::std::vector<Joint> jointList, PowerDistributionBoard powerDistributionBoard, ::std::string ifName,

march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h

@@ -26,6 +26,8 @@ namespace march_hardware_interface
 {
 static const double POSITION_STEP_FACTOR = 10;
 static const double VELOCITY_STEP_FACTOR = 10;
+static const int LOWER_BOUNDARY_ANGLE_IU = -2;
+static const int UPPER_BOUNDARY_ANGLE_IU = 2;
 
 /**
  * @brief HardwareInterface to allow ros_control to actuate our hardware.
@@ -84,7 +86,7 @@ class MarchHardwareInterface : public march_hardware_interface::MarchHardware
   void updatePowerDistributionBoard();
   void updateAfterLimitJointCommand();
   void updateIMotionCubeState();
-  void resetIMotionCubesUntilTheyWork();
+  void initiateIMC();
   void outsideLimitsCheck(int joint_index);
   void iMotionCubeStateCheck(int joint_index);
 };

march_hardware_interface/src/march_hardware_interface.cpp

@@ -4,6 +4,7 @@
 #include <joint_limits_interface/joint_limits_interface.h>
 #include <joint_limits_interface/joint_limits_urdf.h>
 #include <sstream>
+#include <string>
 
 #include <march_hardware/MarchRobot.h>
 #include <march_hardware/Joint.h>
@@ -23,6 +24,7 @@ using joint_limits_interface::JointLimits;
 using joint_limits_interface::PositionJointSoftLimitsHandle;
 using joint_limits_interface::PositionJointSoftLimitsInterface;
 using joint_limits_interface::SoftJointLimits;
+using march4cpp::Joint;
 
 namespace march_hardware_interface
 {
@@ -84,7 +86,7 @@ void MarchHardwareInterface::init()
     soft_limits_[i] = soft_limits;
   }
 
-  resetIMotionCubesUntilTheyWork();
+  initiateIMC();
 
   // Print all joint positions on startup in case initialization fails.
   this->read();
@@ -123,7 +125,7 @@ void MarchHardwareInterface::init()
       {
         marchRobot.getPowerDistributionBoard()->getHighVoltage().setNetOnOff(true, netNumber);
         usleep(100000);
-        ROS_INFO_THROTTLE(1, "[%s] Waiting on high voltage", joint_names_[i].c_str());
+        ROS_WARN("[%s] Waiting on high voltage", joint_names_[i].c_str());
       }
     }
   }
@@ -317,33 +319,34 @@ void MarchHardwareInterface::write(const ros::Duration& elapsed_time)
   }
 }
 
-void MarchHardwareInterface::resetIMotionCubesUntilTheyWork()
+void MarchHardwareInterface::initiateIMC()
 {
-  bool encoderSetCorrectly = false;
-
-  while (!encoderSetCorrectly)
+  ROS_INFO("Resetting all IMC on initialization");
+  for (const std::string& joint_name : joint_names_)
   {
-    encoderSetCorrectly = true;
-    for (int i = 0; i < num_joints_; ++i)
+    Joint joint = marchRobot.getJoint(joint_name);
+
+    if (LOWER_BOUNDARY_ANGLE_IU <= joint.getAngleIU() && joint.getAngleIU() <= UPPER_BOUNDARY_ANGLE_IU)
     {
-      march4cpp::Joint joint = marchRobot.getJoint(joint_names_[i]);
-      if (joint.getAngleIU() == 0)
-      {
-        ROS_ERROR("[%s] Failed (encoder reset)", joint_names_[i].c_str());
-        encoderSetCorrectly = false;
-      }
+      ROS_WARN("Before reset joint: [%s] has angle-value of: %i. Which is within boundary of lower: %i and upper: %i",
+               joint_name.c_str(), joint.getAngleIU(), LOWER_BOUNDARY_ANGLE_IU, UPPER_BOUNDARY_ANGLE_IU);
     }
-    if (!encoderSetCorrectly)
+
+    joint.resetIMotionCube();
+  }
+
+  ROS_INFO("Restarting EtherCAT");
+  marchRobot.stopEtherCAT();
+  marchRobot.startEtherCAT();
+
+  for (const std::string& joint_name : joint_names_)
+  {
+    Joint joint = marchRobot.getJoint(joint_name);
+
+    if (LOWER_BOUNDARY_ANGLE_IU <= joint.getAngleIU() && joint.getAngleIU() <= UPPER_BOUNDARY_ANGLE_IU)
     {
-      // TODO(Martijn) check if you need to reset all joints.
-      for (int i = 0; i < num_joints_; ++i)
-      {
-        march4cpp::Joint joint = marchRobot.getJoint(joint_names_[i]);
-        joint.resetIMotionCube();
-      }
-      ROS_INFO("Restarting EtherCAT");
-      marchRobot.stopEtherCAT();
-      marchRobot.startEtherCAT();
+      ROS_WARN("After reset joint: [%s] has angle-value of: %i. Which is within boundary of lower: %i and upper: %i",
+               joint_name.c_str(), joint.getAngleIU(), LOWER_BOUNDARY_ANGLE_IU, UPPER_BOUNDARY_ANGLE_IU);
     }
   }
 }

march_hardware_interface/src/march_hardware_interface_node.cpp

@@ -32,10 +32,19 @@ int main(int argc, char** argv)
 
   const double loop_hz = ros::param::param("~loop_hz", 100.0);
   ros::Rate rate(loop_hz);
+
   while (ros::ok())
   {
-    march.update(rate.expectedCycleTime());
-    rate.sleep();
+    try
+    {
+      march.update(rate.expectedCycleTime());
+      rate.sleep();
+    }
+    catch (const std::exception& e)
+    {
+      ROS_FATAL("Hardware interface caught an exception during update: %s", e.what());
+      return 1;
+    }
   }
 
   return 0;