multi_controller_node.cpp
Go to the documentation of this file.
00001 
00025 #include <string>
00026 #include <vector>
00027 
00028 #include "boost/foreach.hpp"
00029 #include "boost/scoped_ptr.hpp"
00030 #include "boost/shared_ptr.hpp"
00031 #include "serial/serial.h"
00032 
00033 #include "ros/ros.h"
00034 #include "sensor_msgs/JointState.h"
00035 #include "puma_motor_driver/driver.h"
00036 #include "puma_motor_driver/serial_gateway.h"
00037 #include "puma_motor_driver/socketcan_gateway.h"
00038 #include "puma_motor_driver/multi_driver_node.h"
00039 #include "puma_motor_driver/diagnostic_updater.h"
00040 #include "puma_motor_msgs/MultiStatus.h"
00041 #include "puma_motor_msgs/Status.h"
00042 #include "puma_motor_msgs/MultiFeedback.h"
00043 #include "puma_motor_msgs/Feedback.h"
00044 
00045 
00046 class MultiControllerNode
00047 {
00048 public:
00049   MultiControllerNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private,
00050                       puma_motor_driver::Gateway& gateway) :
00051     nh_(nh),
00052     nh_private_(nh_private),
00053     gateway_(gateway),
00054     active_(false),
00055     status_count_(0),
00056     desired_mode_(puma_motor_msgs::Status::MODE_SPEED)
00057   {
00058     drivers_.push_back(puma_motor_driver::Driver(gateway_, 3, "fl"));
00059     drivers_.push_back(puma_motor_driver::Driver(gateway_, 5, "fr"));
00060     drivers_.push_back(puma_motor_driver::Driver(gateway_, 2, "rl"));
00061     drivers_.push_back(puma_motor_driver::Driver(gateway_, 4, "rr"));
00062 
00063     cmd_sub_ = nh_.subscribe("cmd", 1, &MultiControllerNode::cmdCallback, this);
00064 
00065     nh_private_.param<double>("gear_ratio", gear_ratio_, 79.0);
00066     nh_private_.param<int>("encoder_cpr", encoder_cpr_, 1024);
00067     nh_private_.param<int>("frequency", freq_, 25);
00068 
00069 
00070     BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00071     {
00072       driver.clearStatusCache();
00073       driver.setEncoderCPR(encoder_cpr_);
00074       driver.setGearRatio(gear_ratio_);
00075       driver.setMode(desired_mode_, 0.1, 0.01, 0.0);
00076     }
00077 
00078     multi_driver_node_.reset(new puma_motor_driver::MultiDriverNode(nh_, drivers_));
00079   }
00080 
00081 
00082   void cmdCallback(const sensor_msgs::JointStateConstPtr& cmd_msg)
00083   {
00084     if (active_)
00085     {
00086       BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00087       {
00088         for (int i = 0; i < cmd_msg->name.size(); i++)
00089         {
00090           if (driver.deviceName() == cmd_msg->name[i])
00091           {
00092             if (desired_mode_ == puma_motor_msgs::Status::MODE_VOLTAGE)
00093             {
00094               driver.commandDutyCycle(cmd_msg->velocity[i]);
00095             }
00096             else if (desired_mode_ == puma_motor_msgs::Status::MODE_SPEED)
00097             {
00098               driver.commandSpeed(cmd_msg->velocity[i] * 6.28);
00099             }
00100           }
00101         }
00102       }
00103     }
00104   }
00105 
00106 
00107   bool connectIfNotConnected()
00108   {
00109     if (!gateway_.isConnected())
00110     {
00111       if (!gateway_.connect())
00112       {
00113         ROS_ERROR("Error connecting to motor driver gateway. Retrying in 1 second.");
00114         return false;
00115       }
00116       else
00117       {
00118         ROS_INFO("Connection to motor driver gateway successful.");
00119       }
00120     }
00121     return true;
00122   }
00123 
00124   void run()
00125   {
00126     ros::Rate rate(freq_);
00127 
00128     while (ros::ok())
00129     {
00130       if (!connectIfNotConnected())
00131       {
00132         ros::Duration(1.0).sleep();
00133         continue;
00134       }
00135 
00136       if (active_)
00137       {
00138         // Checks to see if power flag has been reset for each driver
00139         BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00140         {
00141           if (driver.lastPower() != 0)
00142           {
00143             active_ = false;
00144             multi_driver_node_->activePublishers(active_);
00145             ROS_WARN("Power reset detected on device ID %d, will reconfigure all drivers.", driver.deviceNumber());
00146             BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00147             {
00148               driver.resetConfiguration();
00149             }
00150           }
00151         }
00152         // Queue data requests for the drivers in order to assemble an amalgamated status message.
00153         BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00154         {
00155           driver.requestStatusMessages();
00156           driver.requestFeedbackSetpoint();
00157         }
00158       }
00159       else
00160       {
00161         // Set parameters for each driver.
00162         BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00163         {
00164           driver.configureParams();
00165         }
00166       }
00167       gateway_.sendAllQueued();
00168       // Process ROS callbacks, which will queue command messages to the drivers.
00169       ros::spinOnce();
00170       gateway_.sendAllQueued();
00171       // ros::Duration(0.005).sleep();
00172 
00173 
00174       // Process all received messages through the connected driver instances.
00175       puma_motor_driver::Message recv_msg;
00176       while (gateway_.recv(&recv_msg))
00177       {
00178         BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00179         {
00180           driver.processMessage(recv_msg);
00181         }
00182       }
00183 
00184       // Check parameters of each driver instance.
00185       if (!active_)
00186       {
00187         BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00188         {
00189           driver.verifyParams();
00190         }
00191       }
00192 
00193       // Verify that the all drivers are configured.
00194       if ( drivers_[0].isConfigured() == true
00195         && drivers_[1].isConfigured() == true
00196         && drivers_[2].isConfigured() == true
00197         && drivers_[3].isConfigured() == true
00198         && active_ == false)
00199       {
00200         active_ = true;
00201         multi_driver_node_->activePublishers(active_);
00202         ROS_INFO("All contollers active.");
00203       }
00204       // Send the broadcast heartbeat message.
00205       // gateway_.heartbeat();
00206       status_count_++;
00207       rate.sleep();
00208     }
00209   }
00210 
00211 private:
00212   ros::NodeHandle nh_;
00213   ros::NodeHandle nh_private_;
00214   puma_motor_driver::Gateway& gateway_;
00215   std::vector<puma_motor_driver::Driver> drivers_;
00216 
00217   int freq_;
00218   int encoder_cpr_;
00219   double gear_ratio_;
00220   uint8_t status_count_;
00221   uint8_t desired_mode_;
00222   bool active_;
00223 
00224   ros::Subscriber cmd_sub_;
00225   boost::shared_ptr<puma_motor_driver::MultiDriverNode> multi_driver_node_;
00226 };
00227 
00228 
00229 int main(int argc, char *argv[])
00230 {
00231   ros::init(argc, argv, "puma_multi_controller_node");
00232   ros::NodeHandle nh;
00233   ros::NodeHandle nh_private("~");
00234 
00235   std::string serial_port;
00236   std::string canbus_dev;
00237 
00238   boost::scoped_ptr<puma_motor_driver::Gateway> gateway;
00239 
00240   if (nh_private.getParam("canbus_dev", canbus_dev))
00241   {
00242     gateway.reset(new puma_motor_driver::SocketCANGateway(canbus_dev));
00243   }
00244   else if (nh_private.getParam("serial_port", serial_port))
00245   {
00246     serial::Serial serial;
00247     serial.setPort(serial_port);
00248     gateway.reset(new puma_motor_driver::SerialGateway(serial));
00249   }
00250   else
00251   {
00252     ROS_FATAL("No communication method given.");
00253     return 1;
00254   }
00255 
00256   puma_motor_driver::PumaMotorDriverDiagnosticUpdater puma_motor_driver_diagnostic_updater;
00257   MultiControllerNode n(nh, nh_private, *gateway);
00258   n.run();
00259 }


puma_motor_driver
Author(s):
autogenerated on Sat Jun 8 2019 18:55:15