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
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
00153 BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00154 {
00155 driver.requestStatusMessages();
00156 driver.requestFeedbackSetpoint();
00157 }
00158 }
00159 else
00160 {
00161
00162 BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00163 {
00164 driver.configureParams();
00165 }
00166 }
00167 gateway_.sendAllQueued();
00168
00169 ros::spinOnce();
00170 gateway_.sendAllQueued();
00171
00172
00173
00174
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
00185 if (!active_)
00186 {
00187 BOOST_FOREACH(puma_motor_driver::Driver& driver, drivers_)
00188 {
00189 driver.verifyParams();
00190 }
00191 }
00192
00193
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
00205
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 }