Merge pull request #72 from ye-luo-xi-tui/dev

Fix realtime loop

Author: qiayuanl

rm_hw/include/rm_hw/control_loop.h

@@ -39,19 +39,16 @@
 
 #include "rm_hw/hardware_interface/hardware_interface.h"
 
-// Timer
 #include <chrono>
-#include <utility>
+#include <thread>
 
-// ROS
 #include <ros/ros.h>
-
-// ROS control
 #include <controller_manager/controller_manager.h>
 
 namespace rm_hw
 {
 using namespace std::chrono;
+using clock = high_resolution_clock;
 
 class RmRobotHWLoop
 {
@@ -63,6 +60,8 @@ class RmRobotHWLoop
    * @param hardware_interface A pointer which point to hardware_interface.
    */
   RmRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr<RmRobotHW> hardware_interface);
+
+  ~RmRobotHWLoop();
   /** \brief Timed method that reads current hardware's state, runs the controller code once and sends the new commands
    * to the hardware.
    *
@@ -73,22 +72,21 @@ class RmRobotHWLoop
    * linearly increasing.
    */
 
-  void update(const ros::TimerEvent&);
+  void update();
 
 private:
   // Startup and shutdown of the internal node inside a roscpp program
   ros::NodeHandle nh_;
 
   // Settings
-  ros::Duration desired_update_freq_;
   double cycle_time_error_threshold_{};
 
   // Timing
-  ros::Timer loop_timer_;
-  ros::Duration elapsed_time_;
+  std::thread loop_thread_;
+  std::atomic_bool loop_running_;
   double loop_hz_{};
-  steady_clock::time_point last_time_;
-  steady_clock::time_point current_time_;
+  ros::Duration elapsed_time_;
+  clock::time_point last_time_;
 
   /** ROS Controller Manager and Runner
 

rm_hw/include/rm_hw/hardware_interface/can_bus.h

@@ -61,7 +61,7 @@ class CanBus
    * \param bus_name Bus's name(example: can0).
    * \param data_ptr Pointer which point to CAN data.
    */
-  CanBus(const std::string& bus_name, CanDataPtr data_ptr);
+  CanBus(const std::string& bus_name, CanDataPtr data_ptr, int thread_priority);
   /** \brief Read active data from read_buffer_ to data_ptr_, such as position, velocity, torque and so on. Clear
    * read_buffer_ after reading.
    *

rm_hw/include/rm_hw/hardware_interface/hardware_interface.h

@@ -102,6 +102,8 @@ class RmRobotHW : public hardware_interface::RobotHW
    */
   void write(const ros::Time& time, const ros::Duration& period) override;
 
+  void setCanBusThreadPriority(int thread_priority);
+
 private:
   /** \brief Check whether some coefficients that are related to actuator are set up and load these coefficients.
    *
@@ -177,6 +179,7 @@ class RmRobotHW : public hardware_interface::RobotHW
   ros::ServiceServer service_server_;
 
   bool is_actuator_specified_ = false;
+  int thread_priority_;
   // Interface
   std::vector<CanBus*> can_buses_{};
   GpioManager gpio_manager_{};

rm_hw/include/rm_hw/hardware_interface/socketcan.h

@@ -73,7 +73,7 @@ class SocketCAN
    *
    * \returns \c true if it successfully open and bind socket.
    */
-  bool open(const std::string& interface, boost::function<void(const can_frame& frame)> handler);
+  bool open(const std::string& interface, boost::function<void(const can_frame& frame)> handler, int thread_priority);
   /** \brief Close and unbind socket.
    *
    */
@@ -91,7 +91,7 @@ class SocketCAN
   /** \brief Starts a new thread, that will wait for socket events.
    *
    */
-  bool startReceiverThread();
+  bool startReceiverThread(int thread_priority);
   /**
    * Pointer to a function which shall be called
    * when frames are being received from the CAN bus

rm_hw/src/control_loop.cpp

@@ -36,51 +36,68 @@
 //
 #include "rm_hw/control_loop.h"
 
-rm_hw::RmRobotHWLoop::RmRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr<RmRobotHW> hardware_interface)
+namespace rm_hw
+{
+RmRobotHWLoop::RmRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr<RmRobotHW> hardware_interface)
   : nh_(nh), hardware_interface_(std::move(hardware_interface))
 {
-  // Create the controller manager
-  controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
-
-  // Load rosparams
-  int error = 0;
+  // Load ros params
+  int error = 0, thread_priority;
   ros::NodeHandle nh_p("~");
   error += !nh_p.getParam("loop_frequency", loop_hz_);
   error += !nh_p.getParam("cycle_time_error_threshold", cycle_time_error_threshold_);
+  error += !nh_p.getParam("thread_priority", thread_priority);
   if (error > 0)
   {
-    char error_message[] =
-        "could not retrieve one of the required parameters\n\trm_hw/loop_hz or rm_hw/cycle_time_error_threshold";
+    char error_message[] = "could not retrieve one of the required parameters\n\trm_hw/loop_hz or "
+                           "rm_hw/cycle_time_error_threshold or "
+                           "rm_hw/thread_priority";
     ROS_ERROR_STREAM(error_message);
     throw std::runtime_error(error_message);
   }
 
+  // Initialise the hardware interface:
+  // 1. retrieve configuration from rosparam
+  // 2. initialise the hardware and interface it with ros_control
+  hardware_interface_->setCanBusThreadPriority(thread_priority);
+  hardware_interface_->init(nh, nh_p);
+  // Create the controller manager
+  controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
+
   // Get current time for use with first update
-  last_time_ = steady_clock::now();
+  last_time_ = clock::now();
 
-  // Start timer that will periodically call RmRobotHWLoop::update
-  desired_update_freq_ = ros::Duration(1 / loop_hz_);
-  loop_timer_ = nh_.createTimer(desired_update_freq_, &RmRobotHWLoop::update, this);
+  // Setup loop thread
+  loop_thread_ = std::thread([&]() {
+    while (loop_running_)
+    {
+      if (loop_running_)
+        update();
+    }
+  });
+  sched_param sched{ .sched_priority = thread_priority };
+  if (pthread_setschedparam(loop_thread_.native_handle(), SCHED_FIFO, &sched) != 0)
+    ROS_WARN("Failed to set threads priority (one possible reason could be that the user and the group permissions "
+             "are not set properly.).\n");
 }
 
-void rm_hw::RmRobotHWLoop::update(const ros::TimerEvent&)
+void RmRobotHWLoop::update()
 {
+  const auto current_time = clock::now();
+  // Compute desired duration rounded to clock decimation
+  const duration<double> desired_duration(1.0 / loop_hz_);
   // Get change in time
-  current_time_ = steady_clock::now();
-  duration<double> time_span = duration_cast<duration<double>>(current_time_ - last_time_);
+  duration<double> time_span = duration_cast<duration<double>>(current_time - last_time_);
   elapsed_time_ = ros::Duration(time_span.count());
-  last_time_ = current_time_;
+  last_time_ = current_time;
 
   // Check cycle time for excess delay
-  const double cycle_time_error = (elapsed_time_ - desired_update_freq_).toSec();
+  const double cycle_time_error = (elapsed_time_ - ros::Duration(desired_duration.count())).toSec();
   if (cycle_time_error > cycle_time_error_threshold_)
-  {
     ROS_WARN_STREAM("Cycle time exceeded error threshold by: " << cycle_time_error - cycle_time_error_threshold_
                                                                << "s, "
                                                                << "cycle time: " << elapsed_time_ << "s, "
                                                                << "threshold: " << cycle_time_error_threshold_ << "s");
-  }
-
   // Input
   // get the hardware's state
   hardware_interface_->read(ros::Time::now(), elapsed_time_);
@@ -92,4 +109,16 @@ void rm_hw::RmRobotHWLoop::update(const ros::TimerEvent&)
   // Output
   // send the new command to hardware
   hardware_interface_->write(ros::Time::now(), elapsed_time_);
+
+  // Sleep
+  const auto sleep_till = current_time + duration_cast<clock::duration>(desired_duration);
+  std::this_thread::sleep_until(sleep_till);
+}
+
+RmRobotHWLoop::~RmRobotHWLoop()
+{
+  loop_running_ = false;
+  if (loop_thread_.joinable())
+    loop_thread_.join();
 }
+}  // namespace rm_hw

rm_hw/src/hardware_interface/can_bus.cpp

@@ -41,10 +41,11 @@
 #include <rm_common/math_utilities.h>
 namespace rm_hw
 {
-CanBus::CanBus(const std::string& bus_name, CanDataPtr data_ptr) : bus_name_(bus_name), data_ptr_(data_ptr)
+CanBus::CanBus(const std::string& bus_name, CanDataPtr data_ptr, int thread_priority)
+  : bus_name_(bus_name), data_ptr_(data_ptr)
 {
   // Initialize device at can_device, false for no loop back.
-  while (!socket_can_.open(bus_name, boost::bind(&CanBus::frameCallback, this, _1)) && ros::ok())
+  while (!socket_can_.open(bus_name, boost::bind(&CanBus::frameCallback, this, _1), thread_priority) && ros::ok())
     ros::Duration(.5).sleep();
 
   ROS_INFO("Successfully connected to %s.", bus_name.c_str());

rm_hw/src/hardware_interface/hardware_interface.cpp

@@ -77,10 +77,12 @@ bool RmRobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     {
       std::string bus_name = xml_rpc_value[i];
       if (bus_name.find("can") != std::string::npos)
-        can_buses_.push_back(new CanBus(bus_name, CanDataPtr{ .type2act_coeffs_ = &type2act_coeffs_,
-                                                              .id2act_data_ = &bus_id2act_data_[bus_name],
-                                                              .id2imu_data_ = &bus_id2imu_data_[bus_name],
-                                                              .id2tof_data_ = &bus_id2tof_data_[bus_name] }));
+        can_buses_.push_back(new CanBus(bus_name,
+                                        CanDataPtr{ .type2act_coeffs_ = &type2act_coeffs_,
+                                                    .id2act_data_ = &bus_id2act_data_[bus_name],
+                                                    .id2imu_data_ = &bus_id2imu_data_[bus_name],
+                                                    .id2tof_data_ = &bus_id2tof_data_[bus_name] },
+                                        thread_priority_));
       else
         ROS_ERROR_STREAM("Unknown bus: " << bus_name);
     }
@@ -175,6 +177,11 @@ void RmRobotHW::write(const ros::Time& time, const ros::Duration& period)
   publishActuatorState(time);
 }
 
+void RmRobotHW::setCanBusThreadPriority(int thread_priority)
+{
+  thread_priority_ = thread_priority;
+}
+
 void RmRobotHW::publishActuatorState(const ros::Time& time)
 {
   if (!is_actuator_specified_)

rm_hw/src/hardware_interface/socketcan.cpp

@@ -54,7 +54,8 @@ SocketCAN::~SocketCAN()
     this->close();
 }
 
-bool SocketCAN::open(const std::string& interface, boost::function<void(const can_frame& frame)> handler)
+bool SocketCAN::open(const std::string& interface, boost::function<void(const can_frame& frame)> handler,
+                     int thread_priority)
 {
   reception_handler = std::move(handler);
   // Request a socket
@@ -86,7 +87,7 @@ bool SocketCAN::open(const std::string& interface, boost::function<void(const ca
     return false;
   }
   // Start a separate, event-driven thread for frame reception
-  return startReceiverThread();
+  return startReceiverThread(thread_priority);
 }
 
 void SocketCAN::close()
@@ -158,7 +159,7 @@ static void* socketcan_receiver_thread(void* argv)
   return nullptr;
 }
 
-bool SocketCAN::startReceiverThread()
+bool SocketCAN::startReceiverThread(int thread_priority)
 {
   // Frame reception is accomplished in a separate, event-driven thread.
   // See also: https://www.thegeekstuff.com/2012/04/create-threads-in-linux/
@@ -170,6 +171,8 @@ bool SocketCAN::startReceiverThread()
     return false;
   }
   ROS_INFO("Successfully started receiver thread with ID %lu", receiver_thread_id_);
+  sched_param sched{ .sched_priority = thread_priority };
+  pthread_setschedparam(receiver_thread_id_, SCHED_FIFO, &sched);
   return true;
 }
 

rm_hw/src/rm_hw.cpp

@@ -41,7 +41,6 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "rm_hw");
   ros::NodeHandle nh;
-  ros::NodeHandle robot_hw_nh("~");
 
   // Run the hardware interface node
   // -------------------------------
@@ -52,21 +51,10 @@ int main(int argc, char** argv)
   ros::AsyncSpinner spinner(2);
   spinner.start();
 
-  struct sched_param params
-  {
-    .sched_priority = 95
-  };
-  if (sched_setscheduler(0, SCHED_FIFO, &params) == -1)
-    ROS_ERROR("Set scheduler failed, RUN THIS NODE AS SUPER USER.\n");
-
   try
   {
     // Create the hardware interface specific to your robot
     std::shared_ptr<rm_hw::RmRobotHW> rm_hw_hw_interface = std::make_shared<rm_hw::RmRobotHW>();
-    // Initialise the hardware interface:
-    // 1. retrieve configuration from rosparam
-    // 2. initialize the hardware and interface it with ros_control
-    rm_hw_hw_interface->init(nh, robot_hw_nh);
 
     // Start the control loop
     rm_hw::RmRobotHWLoop control_loop(nh, rm_hw_hw_interface);