cob_head_axis: cob_head_axis: cob_head

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob3_driver
00012  * ROS package name: cob_camera_axis
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Ulrich Reiser, email:ulrich.reiser@ipa.fhg.de
00017  *
00018  * Date of creation: Jan 2010
00019  * ToDo:
00020  *
00021  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00022  *
00023  * Redistribution and use in source and binary forms, with or without
00024  * modification, are permitted provided that the following conditions are met:
00025  *
00026  *       * Redistributions of source code must retain the above copyright
00027  *         notice, this list of conditions and the following disclaimer.
00028  *       * Redistributions in binary form must reproduce the above copyright
00029  *         notice, this list of conditions and the following disclaimer in the
00030  *         documentation and/or other materials provided with the distribution.
00031  *       * Neither the name of the Fraunhofer Institute for Manufacturing 
00032  *         Engineering and Automation (IPA) nor the names of its
00033  *         contributors may be used to endorse or promote products derived from
00034  *         this software without specific prior written permission.
00035  *
00036  * This program is free software: you can redistribute it and/or modify
00037  * it under the terms of the GNU Lesser General Public License LGPL as 
00038  * published by the Free Software Foundation, either version 3 of the 
00039  * License, or (at your option) any later version.
00040  * 
00041  * This program is distributed in the hope that it will be useful,
00042  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00043  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00044  * GNU Lesser General Public License LGPL for more details.
00045  * 
00046  * You should have received a copy of the GNU Lesser General Public 
00047  * License LGPL along with this program. 
00048  * If not, see <http://www.gnu.org/licenses/>.
00049  *
00050  ****************************************************************/
00051 
00052 //##################
00053 //#### includes ####
00054 
00055 // standard includes
00056 //--
00057 
00058 // ROS includes
00059 #include <ros/ros.h>
00060 #include <urdf/model.h>
00061 #include <actionlib/server/simple_action_server.h>
00062 
00063 // ROS message includes
00064 #include <sensor_msgs/JointState.h>
00065 //#include <pr2_controllers_msgs/JointTrajectoryAction.h>
00066 #include <control_msgs/FollowJointTrajectoryAction.h>
00067 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00068 #include <diagnostic_msgs/DiagnosticArray.h>
00069 
00070 // ROS service includes
00071 #include <cob_srvs/Trigger.h>
00072 #include <cob_srvs/SetOperationMode.h>
00073 #include <cob_srvs/SetDefaultVel.h>
00074 
00075 // external includes
00076 #include <cob_camera_axis/ElmoCtrl.h>
00077 
00078 //####################
00079 //#### node class ####
00080 class NodeClass
00081 {
00082         //
00083         public:
00084         // create a handle for this node, initialize node
00085         ros::NodeHandle n_;
00086                 
00087         // declaration of topics to publish
00088         ros::Publisher topicPub_JointState_;
00089         ros::Publisher topicPub_ControllerState_;
00090         ros::Publisher topicPub_Diagnostic_;
00091         
00092         // declaration of topics to subscribe, callback is called for new messages arriving
00093         ros::Subscriber topicSub_JointCommand_;
00094         
00095         // declaration of service servers
00096         ros::ServiceServer srvServer_Init_;
00097         ros::ServiceServer srvServer_Stop_;
00098         ros::ServiceServer srvServer_Recover_;
00099         ros::ServiceServer srvServer_SetOperationMode_;
00100         ros::ServiceServer srvServer_SetDefaultVel_;
00101                 
00102         // declaration of service clients
00103         //--
00104 
00105         // action lib server
00106         //actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> as_;
00107         //std::string action_name_;
00108         //pr2_controllers_msgs::JointTrajectoryFeedback feedback_;
00109         //pr2_controllers_msgs::JointTrajectoryResult result_;
00110         actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00111         std::string action_name_;
00112         control_msgs::FollowJointTrajectoryFeedback feedback_;
00113         control_msgs::FollowJointTrajectoryResult result_;
00114         
00115         // global variables
00116         ElmoCtrl * CamAxis_;
00117         ElmoCtrlParams* CamAxisParams_;
00118         
00119         std::string CanDevice_;
00120         std::string CanIniFile_;
00121         int CanBaudrate_;
00122         int HomingDir_;
00123         int HomingDigIn_;
00124         double MaxVel_;
00125         int ModID_;
00126         std::string operationMode_;
00127         double LowerLimit_;
00128         double UpperLimit_; 
00129         double Offset_;
00130         int MotorDirection_;
00131         int EnoderIncrementsPerRevMot_;
00132         double GearRatio_;
00133         
00134         std::string JointName_;
00135         bool isInitialized_;
00136         bool isError_;
00137         bool finished_;
00138         double ActualPos_;
00139         double ActualVel_;
00140         double GoalPos_;
00141         trajectory_msgs::JointTrajectory traj_;
00142         trajectory_msgs::JointTrajectoryPoint traj_point_;
00143         unsigned int traj_point_nr_;
00144 
00145         // Constructor
00146         NodeClass(std::string name):
00147                 as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
00148                 action_name_(name)
00149         {
00150                 n_ = ros::NodeHandle("~");
00151         
00152                 isInitialized_ = false;
00153                 isError_ = false;
00154                 ActualPos_=0.0;
00155                 ActualVel_=0.0;
00156 
00157                 CamAxis_ = new ElmoCtrl();
00158                 CamAxisParams_ = new ElmoCtrlParams();
00159 
00160                 // implementation of topics to publish
00161                 topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00162                 topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
00163                 topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00164 
00165 
00166                 // implementation of topics to subscribe
00167                 
00168                 // implementation of service servers
00169                 srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
00170                 srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
00171                 srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
00172                 srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
00173                 srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
00174                 
00175                 // implementation of service clients
00176                 //--
00177 
00178                 // read parameters from parameter server
00179                 if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");
00180 
00181                 n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
00182                 n_.param<int>("CanBaudrate", CanBaudrate_, 500);
00183                 n_.param<int>("HomingDir", HomingDir_, 1);
00184                 n_.param<int>("HomingDigIn", HomingDigIn_, 11);
00185                 n_.param<int>("ModId",ModID_, 17);
00186                 n_.param<std::string>("JointName",JointName_, "head_axis_joint");
00187                 n_.param<std::string>("CanIniFile",CanIniFile_, "/");
00188                 n_.param<std::string>("operation_mode",operationMode_, "position");
00189                 n_.param<int>("MotorDirection",MotorDirection_, 1);
00190                 n_.param<double>("GearRatio",GearRatio_, 62.5);
00191                 n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
00192                 
00193                 ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
00194                 
00195                 
00196                 // load parameter server string for robot/model
00197                 std::string param_name = "/robot_description";
00198                 std::string full_param_name;
00199                 std::string xml_string;
00200                 n_.searchParam(param_name,full_param_name);
00201                 n_.getParam(full_param_name.c_str(),xml_string);
00202                 ROS_INFO("full_param_name=%s",full_param_name.c_str());
00203                 if (xml_string.size()==0)
00204                 {
00205                         ROS_ERROR("Unable to load robot model from param server robot_description\n");
00206                         exit(2);
00207                 }
00208                 ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00209                 
00210                 // extract limits and velocitys from urdf model
00211                 urdf::Model model;
00212                 if (!model.initString(xml_string))
00213                 {
00214                         ROS_ERROR("Failed to parse urdf file");
00215                         exit(2);
00216                 }
00217                 ROS_INFO("Successfully parsed urdf file");
00218 
00219                 // get LowerLimits out of urdf model
00220                 LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
00221                         //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
00222                 CamAxisParams_->SetLowerLimit(LowerLimit_);
00223 
00224                 // get UpperLimits out of urdf model
00225                 UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
00226                         //std::cout << "LowerLimits[" << JointNames[i].c_str() << "] = " << LowerLimits[i] << std::endl;
00227                 CamAxisParams_->SetUpperLimit(UpperLimit_);
00228 
00229                 // get Offset out of urdf model
00230                 Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
00231                         //std::cout << "Offset[" << JointNames[i].c_str() << "] = " << Offsets[i] << std::endl;
00232                 CamAxisParams_->SetAngleOffset(Offset_);
00233                 
00234                 // get velocitiy out of urdf model
00235                 MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
00236                 ROS_INFO("Successfully read limits from urdf");
00237 
00238 
00239                 //initializing and homing of camera_axis                
00240                 CamAxisParams_->SetCanIniFile(CanIniFile_);
00241                 CamAxisParams_->SetHomingDir(HomingDir_);
00242                 CamAxisParams_->SetHomingDigIn(HomingDigIn_);
00243                 CamAxisParams_->SetMaxVel(MaxVel_);
00244                 CamAxisParams_->SetGearRatio(GearRatio_);
00245                 CamAxisParams_->SetMotorDirection(MotorDirection_);
00246                 CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
00247                 
00248                 
00249                 
00250 
00251                 CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
00252                 
00253 
00254         }
00255         
00256         // Destructor
00257         ~NodeClass() 
00258         {
00259                 delete CamAxis_;
00260         }
00261 
00262         //void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
00263         void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {   
00264                 if(isInitialized_) {    
00265                         ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
00266                         // saving goal into local variables
00267                         traj_ = goal->trajectory;
00268                         traj_point_nr_ = 0;
00269                         traj_point_ = traj_.points[traj_point_nr_];
00270                         GoalPos_ = traj_point_.positions[0];
00271                         finished_ = false;
00272                         
00273                         // stoping axis to prepare for new trajectory
00274                         CamAxis_->Stop();
00275 
00276                         // check that preempt has not been requested by the client
00277                         if (as_.isPreemptRequested())
00278                         {
00279                                 ROS_INFO("%s: Preempted", action_name_.c_str());
00280                                 // set the action state to preempted
00281                                 as_.setPreempted();
00282                         }
00283                         
00284                         usleep(2000000); // needed sleep until drive starts to change status from idle to moving
00285                         
00286                         while (not finished_)
00287                         {
00288                                 if (as_.isNewGoalAvailable())
00289                                 {
00290                                         ROS_WARN("%s: Aborted", action_name_.c_str());
00291                                         as_.setAborted();
00292                                         return;
00293                                 }
00294                                 usleep(10000);
00295                                 //feedback_ = 
00296                                 //as_.send feedback_
00297                         }
00298 
00299                         // set the action state to succeed                      
00300                         //result_.result.data = "executing trajectory";
00301                         ROS_INFO("%s: Succeeded", action_name_.c_str());
00302                         // set the action state to succeeded
00303                         as_.setSucceeded(result_);
00304                 
00305                 } else {
00306                         as_.setAborted();
00307                         ROS_WARN("Camera_axis not initialized yet!");
00308                 }
00309         }
00310 
00311         // service callback functions
00312         // function will be called when a service is querried
00313         bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00314                                   cob_srvs::Trigger::Response &res )
00315         {
00316                 if (isInitialized_ == false) {
00317                         ROS_INFO("...initializing camera axis...");
00318                         // init axis 
00319                         if (CamAxis_->Init(CamAxisParams_))
00320                         {
00321                                 CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
00322                                 ROS_INFO("Initializing of camera axis successfully");
00323                                 isInitialized_ = true;
00324                                 res.success.data = true;
00325                                 res.error_message.data = "initializing camera axis successfully";
00326                         }
00327                         else
00328                         {
00329                                 ROS_ERROR("Initializing camera axis not successfully \n");
00330                                 res.success.data = false;
00331                                 res.error_message.data = "initializing camera axis not successfully";
00332                         }
00333                         }
00334                         else
00335                         {
00336                                 ROS_WARN("...camera axis already initialized...");                      
00337                                 res.success.data = true;
00338                                 res.error_message.data = "camera axis already initialized";
00339                 }
00340                 
00341                 return true;
00342         }
00343 
00344         bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
00345                                   cob_srvs::Trigger::Response &res )
00346         {
00347                 ROS_INFO("Stopping camera axis");
00348                 if(isInitialized_)
00349                 {
00350                         // stopping all movements
00351                         if (CamAxis_->Stop()) {
00352                                 ROS_INFO("Stopping camera axis successfully");
00353                                 res.success.data = true;
00354                                 res.error_message.data = "camera axis stopped successfully";
00355                         }
00356                         else {
00357                                 ROS_ERROR("Stopping camera axis not successfully. error");
00358                                 res.success.data = false;
00359                                 res.error_message.data = "stopping camera axis not successfully";
00360                         }
00361                 }
00362                 return true;
00363         }
00364         
00365         bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
00366                                          cob_srvs::Trigger::Response &res )
00367         {
00368                 if (isInitialized_) {
00369                         ROS_INFO("Recovering camera axis");
00370                         
00371                         // stopping all arm movements
00372                         if (CamAxis_->RecoverAfterEmergencyStop()) {
00373                                 ROS_INFO("Recovering camera axis successfully");
00374                                 res.success.data = true;
00375                                 res.error_message.data = "camera axis successfully recovered";
00376                         } else {
00377                                 ROS_ERROR("Recovering camera axis not successfully. error");
00378                                 res.success.data = false;
00379                                 res.error_message.data = "recovering camera axis failed";
00380                         }
00381                 } else {
00382                         ROS_WARN("...camera axis already recovered...");                        
00383                         res.success.data = true;
00384                         res.error_message.data = "camera axis already recovered";
00385                 }
00386 
00387                 return true;
00388         }
00389 
00397         bool srvCallback_SetOperationMode(      cob_srvs::SetOperationMode::Request &req,
00398                                                                                 cob_srvs::SetOperationMode::Response &res )
00399         {
00400                 ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
00401                 n_.setParam("operation_mode", req.operation_mode.data.c_str());
00402                 res.success.data = true; // 0 = true, else = false
00403                 return true;
00404         }
00405 
00413         bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
00414                                                                         cob_srvs::SetDefaultVel::Response &res )
00415         {
00416                 ROS_INFO("Set default velocity to [%f]", req.default_vel);
00417                 MaxVel_ = req.default_vel;
00418                 CamAxisParams_->SetMaxVel(MaxVel_);
00419                 CamAxis_->setMaxVelocity(MaxVel_);
00420                 res.success.data = true; // 0 = true, else = false
00421                 return true;
00422         }
00423 
00424         // other function declarations
00425         void updateCommands()
00426                 {
00427                         if (isInitialized_ == true)
00428                         {
00429                                 if (operationMode_ == "position")
00430                                 {
00431                                         ROS_DEBUG("moving head_axis in position mode");
00432 
00433                                         if (fabs(ActualVel_) < 0.02)
00434                                         {
00435                                                 //feedback_.isMoving = false;
00436                                 
00437                                                 ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
00438                                                 
00439                                                 if (traj_point_nr_ < traj_.points.size())
00440                                                 {
00441                                                         // if axis is not moving and not reached last point of trajectory, then send new target point
00442                                                         ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
00443                                                         traj_point_ = traj_.points[traj_point_nr_];
00444                                                         CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
00445                                                         usleep(900000);
00446                                                         CamAxis_->m_Joint->requestPosVel();
00447                                                         traj_point_nr_++;
00448                                                         //feedback_.isMoving = true;
00449                                                         //feedback_.pointNr = traj_point_nr;
00450                                                         //as_.publishFeedback(feedback_);
00451                                                 }
00452                                                 else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
00453                                                 {
00454                                                         ROS_DEBUG("...reached end of trajectory");
00455                                                         finished_ = true;
00456                                                 }
00457                                                 else
00458                                                 {
00459                                                         //do nothing until GoalPos_ is reached
00460                                                 }
00461                                         }
00462                                         else
00463                                         {
00464                                                 ROS_INFO("...axis still moving to point[%d]",traj_point_nr_);
00465                                         }
00466                                 }
00467                                 else if (operationMode_ == "velocity")
00468                                 {
00469                                         ROS_WARN("Moving in velocity mode currently disabled");
00470                                 }
00471                                 else
00472                                 {
00473                                         ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
00474                                 }
00475                         }
00476                         else
00477                         {
00478                                 ROS_DEBUG("axis not initialized");
00479                         }
00480                 }
00481         
00482         void publishJointState()
00483         {
00484                 if (isInitialized_ == true) {                   
00485                         // create message
00486                         int DOF = 1;
00487 
00488                         CamAxis_->evalCanBuffer();
00489                         CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
00490                         CamAxis_->m_Joint->requestPosVel();
00491 
00492                         // really bad hack
00493                         ActualPos_ = HomingDir_ * ActualPos_;
00494                         ActualVel_ = HomingDir_ * ActualVel_;
00495 
00496                         sensor_msgs::JointState msg;
00497                         msg.header.stamp = ros::Time::now();
00498                         msg.name.resize(DOF);
00499                         msg.position.resize(DOF);
00500                         msg.velocity.resize(DOF);
00501                         msg.effort.resize(DOF);
00502                         
00503                         msg.name[0] = JointName_;
00504                         msg.position[0] = ActualPos_;
00505                         msg.velocity[0] = ActualVel_;
00506                         msg.effort[0] = 0.0;
00507 
00508 
00509                         //std::cout << "Joint " << msg.name[0] <<": pos="<<  msg.position[0] << " vel=" << msg.velocity[0] << std::endl;
00510                                 
00511                         // publish message
00512                         topicPub_JointState_.publish(msg);
00513 
00514                         // publish controller state message
00515                         pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
00516                         controllermsg.header = msg.header;
00517                         controllermsg.joint_names.resize(DOF);
00518                         controllermsg.desired.positions.resize(DOF);
00519                         controllermsg.desired.velocities.resize(DOF);
00520                         controllermsg.actual.positions.resize(DOF);
00521                         controllermsg.actual.velocities.resize(DOF);
00522                         controllermsg.error.positions.resize(DOF);
00523                         controllermsg.error.velocities.resize(DOF);
00524                         
00525                         controllermsg.joint_names = msg.name;
00526                         controllermsg.desired.positions = msg.position;
00527                         controllermsg.desired.velocities = msg.velocity;
00528                         controllermsg.actual.positions = msg.position;
00529                         controllermsg.actual.velocities = msg.velocity;
00530                         // error, calculated out of desired and actual values
00531                         for (int i = 0; i<DOF; i++ )
00532                         {
00533                                 controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
00534                                 controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
00535                         }
00536                         topicPub_ControllerState_.publish(controllermsg);
00537                 }
00538                 // publishing diagnotic messages
00539             diagnostic_msgs::DiagnosticArray diagnostics;
00540             diagnostics.status.resize(1);
00541             // set data to diagnostics
00542             if(isError_)
00543             {
00544               diagnostics.status[0].level = 2;
00545               diagnostics.status[0].name = "head_axis";
00546               diagnostics.status[0].message = "one or more drives are in Error mode";
00547             }
00548             else
00549             {
00550               if (isInitialized_)
00551               {
00552                 diagnostics.status[0].level = 0;
00553                 diagnostics.status[0].name = n_.getNamespace();
00554                 diagnostics.status[0].message = "head axis initialized and running";
00555               }
00556               else
00557               {
00558                 diagnostics.status[0].level = 1;
00559                 diagnostics.status[0].name = n_.getNamespace();
00560                 diagnostics.status[0].message = "head axis not initialized";
00561               }
00562             }
00563             // publish diagnostic message
00564             topicPub_Diagnostic_.publish(diagnostics);
00565         }
00566 
00567 }; //NodeClass
00568 
00569 
00570 //#######################
00571 //#### main programm ####
00572 int main(int argc, char** argv)
00573 {
00574         // initialize ROS, spezify name of node
00575         ros::init(argc, argv, "cob_camera_axis");
00576         
00577         // create nodeClass
00578         //NodeClass nodeClass(ros::this_node::getName() + "/joint_trajectory_action");
00579         NodeClass nodeClass(ros::this_node::getName() + "/follow_joint_trajectory");
00580  
00581         // main loop
00582         ros::Rate loop_rate(10); // Hz
00583         while(nodeClass.n_.ok()) {
00584           
00585                 // publish JointState
00586                 nodeClass.publishJointState();
00587 
00588                 // update commands
00589                 nodeClass.updateCommands();
00590 
00591                 // sleep and waiting for messages, callbacks 
00592                 ros::spinOnce();
00593                 loop_rate.sleep();
00594         }
00595         
00596         return 0;
00597 }
00598
cob_head_axis.cpp
