cob_powercube_chain: cob_powercube

00001 
00060 //##################
00061 //#### includes ####
00062 
00063 // standard includes
00064 //--
00065 
00066 // ROS includes
00067 #include <ros/ros.h>
00068 #include <urdf/model.h>
00069 #include <actionlib/server/simple_action_server.h>
00070 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00071 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00072 
00073 // ROS message includes
00074 #include <sensor_msgs/JointState.h>
00075 #include <trajectory_msgs/JointTrajectory.h>
00076 
00077 // ROS service includes
00078 #include <cob_srvs/Trigger.h>
00079 #include <cob_srvs/SetOperationMode.h>
00080 #include <cob_srvs/SetDefaultVel.h>
00081 
00082 // ROS diagnostic msgs
00083 #include <diagnostic_updater/diagnostic_updater.h>
00084 
00085 // external includes
00086 #include <cob_powercube_chain/PowerCubeCtrl.h>
00087 #include <cob_powercube_chain/simulatedArm.h>
00088 
00089 //semaphores
00090 #include <fcntl.h>           /* For O_* constants */
00091 #include <sys/stat.h>        /* For mode constants */
00092 #include <semaphore.h>
00093 #include <stdio.h>
00094 #include<unistd.h>
00095 #include <stdlib.h>
00096 
00102 class PowercubeChainNode
00103 {
00104         //
00105         public:
00107                 ros::NodeHandle n_;
00108 
00109                 // declaration of topics to publish
00110                 ros::Publisher topicPub_JointState_;
00111                 ros::Publisher topicPub_ControllerState_;
00112 
00113                 // declaration of topics to subscribe, callback is called for new messages arriving
00114                 ros::Subscriber topicSub_DirectCommand_;
00115 
00116                 // declaration of service servers
00117                 ros::ServiceServer srvServer_Init_;
00118                 ros::ServiceServer srvServer_Stop_;
00119                 ros::ServiceServer srvServer_Recover_;
00120                 ros::ServiceServer srvServer_SetOperationMode_;
00121                 ros::ServiceServer srvServer_SetDefaultVel_;
00122 
00123                 // actionlib server
00124                 actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> as_;
00125                 std::string action_name_;
00126                 // create messages that are used to published feedback/result
00127                 pr2_controllers_msgs::JointTrajectoryFeedback feedback_;
00128                 pr2_controllers_msgs::JointTrajectoryResult result_;
00129 
00130                 // diagnostic stuff
00131                 diagnostic_updater::Updater updater_;
00132 
00133                 // declaration of service clients
00134                 //--
00135 
00136                 // member variables
00137 #ifndef SIMU
00138                 PowerCubeCtrl* PCube_;
00139 #else
00140                 simulatedArm* PCube_;
00141 #endif
00142                 PowerCubeCtrlParams* PCubeParams_;
00143                 std::string CanModule_;
00144                 std::string CanDevice_;
00145                 int CanBaudrate_;
00146                 XmlRpc::XmlRpcValue ModIds_param_;
00147                 std::vector<int> ModIds_;
00148                 XmlRpc::XmlRpcValue JointNames_param_;
00149                 std::vector<std::string> JointNames_;
00150                 XmlRpc::XmlRpcValue MaxAcc_param_;
00151                 std::vector<double> MaxAcc_;
00152                 bool isInitialized_;
00153                 bool finished_;
00154                 std::vector<double>cmd_vel_;
00155                 bool newvel_;
00156                 
00157                 trajectory_msgs::JointTrajectory traj_;
00158                 trajectory_msgs::JointTrajectoryPoint traj_point_;
00159                 int traj_point_nr_;
00160                 sem_t * can_sem ;
00161                 bool sem_can_available;
00162 
00163                 void lock_semaphore(sem_t* sem)
00164                 {
00165                         //printf("trying to lock sem ...");
00166                         //sem_wait(sem);
00167                         //printf ("done\n");
00168                 }
00169                 void unlock_semaphore(sem_t* sem)
00170                 {
00171                         //printf ("Unlocking semaphore ...");
00172                         //sem_post(sem);
00173                         //printf("done!\n");
00174                 }
00175 
00181                 PowercubeChainNode(std::string name):
00182                         as_(n_, name, boost::bind(&PowercubeChainNode::executeCB, this, _1)),
00183                         action_name_(name)
00184                 {
00185                         sem_can_available = false;
00186                         can_sem = SEM_FAILED;
00187 
00188                         isInitialized_ = false;
00189                         traj_point_nr_ = 0;
00190                         finished_ = false;
00191 
00192 #ifndef SIMU
00193                         PCube_ = new PowerCubeCtrl();
00194 #else
00195                         PCube_ = new simulatedArm();
00196 #endif
00197                         PCubeParams_ = new PowerCubeCtrlParams();
00198 
00199                         // implementation of topics to publish
00200                         topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00201                         topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
00202 
00203                         // implementation of topics to subscribe
00204                         topicSub_DirectCommand_ = n_.subscribe("command", 1, &PowercubeChainNode::topicCallback_DirectCommand, this);
00205 
00206                         // implementation of service servers
00207                         srvServer_Init_ = n_.advertiseService("init", &PowercubeChainNode::srvCallback_Init, this);
00208                         srvServer_Stop_ = n_.advertiseService("stop", &PowercubeChainNode::srvCallback_Stop, this);
00209                         srvServer_Recover_ = n_.advertiseService("recover", &PowercubeChainNode::srvCallback_Recover, this);
00210                         srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowercubeChainNode::srvCallback_SetOperationMode, this);
00211                         srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &PowercubeChainNode::srvCallback_SetDefaultVel, this);
00212 
00213                         // implementation of service clients
00214                         //--
00215 
00216                         // diagnostics
00217                         updater_.setHardwareID(ros::this_node::getName());
00218                         updater_.add("initialization", this, &PowercubeChainNode::diag_init);
00219 
00220 
00221                         // read parameters from parameter server
00222                         n_.getParam("can_module", CanModule_);
00223                         n_.getParam("can_device", CanDevice_);
00224                         n_.getParam("can_baudrate", CanBaudrate_);
00225                         ROS_INFO("CanModule=%s, CanDevice=%s, CanBaudrate=%d",CanModule_.c_str(),CanDevice_.c_str(),CanBaudrate_);
00226 
00227                         // get ModIds from parameter server
00228                         if (n_.hasParam("modul_ids"))
00229                         {
00230                                 n_.getParam("modul_ids", ModIds_param_);
00231                         }
00232                         else
00233                         {
00234                                 ROS_ERROR("Parameter modul_ids not set");
00235                         }
00236                         ModIds_.resize(ModIds_param_.size());
00237                         for (int i = 0; i<ModIds_param_.size(); i++ )
00238                         {
00239                                 ModIds_[i] = (int)ModIds_param_[i];
00240                         }
00241                         std::cout << "modul_ids = " << ModIds_param_ << std::endl;
00242                         
00243                         // get JointNames from parameter server
00244                         ROS_INFO("getting JointNames from parameter server");
00245                         if (n_.hasParam("joint_names"))
00246                         {
00247                                 n_.getParam("joint_names", JointNames_param_);
00248                         }
00249                         else
00250                         {
00251                                 ROS_ERROR("Parameter joint_names not set");
00252                         }
00253                         JointNames_.resize(JointNames_param_.size());
00254                         for (int i = 0; i<JointNames_param_.size(); i++ )
00255                         {
00256                                 JointNames_[i] = (std::string)JointNames_param_[i];
00257                         }
00258                         std::cout << "joint_names = " << JointNames_param_ << std::endl;
00259 
00260                         PCubeParams_->Init(CanModule_, CanDevice_, CanBaudrate_, ModIds_);
00261                         
00262                         // get MaxAcc from parameter server
00263                         ROS_INFO("getting max_accelertion from parameter server");
00264                         if (n_.hasParam("max_accelerations"))
00265                         {
00266                                 n_.getParam("max_accelerations", MaxAcc_param_);
00267                         }
00268                         else
00269                         {
00270                                 ROS_ERROR("Parameter max_accelerations not set");
00271                         }
00272                         MaxAcc_.resize(MaxAcc_param_.size());
00273                         for (int i = 0; i<MaxAcc_param_.size(); i++ )
00274                         {
00275                                 MaxAcc_[i] = (double)MaxAcc_param_[i];
00276                         }
00277                         PCubeParams_->SetMaxAcc(MaxAcc_);
00278                         std::cout << "max_accelerations = " << MaxAcc_param_ << std::endl;
00279                         
00280                         // load parameter server string for robot/model
00281                         std::string param_name = "robot_description";
00282                         std::string full_param_name;
00283                         std::string xml_string;
00284                         n_.searchParam(param_name,full_param_name);
00285                         n_.getParam(full_param_name.c_str(),xml_string);
00286                         ROS_INFO("full_param_name=%s",full_param_name.c_str());
00287                         if (xml_string.size()==0)
00288                         {
00289                                 ROS_ERROR("Unable to load robot model from param server robot_description\n");
00290                                 exit(2);
00291                         }
00292                         ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00293                         
00294                         // extract limits and velocities from urdf model
00295                         urdf::Model model;
00296                         if (!model.initString(xml_string))
00297                         {
00298                                 ROS_ERROR("Failed to parse urdf file");
00299                                 exit(2);
00300                         }
00301                         ROS_INFO("Successfully parsed urdf file");
00302 
00305                         // get MaxVel out of urdf model
00306                         std::vector<double> MaxVel;
00307                         MaxVel.resize(ModIds_param_.size());
00308                         for (int i = 0; i<ModIds_param_.size(); i++ )
00309                         {
00310                                 MaxVel[i] = model.getJoint(JointNames_[i].c_str())->limits->velocity;
00311                                 //std::cout << "MaxVel[" << JointNames_[i].c_str() << "] = " << MaxVel[i] << std::endl;
00312                         }
00313                         PCubeParams_->SetMaxVel(MaxVel);
00314                         PCube_->setMaxVelocity(MaxVel);
00315                         
00316                         // get LowerLimits out of urdf model
00317                         std::vector<double> LowerLimits;
00318                         LowerLimits.resize(ModIds_param_.size());
00319                         for (int i = 0; i<ModIds_param_.size(); i++ )
00320                         {
00321                                 LowerLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->lower;
00322                                 std::cout << "LowerLimits[" << JointNames_[i].c_str() << "] = " << LowerLimits[i] << std::endl;
00323                         }
00324                         PCubeParams_->SetLowerLimits(LowerLimits);
00325 
00326                         // get UpperLimits out of urdf model
00327                         std::vector<double> UpperLimits;
00328                         UpperLimits.resize(ModIds_param_.size());
00329                         for (int i = 0; i<ModIds_param_.size(); i++ )
00330                         {
00331                                 UpperLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->upper;
00332                                 std::cout << "UpperLimits[" << JointNames_[i].c_str() << "] = " << UpperLimits[i] << std::endl;
00333                         }
00334                         PCubeParams_->SetUpperLimits(UpperLimits);
00335 
00336                         // get UpperLimits out of urdf model
00337                         std::vector<double> Offsets;
00338                         Offsets.resize(ModIds_param_.size());
00339                         for (int i = 0; i<ModIds_param_.size(); i++ )
00340                         {
00341                                 Offsets[i] = model.getJoint(JointNames_[i].c_str())->calibration->rising.get()[0];
00342                                 std::cout << "Offset[" << JointNames_[i].c_str() << "] = " << Offsets[i] << std::endl;
00343                         }
00344                         PCubeParams_->SetAngleOffsets(Offsets);
00345                         
00346                         cmd_vel_.resize(ModIds_param_.size());
00347                         newvel_ = false;
00348                         can_sem = sem_open("/semcan", O_CREAT, S_IRWXU | S_IRWXG,1);
00349                         if (can_sem != SEM_FAILED)
00350                                 sem_can_available = true;
00351                 }
00352 
00356                 ~PowercubeChainNode()
00357                 {
00358                         bool closed = PCube_->Close();
00359                         if (closed) ROS_INFO("PowerCube Device closed!");
00360                         if (sem_can_available) sem_close(can_sem);
00361                 }
00362 
00369                 void topicCallback_DirectCommand(const trajectory_msgs::JointTrajectory::ConstPtr& msg)
00370                 {
00371                         ROS_DEBUG("Received new direct command");
00372                         newvel_ = true;
00373                         cmd_vel_ = msg->points[0].velocities;
00374                 }
00375 
00376   void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
00377   {
00378     if(isInitialized_)
00379       stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
00380     else
00381       stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
00382     stat.add("Initialized", isInitialized_);
00383   }
00384 
00391                 void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
00392                 {
00393                         ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
00394                         if (!isInitialized_)
00395                         {
00396                                 ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
00397                                 as_.setAborted();
00398                                 return;
00399                         }
00400                         // saving goal into local variables
00401                         traj_ = goal->trajectory;
00402                         traj_point_nr_ = 0;
00403                         traj_point_ = traj_.points[traj_point_nr_];
00404                         finished_ = false;
00405                         
00406                         // stoping arm to prepare for new trajectory
00407                         std::vector<double> VelZero;
00408                         VelZero.resize(ModIds_param_.size());
00409                         PCube_->MoveVel(VelZero);
00410 
00411                         // check that preempt has not been requested by the client
00412                         if (as_.isPreemptRequested())
00413                         {
00414                                 ROS_INFO("%s: Preempted", action_name_.c_str());
00415                                 // set the action state to preempted
00416                                 as_.setPreempted();
00417                         }
00418                         
00419                         usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
00420                         
00421                         while(finished_ == false)
00422                         {
00423                                 if (as_.isNewGoalAvailable())
00424                                 {
00425                                         ROS_WARN("%s: Aborted", action_name_.c_str());
00426                                         as_.setAborted();
00427                                         return;
00428                                 }
00429                                 usleep(10000);
00430                                 //feedback_ = 
00431                                 //as_.send feedback_
00432                         }
00433 
00434                         // set the action state to succeed                      
00435                         //result_.result.data = "executing trajectory";
00436                         ROS_INFO("%s: Succeeded", action_name_.c_str());
00437                         // set the action state to succeeded
00438                         as_.setSucceeded(result_);
00439                 }
00440 
00448                 bool srvCallback_Init(  cob_srvs::Trigger::Request &req,
00449                                                                 cob_srvs::Trigger::Response &res )
00450                 {
00451                         if (isInitialized_ == false)
00452                         {
00453                                 ROS_INFO("...initializing powercubes...");
00454                                 // init powercubes 
00455                                 if (PCube_->Init(PCubeParams_))
00456                                 {
00457                                         ROS_INFO("Initializing succesfull");
00458                                         isInitialized_ = true;
00459                                         res.success.data = true;
00460                                         res.error_message.data = "success";
00461                                 }
00462                                 else
00463                                 {
00464                                         ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
00465                                         res.success.data = false;
00466                                         res.error_message.data = PCube_->getErrorMessage();
00467                                 }
00468                         }
00469                         else
00470                         {
00471                                 ROS_WARN("...powercubes already initialized...");                       
00472                                 res.success.data = true;
00473                                 res.error_message.data = "powercubes already initialized";
00474                         }
00475 
00476                         return true;
00477                 }
00478 
00486                 bool srvCallback_Stop(  cob_srvs::Trigger::Request &req,
00487                                                                 cob_srvs::Trigger::Response &res )
00488                 {
00489                         ROS_INFO("Stopping powercubes");
00490                         newvel_ = false;
00491         
00492                         // set current trajectory to be finished
00493                         traj_point_nr_ = traj_.points.size();
00494         
00495                         // stopping all arm movements
00496                         if (PCube_->Stop())
00497                         {
00498                                 ROS_INFO("Stopping powercubes succesfull");
00499                                 res.success.data = true;
00500                         }
00501                         else
00502                         {
00503                                 ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
00504                                 res.success.data = false;
00505                                 res.error_message.data = PCube_->getErrorMessage();
00506                         }
00507                         return true;
00508                 }
00509 
00517                 bool srvCallback_Recover(       cob_srvs::Trigger::Request &req,
00518                                                                         cob_srvs::Trigger::Response &res )
00519                 {
00520                         if (isInitialized_ == true)
00521                         {
00522                                 ROS_INFO("Recovering powercubes");
00523                 
00524                                 // stopping all arm movements
00525                                 if (PCube_->Stop())
00526                                 {
00527                                         ROS_INFO("Recovering powercubes succesfull");
00528                                         res.success.data = true;
00529                                 }
00530                                 else
00531                                 {
00532                                         ROS_ERROR("Recovering powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
00533                                         res.success.data = false;
00534                                         res.error_message.data = PCube_->getErrorMessage();
00535                                 }
00536                         }
00537                         else
00538                         {
00539                                 ROS_WARN("...powercubes already recovered...");
00540                                 res.success.data = true;
00541                                 res.error_message.data = "powercubes already recovered";
00542                         }
00543 
00544                         return true;
00545                 }
00546 
00554                 bool srvCallback_SetOperationMode(      cob_srvs::SetOperationMode::Request &req,
00555                                                                                         cob_srvs::SetOperationMode::Response &res )
00556                 {
00557                         ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
00558                         n_.setParam("OperationMode", req.operation_mode.data.c_str());
00559                         res.success.data = true; // 0 = true, else = false
00560                         return true;
00561                 }
00562 
00570                 bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
00571                                                 cob_srvs::SetDefaultVel::Response &res )
00572                 {
00573                         ROS_INFO("Set default vel to [%f]", req.default_vel);
00574                         PCube_->setMaxVelocity(double(req.default_vel));
00575                         res.success.data = true; // 0 = true, else = false
00576                         return true;
00577                 }
00578 
00584                 void publishJointState()
00585                 {
00586                   updater_.update();
00587                         if (isInitialized_ == true)
00588                         {
00589                                 // publish joint state message
00590                                 int DOF = ModIds_param_.size();
00591                                 std::vector<double> ActualPos;
00592                                 std::vector<double> ActualVel;
00593                                 ActualPos.resize(DOF);
00594                                 ActualVel.resize(DOF);
00595 
00596                                 lock_semaphore(can_sem);
00597                                 int success = PCube_->getConfig(ActualPos);
00598                                 if (!success) return;
00599                                 PCube_->getJointVelocities(ActualVel);
00600                                 unlock_semaphore(can_sem);
00601 
00602                                 sensor_msgs::JointState msg;
00603                                 msg.header.stamp = ros::Time::now();
00604                                 msg.name.resize(DOF);
00605                                 msg.position.resize(DOF);
00606                                 msg.velocity.resize(DOF);
00607 
00608                                 msg.name = JointNames_;
00609 
00610                                 for (int i = 0; i<DOF; i++ )
00611                                 {
00612                                         msg.position[i] = ActualPos[i];
00613                                         msg.velocity[i] = ActualVel[i];
00614                                         //std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
00615                                 }
00616 
00617                                 topicPub_JointState_.publish(msg);
00618 
00619                                 // publish controller state message
00620                                 pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
00621                                 controllermsg.header.stamp = ros::Time::now();
00622                                 controllermsg.joint_names.resize(DOF);
00623                                 controllermsg.desired.positions.resize(DOF);
00624                                 controllermsg.desired.velocities.resize(DOF);
00625                                 controllermsg.actual.positions.resize(DOF);
00626                                 controllermsg.actual.velocities.resize(DOF);
00627                                 controllermsg.error.positions.resize(DOF);
00628                                 controllermsg.error.velocities.resize(DOF);
00629 
00630                                 controllermsg.joint_names = JointNames_;
00631 
00632                                 for (int i = 0; i<DOF; i++ )
00633                                 {
00634                                         //std::cout << "Joint " << msg.name[i] <<": pos="<<  msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
00635                                         
00636                                         if (traj_point_.positions.size() != 0)
00637                                                 controllermsg.desired.positions[i] = traj_point_.positions[i];
00638                                         else
00639                                                 controllermsg.desired.positions[i] = 0.0;
00640                                         controllermsg.desired.velocities[i] = 0.0;
00641                                         
00642                                         controllermsg.actual.positions[i] = ActualPos[i];
00643                                         controllermsg.actual.velocities[i] = ActualVel[i];
00644                                         
00645                                         controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
00646                                         controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
00647                                 }
00648                                 topicPub_ControllerState_.publish(controllermsg);
00649 
00650                         }
00651                 }
00652 
00658                 void updatePCubeCommands()
00659                 {
00660                         if (isInitialized_ == true)
00661                         {
00662                                 std::string operationMode;
00663                                 n_.getParam("OperationMode", operationMode);
00664                                 if (operationMode == "position")
00665                                 {
00666                                         ROS_DEBUG("moving powercubes in position mode");
00667                                         if (PCube_->statusMoving() == false)
00668                                         {
00669                                                 //feedback_.isMoving = false;
00670                         
00671                                                 ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
00672                         
00673                                                 if (traj_point_nr_ < traj_.points.size())
00674                                                 {
00675                                                         // if powercubes are not moving and not reached last point of trajectory, then send new target point
00676                                                         ROS_DEBUG("...moving to trajectory point[%d]",traj_point_nr_);
00677                                                         traj_point_ = traj_.points[traj_point_nr_];
00678                                                         lock_semaphore(can_sem);
00679                                                         printf("cob_powercube_chain: Moving to position: ");
00680                                                         for (int i = 0; i < traj_point_.positions.size(); i++)
00681                                                         {
00682                                                                 printf("%f ",traj_point_.positions[i]);
00683                                                         }
00684                                                         printf("\n");
00685                                 
00686                                                         PCube_->MoveJointSpaceSync(traj_point_.positions);
00687                                                         unlock_semaphore(can_sem);
00688                                                         traj_point_nr_++;
00689                                                         //feedback_.isMoving = true;
00690                                                         //feedback_.pointNr = traj_point_nr;
00691                                                         //as_.publishFeedback(feedback_);
00692                                                 }
00693                                                 else
00694                                                 {
00695                                                         ROS_DEBUG("...reached end of trajectory");
00696                                                         finished_ = true;
00697                                                 }
00698                                         }
00699                                         else
00700                                         {
00701                                                 ROS_DEBUG("...powercubes still moving to point[%d]",traj_point_nr_);
00702                                         }
00703                                 }
00704                                 else if (operationMode == "velocity")
00705                                 {
00706                                         ROS_DEBUG("moving powercubes in velocity mode");
00707                                         if(newvel_)
00708                                         {       
00709                                                 ROS_INFO("MoveVel Call");
00710                                                 lock_semaphore(can_sem);
00711                                                 PCube_->MoveVel(cmd_vel_);
00712                                                 newvel_ = false;
00713                                                 unlock_semaphore(can_sem);
00714                                         }
00715                                 }
00716                                 else
00717                                 {
00718                                         ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
00719                                 }
00720                         }
00721                         else
00722                         {
00723                                 ROS_DEBUG("powercubes not initialized");
00724                         }
00725                 }
00726 }; //PowercubeChainNode
00727 
00733 int main(int argc, char** argv)
00734 {
00735         // initialize ROS, specify name of node
00736         ros::init(argc, argv, "powercube_chain");
00737 
00738         // create class
00739         PowercubeChainNode pc_node("joint_trajectory_action");
00740 
00741 
00742         
00743 
00744         // main loop
00745         double frequency;
00746         if (pc_node.n_.hasParam("frequency"))
00747         {
00748                 pc_node.n_.getParam("frequency", frequency);
00749         }
00750         else
00751         {
00752                 frequency = 10; //Hz
00753                 ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
00754         }
00755         
00756         ros::Rate loop_rate(frequency); // Hz
00757         while(pc_node.n_.ok())
00758         {
00759                 // publish JointState
00760                 pc_node.publishJointState();
00761 
00762                 // update commands to powercubes
00763                 pc_node.updatePCubeCommands();
00764                 // read parameter
00765                 std::string operationMode;
00766                 pc_node.n_.getParam("OperationMode", operationMode);
00767                 ROS_DEBUG("running with OperationMode [%s]", operationMode.c_str());
00768 
00769                 // sleep and waiting for messages, callbacks 
00770                 ros::spinOnce();
00771                 loop_rate.sleep();
00772         }
00773 
00774         return 0;
00775 }
cob_powercube_chain.cpp
