jackal_base.cpp

Go to the documentation of this file.

#include <string>
#include <boost/asio/io_service.hpp>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>

#include "controller_manager/controller_manager.h"
#include "jackal_base/jackal_diagnostic_updater.h"
#include "jackal_base/jackal_hardware.h"
#include "ros/ros.h"
#include "rosserial_server/serial_session.h"

typedef boost::chrono::steady_clock time_source;

void controlThread(ros::Rate rate, jackal_base::JackalHardware* robot, controller_manager::ControllerManager* cm)
{
  time_source::time_point last_time = time_source::now();

  while (1)
  {
    // Calculate monotonic time elapsed
    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;

    robot->copyJointsFromHardware();
    cm->update(ros::Time::now(), elapsed);
    robot->publishDriveFromController();
    rate.sleep();
  }
}

int main(int argc, char* argv[])
{
  // Initialize ROS node.
  ros::init(argc, argv, "jackal_node");
  jackal_base::JackalHardware jackal;

  // Create the serial rosserial server in a background ASIO event loop.
  std::string port;
  ros::param::param<std::string>("~port", port, "/dev/jackal");
  boost::asio::io_service io_service;
  new rosserial_server::SerialSession(io_service, port, 115200);
  boost::thread(boost::bind(&boost::asio::io_service::run, &io_service));

  // Background thread for the controls callback.
  ros::NodeHandle controller_nh("");
  controller_manager::ControllerManager cm(&jackal, controller_nh);
  boost::thread(boost::bind(controlThread, ros::Rate(50), &jackal, &cm));

  // Create diagnostic updater, to update itself on the ROS thread.
  jackal_base::JackalDiagnosticUpdater jackal_diagnostic_updater;

  // Foreground ROS spinner for ROS callbacks, including rosserial, diagnostics
  ros::spin();

  return 0;
}