schunk_powercube_chain.cpp
Go to the documentation of this file.
00001 
00060 //##################
00061 //#### includes ####
00062 //##################
00063 
00064 // standard includes
00065 // --
00066 
00067 // ROS includes
00068 #include <ros/ros.h>
00069 #include <urdf/model.h>
00070 
00071 // ROS message includes
00072 #include <sensor_msgs/JointState.h>
00073 #include <trajectory_msgs/JointTrajectory.h>
00074 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00075 #include <diagnostic_msgs/DiagnosticArray.h>
00076 #include <brics_actuator/JointPositions.h>
00077 #include <brics_actuator/JointVelocities.h>
00078 
00079 
00080 // ROS service includes
00081 #include <cob_srvs/Trigger.h>
00082 #include <cob_srvs/SetOperationMode.h>
00083 
00084 // own includes
00085 #include <schunk_powercube_chain/PowerCubeCtrl.h>
00086 #include <schunk_powercube_chain/PowerCubeCtrlParams.h>
00087 //#include <schunk_powercube_chain/Diagnostics.h>
00088 
00094 class PowerCubeChainNode
00095 {
00096 
00097 public:
00099   ros::NodeHandle n_;
00100 
00102   ros::Publisher topicPub_JointState_;
00103   ros::Publisher topicPub_ControllerState_;
00104   ros::Publisher topicPub_OperationMode_;
00105   ros::Publisher topicPub_Diagnostic_;
00106 
00108   ros::Subscriber topicSub_CommandPos_;
00109   ros::Subscriber topicSub_CommandVel_;
00110 
00112   ros::ServiceServer srvServer_Init_;
00113   ros::ServiceServer srvServer_SetOperationMode_;
00114   ros::ServiceServer srvServer_Stop_;
00115   ros::ServiceServer srvServer_Recover_;
00116 
00118   PowerCubeCtrl* pc_ctrl_;
00119 
00121   PowerCubeCtrlParams* pc_params_;
00122 
00124   bool initialized_;
00125   bool stopped_;
00126   bool error_;
00127   std::string error_msg_;
00128   ros::Time last_publish_time_;
00129 
00131   PowerCubeChainNode()
00132   {
00133         n_ = ros::NodeHandle("~");
00134 
00135     pc_params_ = new PowerCubeCtrlParams();
00136     pc_ctrl_ = new PowerCubeCtrl(pc_params_);
00137 
00139     topicPub_JointState_ = n_.advertise<sensor_msgs::JointState> ("/joint_states", 1);
00140     topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("state", 1);
00141     topicPub_OperationMode_ = n_.advertise<std_msgs::String> ("current_operationmode", 1);
00142     topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00143 
00145     topicSub_CommandPos_ = n_.subscribe("command_pos", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
00146     topicSub_CommandVel_ = n_.subscribe("command_vel", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);
00147 
00149     srvServer_Init_ = n_.advertiseService("init", &PowerCubeChainNode::srvCallback_Init, this);
00150     srvServer_Stop_ = n_.advertiseService("stop", &PowerCubeChainNode::srvCallback_Stop, this);
00151     srvServer_Recover_ = n_.advertiseService("recover", &PowerCubeChainNode::srvCallback_Recover, this);
00152     srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);
00153 
00154     initialized_ = false;
00155     stopped_ = true;
00156     error_ = false;
00157     last_publish_time_ = ros::Time::now();
00158   }
00159 
00161   ~PowerCubeChainNode()
00162   {
00163           bool closed = pc_ctrl_->Close();
00164           if (closed)
00165       ROS_INFO("PowerCube Device closed!");
00166   }
00167 
00171   void getROSParameters()
00172   {
00173 
00175     std::string CanModule;
00176     if (n_.hasParam("can_module"))
00177     {
00178         n_.getParam("can_module", CanModule);
00179     }
00180 
00181     else
00182     {
00183         ROS_ERROR("Parameter can_module not set, shutting down node...");
00184                 n_.shutdown();
00185     }
00186 
00188     std::string CanDevice;
00189     if (n_.hasParam("can_device"))
00190     {
00191         n_.getParam("can_device", CanDevice);
00192     }
00193 
00194     else
00195     {
00196         ROS_ERROR("Parameter can_device not set, shutting down node...");
00197                   n_.shutdown();
00198     }
00199 
00201     int CanBaudrate;
00202     if (n_.hasParam("can_baudrate"))
00203     {
00204         n_.getParam("can_baudrate", CanBaudrate);
00205     }
00206     else
00207     {
00208         ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
00209         n_.shutdown();
00210     }
00211 
00213     XmlRpc::XmlRpcValue ModulIDsXmlRpc;
00214     std::vector<int> ModulIDs;
00215     if (n_.hasParam("modul_ids"))
00216     {
00217         n_.getParam("modul_ids", ModulIDsXmlRpc);
00218     }
00219 
00220     else
00221     {
00222         ROS_ERROR("Parameter modul_ids not set, shutting down node...");
00223         n_.shutdown();
00224     }
00225 
00227     bool UseMoveVel;
00228     if (n_.hasParam("force_use_movevel"))
00229     {
00230         n_.getParam("force_use_movevel", UseMoveVel);
00231                 ROS_INFO("Parameter force_use_movevel set, using moveVel");
00232     }
00233     else
00234     {
00235         ROS_INFO("Parameter force_use_movevel not set, using moveStep");
00236                 UseMoveVel = false;
00237     }
00238         pc_params_->SetUseMoveVel(UseMoveVel);
00239 
00241     ModulIDs.resize(ModulIDsXmlRpc.size());
00242     for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
00243     {
00244         ModulIDs[i] = (int)ModulIDsXmlRpc[i];
00245     }
00246 
00248     pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
00249 
00251     XmlRpc::XmlRpcValue JointNamesXmlRpc;
00252     std::vector<std::string> JointNames;
00253     if (n_.hasParam("joint_names"))
00254     {
00255         n_.getParam("joint_names", JointNamesXmlRpc);
00256     }
00257 
00258     else
00259     {
00260         ROS_ERROR("Parameter joint_names not set, shutting down node...");
00261         n_.shutdown();
00262     }
00263 
00265     JointNames.resize(JointNamesXmlRpc.size());
00266     for (int i = 0; i < JointNamesXmlRpc.size(); i++)
00267     {
00268         JointNames[i] = (std::string)JointNamesXmlRpc[i];
00269     }
00270 
00272     if ((int)JointNames.size() != pc_params_->GetDOF())
00273     {
00274         ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
00275         n_.shutdown();
00276     }
00277     pc_params_->SetJointNames(JointNames);
00278 
00280     XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
00281     std::vector<double> MaxAccelerations;
00282     if (n_.hasParam("max_accelerations"))
00283     {
00284         n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
00285     }
00286 
00287     else
00288     {
00289         ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
00290         n_.shutdown();
00291     }
00292 
00294     MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
00295     for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
00296     {
00297         MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
00298     }
00299 
00301     if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
00302     {
00303         ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
00304         n_.shutdown();
00305     }
00306     pc_params_->SetMaxAcc(MaxAccelerations);
00307 
00309     double Horizon;
00310     if (n_.hasParam("horizon"))
00311     {
00312         n_.getParam("horizon", Horizon);
00313     }
00314 
00315     else
00316     {
00318         Horizon = 0.05;
00319         ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
00320     }
00321     pc_ctrl_->setHorizon(Horizon);
00322   }
00323 
00327   void getRobotDescriptionParameters()
00328   {
00329           unsigned int DOF = pc_params_->GetDOF();
00330           std::vector<std::string> JointNames = pc_params_->GetJointNames();
00331 
00333           std::string param_name = "robot_description";
00334           std::string full_param_name;
00335           std::string xml_string;
00336           n_.searchParam(param_name, full_param_name);
00337           if (n_.hasParam(full_param_name))
00338           {
00339                   n_.getParam(full_param_name.c_str(), xml_string);
00340           }
00341 
00342           else
00343           {
00344                   ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
00345                   n_.shutdown();
00346           }
00347 
00348           if (xml_string.size() == 0)
00349           {
00350                   ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
00351                   n_.shutdown();
00352           }
00353           ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00354 
00356           urdf::Model model;
00357           if (!model.initString(xml_string))
00358           {
00359                   ROS_ERROR("Failed to parse urdf file");
00360                   n_.shutdown();
00361           }
00362           ROS_DEBUG("Successfully parsed urdf file");
00363 
00365           std::vector<double> MaxVelocities(DOF);
00366           for (unsigned int i = 0; i < DOF; i++)
00367           {
00368                   MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
00369           }
00370 
00372           std::vector<double> LowerLimits(DOF);
00373           for (unsigned int i = 0; i < DOF; i++)
00374           {
00375                   LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
00376           }
00377 
00378           // Get upper limits out of urdf model
00379           std::vector<double> UpperLimits(DOF);
00380           for (unsigned int i = 0; i < DOF; i++)
00381           {
00382                   UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
00383           }
00384 
00386           std::vector<double> Offsets(DOF);
00387           for (unsigned int i = 0; i < DOF; i++)
00388           {
00389                   Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
00390           }
00391 
00393           pc_params_->SetMaxVel(MaxVelocities);
00394           pc_params_->SetLowerLimits(LowerLimits);
00395           pc_params_->SetUpperLimits(UpperLimits);
00396           pc_params_->SetOffsets(Offsets);
00397   }
00398 
00405   void topicCallback_CommandPos(const brics_actuator::JointPositions::ConstPtr& msg)
00406   {
00407           ROS_WARN("Received new position command. Skipping command: Position commands currently not implemented");
00408   }
00409 
00416   void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
00417   {
00418           ROS_DEBUG("Received new velocity command");
00419           if (!initialized_)
00420           {     
00421                         ROS_WARN("Skipping command: powercubes not initialized");
00422                         publishState(false);
00423                         return; 
00424                 }
00425                 
00426                 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00427                 {
00428                         publishState(false);
00429                         return; 
00430                 }
00431 
00432  
00433           PowerCubeCtrl::PC_CTRL_STATUS status;
00434           std::vector<std::string> errorMessages;
00435           pc_ctrl_->getStatus(status, errorMessages);
00436 
00438 
00439           unsigned int DOF = pc_params_->GetDOF();
00440           std::vector<std::string> jointNames = pc_params_->GetJointNames();
00441           std::vector<double> cmd_vel(DOF);
00442           std::string unit = "rad";
00443 
00445           if (msg->velocities.size() != DOF)
00446           {
00447                   ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
00448                   return;
00449           }
00450 
00452           for (unsigned int i = 0; i < DOF; i++)
00453           {
00455                   if (msg->velocities[i].joint_uri != jointNames[i])
00456                   {
00457                           ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
00458                           return;
00459                   }
00460 
00462                   if (msg->velocities[i].unit != unit)
00463                   {
00464                           ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
00465                           return;
00466                   }
00467 
00469                   ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
00470                   cmd_vel[i] = msg->velocities[i].value;
00471           }
00472                 
00474           if (!pc_ctrl_->MoveVel(cmd_vel))
00475           {
00476         error_ = true;
00477                         error_msg_ = pc_ctrl_->getErrorMessage();
00478                   ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
00479                   return;
00480           }
00481 
00482           ROS_DEBUG("Executed velocity command");
00483          
00484           publishState(false);
00485   }
00486 
00494   bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00495   {
00496           if (!initialized_)
00497           {
00498                   ROS_INFO("Initializing powercubes...");
00499 
00501                   if (pc_ctrl_->Init(pc_params_))
00502                   {
00503                           initialized_ = true;
00504                           res.success.data = true;
00505                           ROS_INFO("...initializing powercubes successful");
00506                   }
00507 
00508                   else
00509                   {
00510                   error_ = true;
00511                   error_msg_ = pc_ctrl_->getErrorMessage();
00512                           res.success.data = false;
00513                           res.error_message.data = pc_ctrl_->getErrorMessage();
00514                           ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str());
00515                   }
00516           }
00517 
00518           else
00519           {
00520                   res.success.data = true;
00521                   res.error_message.data = "powercubes already initialized";
00522                   ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str());
00523           }
00524 
00525           return true;
00526   }
00527 
00535   bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00536   {
00537           ROS_INFO("Stopping powercubes...");
00538 
00540           if (pc_ctrl_->Stop())
00541           {
00542                   res.success.data = true;
00543                   ROS_INFO("...stopping powercubes successful.");
00544           }
00545 
00546           else
00547           {
00548                   res.success.data = false;
00549                   res.error_message.data = pc_ctrl_->getErrorMessage();
00550                   ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str());
00551           }
00552           return true;
00553   }
00554 
00562   bool srvCallback_Recover(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00563   {
00564           ROS_INFO("Recovering powercubes...");
00565           if (initialized_)
00566           {
00568                   if (pc_ctrl_->Recover())
00569                   {
00570                         error_ = false;
00571                         error_msg_ = "";
00572                         res.success.data = true;
00573                         ROS_INFO("...recovering powercubes successful.");
00574                   }
00575                   else
00576                   {
00577                         res.success.data = false;
00578                         error_ = true;
00579                         error_msg_ = pc_ctrl_->getErrorMessage();
00580                         res.error_message.data = pc_ctrl_->getErrorMessage();
00581                         ROS_ERROR("...recovering powercubes not successful. error: %s", res.error_message.data.c_str());
00582                   }
00583           }
00584           else
00585           {
00586                   res.success.data = false;
00587                   res.error_message.data = "powercubes not initialized";
00588                   ROS_ERROR("...recovering powercubes not successful. error: %s",res.error_message.data.c_str());
00589           }
00590 
00591           return true;
00592   }
00593 
00601   bool srvCallback_SetOperationMode(cob_srvs::SetOperationMode::Request &req, cob_srvs::SetOperationMode::Response &res)
00602   {
00603         if(req.operation_mode.data != "velocity")
00604         {
00605                 ROS_WARN("Powercube chain currently only supports velocity commands");
00606                 res.success.data = false;
00607         }
00608         else
00609         {
00610                 res.success.data = true;
00611         }
00612         return true;
00613   }
00614 
00621   void publishState(bool update=true)
00622   {
00623           if (initialized_)
00624           {
00625                   ROS_DEBUG("publish state");
00626 
00627                   if(update)
00628                         {pc_ctrl_->updateStates();}
00629 
00630                   sensor_msgs::JointState joint_state_msg;
00631                   joint_state_msg.header.stamp = ros::Time::now();
00632                   joint_state_msg.name = pc_params_->GetJointNames();
00633                   joint_state_msg.position = pc_ctrl_->getPositions();
00634                   joint_state_msg.velocity = pc_ctrl_->getVelocities();
00635                   joint_state_msg.effort.resize(pc_params_->GetDOF());
00636 
00637                   pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
00638                   controller_state_msg.header.stamp = joint_state_msg.header.stamp;
00639                   controller_state_msg.joint_names = pc_params_->GetJointNames();
00640                   controller_state_msg.actual.positions = pc_ctrl_->getPositions();
00641                   controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
00642                   controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
00643 
00644                   std_msgs::String opmode_msg;
00645                   opmode_msg.data = "velocity";
00646 
00648                   topicPub_JointState_.publish(joint_state_msg);
00649                   topicPub_ControllerState_.publish(controller_state_msg);
00650                   topicPub_OperationMode_.publish(opmode_msg);
00651 
00652                   last_publish_time_ = joint_state_msg.header.stamp;
00653 
00654           }
00655 
00656         // check status of PowerCube chain
00657         if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
00658         
00659                 // check status of PowerCube chain 
00660                 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) 
00661                 {       
00662                         error_ = true;
00663                 } 
00664                 else 
00665                 {
00666                         error_ = false;
00667                 } 
00668 
00669     // publishing diagnotic messages
00670     diagnostic_msgs::DiagnosticArray diagnostics;
00671     diagnostics.status.resize(1);
00672 
00673     // set data to diagnostics
00674     if(error_)
00675     {
00676       diagnostics.status[0].level = 2;
00677       diagnostics.status[0].name = n_.getNamespace();;
00678       diagnostics.status[0].message =  pc_ctrl_->getErrorMessage();
00679     }
00680     else
00681     {
00682       if (initialized_)
00683       {
00684         diagnostics.status[0].level = 0;
00685         diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
00686         diagnostics.status[0].message = "powercubechain initialized and running";
00687       }
00688       else
00689       {
00690         diagnostics.status[0].level = 1;
00691         diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
00692         diagnostics.status[0].message = "powercubechain not initialized";
00693       }
00694     }
00695     // publish diagnostic message
00696     topicPub_Diagnostic_.publish(diagnostics);
00697   }
00698 }; //PowerCubeChainNode
00699 
00705 int main(int argc, char** argv)
00706 {
00708         ros::init(argc, argv, "powercube_chain");
00709 
00710         // create PowerCubeChainNode
00711         PowerCubeChainNode pc_node;
00712         pc_node.getROSParameters();
00713         pc_node.getRobotDescriptionParameters();
00714 
00716         double frequency;
00717         if (pc_node.n_.hasParam("frequency"))
00718         {
00719                 pc_node.n_.getParam("frequency", frequency);
00720                 //frequency of driver has to be much higher then controller frequency
00721         }
00722 
00723         else
00724         {
00725                 //frequency of driver has to be much higher then controller frequency
00726                 frequency = 10; //Hz
00727                 ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
00728         }
00729 
00730         ros::Duration min_publish_duration;
00731         if (pc_node.n_.hasParam("min_publish_duration"))
00732         {
00733                 double sec;
00734                 pc_node.n_.getParam("min_publish_duration", sec);
00735                 min_publish_duration.fromSec(sec);
00736         }
00737         else
00738         {
00739                 ROS_ERROR("Parameter min_publish_time not available");
00740                 return 0;
00741         }
00742 
00743         if((1.0/min_publish_duration.toSec()) > frequency)
00744         {
00745                 ROS_ERROR("min_publish_duration has to be longer then delta_t of controller frequency!");
00746                 return 0;
00747         }
00748 
00750         ros::Rate loop_rate(frequency); // Hz
00751         while (pc_node.n_.ok())
00752         {
00753                 if ((ros::Time::now() - pc_node.last_publish_time_) >= min_publish_duration)
00754                 {
00755                         pc_node.publishState();
00756                 }
00757 
00759                 ros::spinOnce();
00760                 loop_rate.sleep();
00761         }
00762 
00763         return 0;
00764 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


schunk_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Tue Mar 5 2013 14:44:47