Move init to main

Author: ProjectMarch

march_hardware/src/EtherCAT/EthercatMaster.cpp

@@ -32,7 +32,6 @@ EthercatMaster::EthercatMaster(std::vector<Joint>* jointListPtr, std::string ifn
 
 EthercatMaster::~EthercatMaster()
 {
-  std::cout << "Ethercat master has been deconstructed\n";
   this->stop();
 }
 
@@ -79,7 +78,7 @@ void EthercatMaster::ethercatMasterInitiation()
  */
 void EthercatMaster::ethercatSlaveInitiation()
 {
-  ROS_INFO("Request per-operational state for all slaves");
+  ROS_INFO("Request pre-operational state for all slaves");
   ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
 
   for (auto& i : *jointListPtr)

march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h

@@ -42,12 +42,12 @@ class MarchHardwareInterface : public march_hardware_interface::MarchHardware
    * for each joint.
    */
   void init();
-  void update(const ros::TimerEvent& e);
+  void update(const ros::Duration& elapsed_time);
 
   /**
    * @brief Read actual postion from the hardware.
    */
-  void read(ros::Duration elapsed_time = ros::Duration(0.01));
+  void read(const ros::Duration& elapsed_time = ros::Duration(0.01));
 
   /**
    * @brief Perform all safety checks that might crash the exoskeleton.
@@ -58,18 +58,16 @@ class MarchHardwareInterface : public march_hardware_interface::MarchHardware
    * @brief Write position commands to the hardware.
    * @param elapsed_time Duration since last write action
    */
-  void write(ros::Duration elapsed_time);
+  void write(const ros::Duration& elapsed_time);
 
 protected:
   ::march4cpp::MarchRobot marchRobot;
   ros::NodeHandle nh_;
-  ros::Timer non_realtime_loop_;
   ros::Duration control_period_;
   ros::Duration elapsed_time_;
   PositionJointInterface positionJointInterface;
   PositionJointSoftLimitsInterface positionJointSoftLimitsInterface;
   EffortJointSoftLimitsInterface effortJointSoftLimitsInterface;
-  double loop_hz_;
   bool hasPowerDistributionBoard = false;
   boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
   typedef boost::shared_ptr<realtime_tools::RealtimePublisher<march_shared_resources::ImcErrorState> > RtPublisherPtr;

march_hardware_interface/src/march_hardware_interface.cpp

@@ -29,10 +29,6 @@ namespace march_hardware_interface
 MarchHardwareInterface::MarchHardwareInterface(ros::NodeHandle& nh, AllowedRobot robotName)
   : nh_(nh), marchRobot(HardwareBuilder(robotName).createMarchRobot())
 {
-  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
-  nh_.param("/march/hardware_interface/loop_hz", loop_hz_, 100.0);
-  ros::Duration update_freq = ros::Duration(1.0 / loop_hz_);
-  non_realtime_loop_ = nh_.createTimer(update_freq, &MarchHardwareInterface::update, this);
 }
 
 void MarchHardwareInterface::init()
@@ -212,15 +208,15 @@ void MarchHardwareInterface::init()
     }
   }
   ROS_INFO("Successfully actuated all joints");
+  controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
 }
 
-void MarchHardwareInterface::update(const ros::TimerEvent& e)
+void MarchHardwareInterface::update(const ros::Duration& elapsed_time)
 {
-  elapsed_time_ = ros::Duration(e.current_real - e.last_real);
-  read(elapsed_time_);
+  read(elapsed_time);
   validate();
-  controller_manager_->update(ros::Time::now(), elapsed_time_);
-  write(elapsed_time_);
+  controller_manager_->update(ros::Time::now(), elapsed_time);
+  write(elapsed_time);
 }
 
 void MarchHardwareInterface::validate()
@@ -232,7 +228,7 @@ void MarchHardwareInterface::validate()
   }
 }
 
-void MarchHardwareInterface::read(ros::Duration elapsed_time)
+void MarchHardwareInterface::read(const ros::Duration& elapsed_time)
 {
   for (int i = 0; i < num_joints_; i++)
   {
@@ -272,7 +268,7 @@ void MarchHardwareInterface::read(ros::Duration elapsed_time)
   }
 }
 
-void MarchHardwareInterface::write(ros::Duration elapsed_time)
+void MarchHardwareInterface::write(const ros::Duration& elapsed_time)
 {
   joint_effort_command_copy.clear();
   joint_effort_command_copy.resize(joint_effort_command_.size());

march_hardware_interface/src/march_hardware_interface_node.cpp

@@ -1,4 +1,5 @@
 // Copyright 2019 Project March.
+#include <ros/ros.h>
 #include <march_hardware_interface/march_hardware_interface.h>
 
 int main(int argc, char** argv)
@@ -18,8 +19,24 @@ int main(int argc, char** argv)
   spinner.start();
 
   march_hardware_interface::MarchHardwareInterface march(nh, selected_robot);
-  march.init();
 
-  ros::waitForShutdown();
+  try
+  {
+    march.init();
+  }
+  catch (const std::exception& e)
+  {
+    ROS_FATAL("Hardware interface caught an exception during init: %s", e.what());
+    return 1;
+  }
+
+  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();
+  }
+
   return 0;
 }