driver.cpp
Go to the documentation of this file.
00001 
00026 #include "roboteq_driver/controller.h"
00027 #include "roboteq_driver/channel.h"
00028 
00029 #include "ros/ros.h"
00030 
00031 
00032 int main(int argc, char **argv) {
00033   ros::init(argc, argv, "~");
00034   ros::NodeHandle nh("~");
00035 
00036   std::string port = "/dev/ttyUSB0";
00037   int32_t baud = 115200;
00038   nh.param<std::string>("port", port, port);
00039   nh.param<int32_t>("baud", baud, baud);
00040 
00041   // Interface to motor controller.
00042   roboteq::Controller controller(port.c_str(), baud);
00043 
00044   // Setup channels.
00045   if (nh.hasParam("channels")) {
00046     XmlRpc::XmlRpcValue channel_namespaces;
00047     nh.getParam("channels", channel_namespaces);
00048     ROS_ASSERT(channel_namespaces.getType() == XmlRpc::XmlRpcValue::TypeArray);
00049     for (int i = 0; i < channel_namespaces.size(); ++i) 
00050     {
00051       ROS_ASSERT(channel_namespaces[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00052       controller.addChannel(new roboteq::Channel(1 + i, channel_namespaces[i], &controller));
00053     }
00054   } else {
00055     // Default configuration is a single channel in the node's namespace.
00056     controller.addChannel(new roboteq::Channel(1, "~", &controller));
00057   } 
00058 
00059   // Attempt to connect and run.
00060   while (ros::ok()) {
00061     ROS_DEBUG("Attempting connection to %s at %i baud.", port.c_str(), baud);
00062     controller.connect();
00063     if (controller.connected()) {
00064       ros::AsyncSpinner spinner(1);
00065       spinner.start();
00066       while (ros::ok()) {
00067         controller.spinOnce();
00068       }
00069       spinner.stop();
00070     } else {
00071       ROS_DEBUG("Problem connecting to serial device.");
00072       ROS_ERROR_STREAM_ONCE("Problem connecting to port " << port << ". Trying again every 1 second.");
00073       sleep(1);
00074     }  
00075   }
00076 
00077   return 0;
00078 }


roboteq_driver
Author(s):
autogenerated on Thu Jun 6 2019 18:40:39