kuka_fri: kuka_fri: ROSComm.cc Source File

00001 
00002 
00003 #include "ROSComm.hh"
00004 #include "FRIComm.hh"
00005 #include "FRICheck.hh"
00006 #include <signal.h>
00007 #include <pthread.h>
00008 
00009 using namespace std;
00010 
00011 ROSComm::ROSComm() :
00012   soft_runstop_handler_(0), n_(0), diagnostic_(0),
00013   running_(false), exitRequested_(false)
00014 {
00015   memset(&status_, 0, sizeof(status_));
00016 }
00017 
00018 ROSComm::~ROSComm()
00019 {
00020   delete n_;
00021   delete diagnostic_;
00022   delete soft_runstop_handler_;
00023 }
00024 
00025 bool ROSComm::configure(FRIThread *fri)
00026 {
00027   if(!ros::master::check())
00028     return false;
00029 
00030   if(!n_)
00031     n_ = new ros::NodeHandle("~");
00032 
00033   int local_port=0, remote_port=0;
00034   std::string remote_ip;
00035   bool ok = true;
00036 
00037   ok &= n_->getParam("local_port", local_port);
00038   ok &= n_->getParam("remote_port", remote_port);
00039   ok &= n_->getParam("remote_ip", remote_ip);
00040 
00041   if(ok)
00042     fri->configureNetwork(local_port, remote_ip.c_str(), remote_port);
00043 
00044   n_->param("side", side_, std::string("none"));
00045 
00046   if(side_ == "left")
00047     fri->limitChecker()->setHandSide(FRICheck::LEFT);
00048   else if(side_ == "right")
00049     fri->limitChecker()->setHandSide(FRICheck::RIGHT);
00050   else
00051     fri->limitChecker()->setHandSide(FRICheck::NONE);
00052 
00053   if(ok)
00054     ROS_INFO("connecting to %s arm: [%d -> %s:%d]",
00055              side_.c_str(), local_port, remote_ip.c_str(), remote_port);
00056 
00057   joint_state_.name.resize(7);
00058   joint_state_.position.resize(7);
00059   joint_state_.velocity.resize(7);
00060   joint_state_.effort.resize(7);
00061 
00062   for(int i=0; i < 7; i++) {
00063     joint_state_.name[i] = side_+string("_arm_")+string(1,'0'+i)+string("_joint");
00064   }
00065 
00066   n_->param("rate", rate_, 125.0);
00067 
00068   return ok;
00069 }
00070 
00071 #define CLAMP(v, min, max) ((v <= min) ? min : ((v >= max) ? max : v))
00072 
00073 void ROSComm::status_update(diagnostic_updater::DiagnosticStatusWrapper &s)
00074 {
00075   int quality = CLAMP(status_.quality, 0, 3);
00076   int state = CLAMP(status_.state, 0, 2);
00077   int control_mode = CLAMP(status_.controlMode, 0, 3);
00078 
00079   char qualities[] = {'x', '-', '+', '*'};
00080   char comm_states[] = {'0', 'M', 'C'};
00081 
00082   unsigned char level = (state == 2 && status_.power == 0x7f)
00083     ? (unsigned char) diagnostic_msgs::DiagnosticStatus::OK
00084     : (unsigned char) diagnostic_msgs::DiagnosticStatus::ERROR;
00085   
00086   s.summaryf(level, "%c%c",
00087     comm_states[state],
00088     qualities[quality]);
00089 
00090   const char *quality_strings[] = {"Unacceptable", "Bad", "Okay", "Perfect"};
00091   const char *state_strings[] = {"Reset", "Monitor", "Command"};
00092   const char *control_modes[] = {"Other", "Position", "Cartesian Impedance", "Joint Impedance"};
00093 
00094   double freq = (status_.cmdSampleTime==0.0) ? 0.0 : 1.0/status_.cmdSampleTime;
00095 
00096   s.addf("Runstop", "%s", (status_.runstop) ? "ON" : "off");
00097   s.addf("Interface Mode", "%s", state_strings[state]);
00098   s.addf("Control Mode", "%s", control_modes[control_mode]);
00099   s.addf("Comm. Status", "%s", (status_.connected) ? "Connected": "Offline");
00100   s.addf("Comm. frequency", "%3.1f Hz", freq);
00101   s.addf("Comm. answer rate", "%3.3f", status_.answerRate);
00102   s.addf("Comm. latency", "%5.5f", status_.latency);
00103   s.addf("Comm. jitter", "%5.5f", status_.jitter);
00104   s.addf("Comm. miss rate", "%5.5f", status_.missRate);
00105   s.addf("Comm. miss counter", "%d", status_.missCounter);
00106   s.addf("Comm. quality", "%s", quality_strings[quality]);
00107   s.addf("Safety Factor", "%f", status_.safety);
00108   s.addf("Mean computation time", "%d us", status_.calcTimeMean);
00109   s.addf("Max computation time", "%d us", status_.calcTimeMax);
00110   s.addf("Motor State", "%s", (status_.power == 0x7f) ? "ON" : "OFF");
00111 
00112   float *t=status_.temperature;
00113   s.addf("Temperatures [deg C]", "%3.1f %3.1f %3.1f %3.1f %3.1f %3.1f %3.1f",
00114          t[0], t[1], t[2], t[3], t[4], t[5], t[6]);
00115 }
00116 
00117 bool ROSComm::open()
00118 {
00119   // avoid blocking for too long...
00120   if(!ros::master::check())
00121     return false;
00122 
00123   if(!n_)
00124     n_ = new ros::NodeHandle("~");
00125 
00126   soft_runstop_handler_ = new soft_runstop::Handler(Duration(0.5));
00127   pub_ = n_->advertise<sensor_msgs::JointState>("/joint_states", 1);
00128   sub_ = n_->subscribe("command", 1, &ROSComm::impedanceCommand, this);
00129 
00130   string Side = side_;
00131   Side[0] = side_[0] + ('A'-'a');
00132 
00133   diagnostic_ = new diagnostic_updater::Updater();
00134   diagnostic_->setHardwareID(string("kuka_lwr_")+string(side_));
00135   diagnostic_->add("Arm "+Side, this, &ROSComm::status_update);
00136 
00137   return true;
00138 }
00139 
00140 
00141 void ROSComm::publishData(const RobotData &data, const RobotCommand &cmd)
00142 {
00143   //TODO: make a timestamp when message is received in FRIComm, not now.
00144   joint_state_.header.stamp = ros::Time::now();
00145 
00146   for(int i=0; i < 7; i++) {
00147     joint_state_.position[i] = data.position[i];  
00148     joint_state_.velocity[i] = 0.0; // TODO: remember old pos and calculate
00149     double dist = cmd.stiffness[i]*fabs(data.position[i] - data.commanded[i]);
00150     double stiffness = cmd.stiffness[i];
00151     joint_state_.effort[i] = stiffness*dist; //TODO: compute effort according to sine distr.
00152   }
00153 
00154   pub_.publish(joint_state_); 
00155 }
00156 
00157 
00158 void ROSComm::publishStatus(const RobotStatus &status)
00159 {
00160   status_ = status;
00161   diagnostic_->update();
00162 }
00163 
00164 
00165 void ROSComm::impedanceCommand(const kuka_fri::ImpedanceCommand::ConstPtr& msg)
00166 {
00167   RobotCommand cmd = fri_->cmd();
00168 
00169   if (msg->velocity.size() == 7)
00170     for (int i=0; i<7; i++)
00171       cmd.command[i] = msg->velocity[i];
00172 
00173   if (msg->stiffness.size() == 7)
00174     for (int i=0; i<7; i++)
00175       cmd.stiffness[i] = msg->stiffness[i];
00176 
00177   if (msg->damping.size() == 7)
00178     for (int i=0; i<7; i++)
00179       cmd.damping[i] = msg->damping[i];
00180 
00181   if (msg->add_torque.size() == 7)
00182     for (int i=0; i<7; i++)
00183       cmd.addTorque[i] = msg->add_torque[i];
00184 
00185   fri_->setCmd(cmd);
00186 }
00187 
00188 
00189 bool ROSComm::runstop()
00190 {
00191   return soft_runstop_handler_->getState();
00192 }
00193 
00194 
00195 void ROSComm::start(FRIThread *fri)
00196 {
00197   fri_ = fri;
00198   pthread_create(&thread_, 0, &ROSComm::run_s, (void *) this);
00199 }
00200 
00201 
00202 void* ROSComm::run()
00203 {
00204   while(!open() && !exitRequested_)
00205     usleep(200000);
00206 
00207   running_ = true;  // roscore is up, we are running
00208 
00209   // some process data
00210   RobotData data;
00211   RobotStatus status;
00212   RobotCommand cmd;
00213 
00214   memset(&status, 0, sizeof(status));
00215 
00216   int seq=0;
00217 
00218   ros::Rate loop_rate(rate_);
00219 
00220   while(!exitRequested_) {
00221 
00222     fri_->setRunstop(runstop());
00223 
00224     cmd = fri_->cmd();
00225     data = fri_->data();
00226     status = fri_->status();
00227 
00228     if(seq != data.seq) {
00229       publishData(data, cmd); // publish results
00230     }
00231     publishStatus(status);  // publish status messages
00232 
00233     seq = data.seq;
00234 
00235     ros::spinOnce();
00236     loop_rate.sleep();
00237   }
00238   running_ = false;
00239 
00240   return 0;
00241 }
00242 
00243 
00244 void ROSComm::finish()
00245 {
00246   exitRequested_=true;
00247   ros::requestShutdown();
00248   pthread_join(thread_, 0);
00249 }