ridgeback_base.cpp

Go to the documentation of this file.

#include <string>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>

#include "controller_manager/controller_manager.h"
#include "ridgeback_base/ridgeback_diagnostic_updater.h"
#include "ridgeback_base/ridgeback_hardware.h"
#include "ridgeback_base/ridgeback_cooling.h"
#include "ridgeback_base/ridgeback_lighting.h"
#include "ridgeback_base/passive_joint_publisher.h"
#include "puma_motor_driver/diagnostic_updater.h"
#include "ros/ros.h"
#include "rosserial_server/serial_session.h"

typedef boost::chrono::steady_clock time_source;

void controlThread(ros::Rate rate, ridgeback_base::RidgebackHardware* 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;

    if (robot->isActive())
    {
      robot->powerHasNotReset();
      robot->updateJointsFromHardware();
    }
    else
    {
      robot->configure();
    }

    robot->canSend();

    cm->update(ros::Time::now(), elapsed, robot->inReset());

    if (robot->isActive())
    {
      robot->command();
      robot->requestData();
    }
    else
    {
      robot->verify();
    }

    robot->canSend();
    rate.sleep();
  }
}

void canReadThread(ros::Rate rate, ridgeback_base::RidgebackHardware* robot)
{
  while (1)
  {
    robot->canRead();
    rate.sleep();
  }
}

int main(int argc, char* argv[])
{
  // Initialize ROS node.
  ros::init(argc, argv, "ridgeback_node");
  ros::NodeHandle nh, pnh("~");

  // Create the socket rosserial server in a background ASIO event loop.
  boost::asio::io_service io_service;

  // For use the serial rosserial server to connect over socat. Future work
  // will create a native UDP rosserial server that this can migrate to. Specify
  // baud rate because we have to, but in reality it doesn't matter.
  new rosserial_server::SerialSession(io_service, "/dev/ttyrosserial", 115200);
  boost::thread(boost::bind(&boost::asio::io_service::run, &io_service));

  std::string canbus_dev;
  pnh.param<std::string>("canbus_dev", canbus_dev, "can0");
  puma_motor_driver::SocketCANGateway gateway(canbus_dev);

  ridgeback_base::RidgebackHardware ridgeback(nh, pnh, gateway);

  // Configure the CAN connection
  ridgeback.init();
  // Create a thread to start reading can messages.
  boost::thread canReadT(&canReadThread, ros::Rate(200), &ridgeback);

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

  // Lighting control.
  ridgeback_base::RidgebackLighting lighting(&nh);

  // Create diagnostic updater, to update itself on the ROS thread.
  ridgeback_base::RidgebackDiagnosticUpdater ridgeback_diagnostic_updater;
  puma_motor_driver::PumaMotorDriverDiagnosticUpdater puma_motor_driver_diagnostic_updater;

  // Joint state publisher for passive front axle.
  ros::V_string passive_joint = boost::assign::list_of("front_rocker");
  ridgeback_base::PassiveJointPublisher passive_joint_publisher(nh, passive_joint, 50);

  // Cooling control for the fans.
  ridgeback_base::RidgebackCooling cooling(&nh);

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

  return 0;
}