driver.cpp

Go to the documentation of this file.

#include "roboteq_driver/controller.h"
#include "roboteq_driver/channel.h"

#include "ros/ros.h"


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

  std::string port = "/dev/ttyUSB0";
  int32_t baud = 115200;
  nh.param<std::string>("port", port, port);
  nh.param<int32_t>("baud", baud, baud);

  // Interface to motor controller.
  roboteq::Controller controller(port.c_str(), baud);

  // Setup channels.
  if (nh.hasParam("channels")) {
    XmlRpc::XmlRpcValue channel_namespaces;
    nh.getParam("channels", channel_namespaces);
    ROS_ASSERT(channel_namespaces.getType() == XmlRpc::XmlRpcValue::TypeArray);
    for (int i = 0; i < channel_namespaces.size(); ++i) 
    {
      ROS_ASSERT(channel_namespaces[i].getType() == XmlRpc::XmlRpcValue::TypeString);
      controller.addChannel(new roboteq::Channel(1 + i, channel_namespaces[i], &controller));
    }
  } else {
    // Default configuration is a single channel in the node's namespace.
    controller.addChannel(new roboteq::Channel(1, "~", &controller));
  } 

  // Attempt to connect and run.
  while (ros::ok()) {
    ROS_DEBUG("Attempting connection to %s at %i baud.", port.c_str(), baud);
    controller.connect();
    if (controller.connected()) {
      ros::AsyncSpinner spinner(1);
      spinner.start();
      while (ros::ok()) {
        controller.spinOnce();
      }
      spinner.stop();
    } else {
      ROS_DEBUG("Problem connecting to serial device.");
      ROS_ERROR_STREAM_ONCE("Problem connecting to port " << port << ". Trying again every 1 second.");
      sleep(1);
    }  
  }

  return 0;
}