roch_base.cpp

Go to the documentation of this file.

#include "roch_base/roch_hardware.h"
#include "controller_manager/controller_manager.h"
#include "ros/callback_queue.h"

#include <boost/chrono.hpp>

typedef boost::chrono::steady_clock time_source;

void controlLoop(roch_base::rochHardware &roch,
                 controller_manager::ControllerManager &cm,
                 time_source::time_point &last_time)
{
  // Calculate monotonic time difference
  time_source::time_point this_time = time_source::now();
  boost::chrono::duration<double> elapsed_duration = this_time - last_time;
  ros::Duration elapsed(elapsed_duration.count());
  last_time = this_time;

  // Process control loop
  roch.reportLoopDuration(elapsed);
  roch.updateJointsFromHardware();
  cm.update(ros::Time::now(), elapsed);
  roch.writeCommandsToHardware(); //for difference driver
  //roch.writeOverallSpeedCommandsToHardware(); //for overall speed
}

void diagnosticLoop(roch_base::rochHardware &roch)
{
  roch.updateDiagnostics();
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "roch_base");
  ros::NodeHandle nh, private_nh("~");

  double control_frequency, diagnostic_frequency;
  private_nh.param<double>("control_frequency", control_frequency, 10.0);
  private_nh.param<double>("diagnostic_frequency", diagnostic_frequency, 1.0);

  // Initialize robot hardware and link to controller manager
  roch_base::rochHardware roch(nh, private_nh, control_frequency);
  controller_manager::ControllerManager cm(&roch, nh);
  // Setup separate queue and single-threaded spinner to process timer callbacks
  // that interface with roch hardware - libcore not threadsafe. This
  // avoids having to lock around hardware access, but precludes realtime safety
  // in the control loop.
  ros::CallbackQueue roch_queue;
  ros::AsyncSpinner roch_spinner(1, &roch_queue);
  
  time_source::time_point last_time = time_source::now();
  ros::TimerOptions control_timer(
    ros::Duration(1 / control_frequency),
    boost::bind(controlLoop, boost::ref(roch), boost::ref(cm), boost::ref(last_time)),
    &roch_queue);
  ros::Timer control_loop = nh.createTimer(control_timer);
  
  ros::TimerOptions diagnostic_timer(
    ros::Duration(1 / diagnostic_frequency),
    boost::bind(diagnosticLoop, boost::ref(roch)),
    &roch_queue);
  ros::Timer diagnostic_loop = nh.createTimer(diagnostic_timer);
  roch_spinner.start();
  // Process remainder of ROS callbacks separately, mainly ControlManager related
  ros::spin();

  return 0;
}