cob_trajectory_controller.cpp
Go to the documentation of this file.
00001 
00062 #include "ros/ros.h"
00063 #include <sensor_msgs/JointState.h>
00064 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00065 #include <actionlib/server/simple_action_server.h>
00066 //#include <pr2_controllers_msgs/JointTrajectoryAction.h>
00067 #include <control_msgs/FollowJointTrajectoryAction.h>
00068 
00069 #include <brics_actuator/JointVelocities.h>
00070 #include <cob_trajectory_controller/genericArmCtrl.h>
00071 // ROS service includes
00072 #include <cob_srvs/Trigger.h>
00073 #include <cob_srvs/SetOperationMode.h>
00074 #include <cob_trajectory_controller/SetFloat.h>
00075 
00076 
00077 #define HZ 100
00078 
00079 class cob_trajectory_controller_node
00080 {
00081 private:
00082     ros::NodeHandle n_;
00083 
00084     ros::Publisher joint_vel_pub_;
00085     ros::Subscriber controller_state_;
00086         ros::Subscriber operation_mode_;
00087         ros::ServiceServer srvServer_Stop_;
00088     ros::ServiceServer srvServer_SetVel_;
00089     ros::ServiceServer srvServer_SetAcc_;
00090         ros::ServiceClient srvClient_SetOperationMode;
00091 
00092     //actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> as_;
00093     actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_follow_;
00094  
00095   
00096     //std::string action_name_;
00097     std::string action_name_follow_;    
00098     std::string current_operation_mode_;
00099     XmlRpc::XmlRpcValue JointNames_param_;
00100     std::vector<std::string> JointNames_;
00101     bool executing_;
00102     bool failure_;
00103     bool rejected_;
00104     int DOF;
00105     double velocity_timeout_;
00106 
00107         int watchdog_counter;
00108     genericArmCtrl* traj_generator_;
00109     trajectory_msgs::JointTrajectory traj_;
00110     std::vector<double> q_current, startposition_, joint_distance_;
00111 
00112 public:
00113 
00114 
00115     cob_trajectory_controller_node():
00116     //as_(n_, "joint_trajectory_action", boost::bind(&cob_trajectory_controller_node::executeTrajectory, this, _1), true),
00117     as_follow_(n_, "follow_joint_trajectory", boost::bind(&cob_trajectory_controller_node::executeFollowTrajectory, this, _1), true),
00118     //action_name_("joint_trajectory_action"),
00119     action_name_follow_("follow_joint_trajectory")
00120  
00121     {
00122                 joint_vel_pub_ = n_.advertise<brics_actuator::JointVelocities>("command_vel", 1);
00123         controller_state_ = n_.subscribe("state", 1, &cob_trajectory_controller_node::state_callback, this);
00124                 operation_mode_ = n_.subscribe("current_operationmode", 1, &cob_trajectory_controller_node::operationmode_callback, this);
00125                 srvServer_Stop_ = n_.advertiseService("stop", &cob_trajectory_controller_node::srvCallback_Stop, this);
00126         srvServer_SetVel_ = n_.advertiseService("set_joint_velocity", &cob_trajectory_controller_node::srvCallback_setVel, this);
00127         srvServer_SetAcc_ = n_.advertiseService("set_joint_acceleration", &cob_trajectory_controller_node::srvCallback_setAcc, this);
00128                 srvClient_SetOperationMode = n_.serviceClient<cob_srvs::SetOperationMode>("set_operation_mode");
00129                 while(!srvClient_SetOperationMode.exists())
00130                   {
00131                     ROS_INFO("Waiting for operationmode service to become available");
00132                       sleep(1);
00133                   }
00134         executing_ = false;
00135         failure_ = false;
00136         rejected_ = false;
00137                 watchdog_counter = 0;
00138                 current_operation_mode_ = "undefined";
00139                 double PTPvel = 0.7;
00140                 double PTPacc = 0.2;
00141                 double maxError = 0.7;
00142         velocity_timeout_ = 2.0;
00143                 DOF = 7;
00144                 // get JointNames from parameter server
00145                 ROS_INFO("getting JointNames from parameter server");
00146                 if (n_.hasParam("joint_names"))
00147                 {
00148                         n_.getParam("joint_names", JointNames_param_);
00149                 }
00150                 else
00151                 {
00152                         ROS_ERROR("Parameter joint_names not set");
00153                 }
00154                 JointNames_.resize(JointNames_param_.size());
00155                 for (int i = 0; i<JointNames_param_.size(); i++ )
00156                 {
00157                         JointNames_[i] = (std::string)JointNames_param_[i];
00158                 }
00159                 DOF = JointNames_param_.size();
00160                 if (n_.hasParam("ptp_vel"))
00161                 {
00162                         n_.getParam("ptp_vel", PTPvel);
00163                 }
00164                 if (n_.hasParam("ptp_acc"))
00165                 {
00166                         n_.getParam("ptp_acc", PTPacc);
00167                 }
00168                 if (n_.hasParam("max_error"))
00169                 {
00170                         n_.getParam("max_error", maxError);
00171                 }
00172                 q_current.resize(DOF);
00173                 ROS_INFO("starting controller with DOF: %d PTPvel: %f PTPAcc: %f maxError %f", DOF, PTPvel, PTPacc, maxError);
00174                 traj_generator_ = new genericArmCtrl(DOF, PTPvel, PTPacc, maxError);
00175         }
00176 
00177   double getFrequency()
00178   {
00179     double frequency;
00180       if (n_.hasParam("frequency"))                                                                   
00181       {                                                                                                     
00182         n_.getParam("frequency", frequency);                                                              
00183         ROS_INFO("Setting controller frequency to %f HZ", frequency);                                       
00184       }                                                                                                     
00185     else                                                                                                    
00186       {                                                                                                     
00187         frequency = 100; //Hz                                                                               
00188         ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);          
00189         }
00190       return frequency;
00191   }
00192 
00193         bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00194         {
00195                 ROS_INFO("Stopping trajectory controller.");
00196 
00197                 // stop trajectory controller
00198                 executing_ = false;
00199                 res.success.data = true;
00200                 traj_generator_->isMoving = false;
00201                 //as_.setPreemted();
00202                 failure_ = true;
00203                 return true;
00204         }
00205     bool srvCallback_setVel(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res)
00206     {
00207         ROS_INFO("Setting velocity to %f", req.value.data);
00208         traj_generator_->SetPTPvel(req.value.data);
00209         res.success.data = true;
00210         return true;
00211     }
00212     bool srvCallback_setAcc(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res)
00213     {
00214         ROS_INFO("Setting acceleration to %f", req.value.data);
00215         traj_generator_->SetPTPacc(req.value.data);
00216         res.success.data = true;
00217         return true;
00218     }
00219 
00220   void operationmode_callback(const std_msgs::StringPtr& message)
00221   {
00222     current_operation_mode_ = message->data;
00223   }
00224   void state_callback(const pr2_controllers_msgs::JointTrajectoryControllerStatePtr& message)
00225   {
00226     std::vector<double> positions = message->actual.positions;
00227     for(unsigned int i = 0; i < positions.size(); i++)
00228     {
00229       q_current[i] = positions[i];
00230     }
00231   }
00232 
00233   void spawnTrajector(trajectory_msgs::JointTrajectory trajectory)
00234   {
00235     if(!executing_)
00236         {
00237            //set component to velocity mode
00238           cob_srvs::SetOperationMode opmode;
00239           opmode.request.operation_mode.data = "velocity";
00240           srvClient_SetOperationMode.call(opmode);
00241           ros::Time begin = ros::Time::now();
00242           while(current_operation_mode_ != "velocity")
00243           {
00244              ROS_INFO("waiting for component to go to velocity mode");
00245              usleep(100000);
00246              //add timeout and set action to rejected
00247              if((ros::Time::now() - begin).toSec() > velocity_timeout_)
00248                                 {
00249                rejected_ = true;
00250                return;
00251                                 }  
00252             }
00253             traj_ = trajectory;
00254             if(traj_.points.size() == 1)
00255             {
00256                 traj_generator_->moveThetas(traj_.points[0].positions, q_current);
00257             }
00258             else
00259             {
00260                 //Insert the current point as first point of trajectory, then generate SPLINE trajectory
00261                 trajectory_msgs::JointTrajectoryPoint p;
00262                 p.positions.resize(DOF);
00263                 p.velocities.resize(DOF);
00264                 p.accelerations.resize(DOF);
00265                 for(int i = 0; i<DOF; i++)
00266                 {
00267                     p.positions.at(i) = q_current.at(i);
00268                     p.velocities.at(i) = 0.0;
00269                     p.accelerations.at(i) = 0.0;
00270                 }
00271                 std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
00272                 it = traj_.points.begin();
00273                 traj_.points.insert(it,p);
00274                 traj_generator_->moveTrajectory(traj_, q_current);
00275             }
00276             executing_ = true;
00277             startposition_ = q_current;
00278 
00279             }
00280         else //suspend current movement and start new one
00281         {
00282             
00283         }
00284         while(executing_)
00285         {
00286             sleep(1);
00287         }
00288         
00289   }
00290 
00291 // swaped the main part out to a separate function which will be used by executeFollowTrajectory and executeTrajectory so that code is not duplicated?
00292 /*  void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
00293         ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
00294         spawnTrajector(goal->trajectory);
00295         // only set to succeeded if component could reach position. this is currently not the care for e.g. by emergency stop, hardware error or exceeds limit.
00296         if(rejected_)
00297             as_.setAborted(); //setRejected not implemented in simpleactionserver ?
00298         else
00299         {
00300             if(failure_)
00301                 as_.setAborted();
00302             else
00303                 as_.setSucceeded();
00304         }
00305         rejected_ = false;
00306         failure_ = false;
00307   }
00308 */
00309 
00310      void executeFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) 
00311   {
00312         ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
00313         spawnTrajector(goal->trajectory);
00314         // only set to succeeded if component could reach position. this is currently not the care for e.g. by emergency stop, hardware error or exceeds limit.
00315         if(rejected_)
00316             as_follow_.setAborted(); //setRejected not implemented in simpleactionserver ?
00317         else
00318         {
00319             if(failure_)
00320                 as_follow_.setAborted();
00321             else
00322                 as_follow_.setSucceeded();
00323         }
00324         rejected_ = false;
00325         failure_ = false;     
00326     }
00327     
00328     void run()
00329     {
00330         if(executing_)
00331         {
00332             failure_ = false;
00333                 watchdog_counter = 0;
00334                         if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
00335                         {
00336                                 
00337                                 // set the action state to preempted
00338                                 executing_ = false;
00339                                 traj_generator_->isMoving = false;
00340                                 //as_.setPreempted();
00341                 failure_ = true;
00342                                 ROS_INFO("Preempted trajectory action");
00343                                 return;
00344                         }
00345                 std::vector<double> des_vel;
00346                 if(traj_generator_->step(q_current, des_vel))
00347                 {
00348                         if(!traj_generator_->isMoving) //Finished trajectory
00349                         {
00350                                 executing_ = false;
00351                         }
00352                                 brics_actuator::JointVelocities target_joint_vel;
00353                                 target_joint_vel.velocities.resize(DOF);
00354                                 for(int i=0; i<DOF; i++)
00355                                 {
00356                                         target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
00357                                         target_joint_vel.velocities[i].unit = "rad";
00358                                         target_joint_vel.velocities[i].value = des_vel.at(i);
00359 
00360                                 }
00361 
00362                                 //send everything
00363                                 joint_vel_pub_.publish(target_joint_vel);
00364                 }
00365                 else
00366                 {
00367                         ROS_INFO("An controller error occured!");
00368                 failure_ = true;
00369                         executing_ = false;
00370                 }
00371         }
00372                 else
00373                 {       //WATCHDOG TODO: don't always send
00374                   if(watchdog_counter < 10)
00375                     {
00376                         brics_actuator::JointVelocities target_joint_vel;
00377                         target_joint_vel.velocities.resize(DOF);
00378                         for (int i = 0; i < DOF; i += 1)
00379                         {
00380                                 target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
00381                                 target_joint_vel.velocities[i].unit = "rad";
00382                                 target_joint_vel.velocities[i].value = 0;
00383                         }
00384                                 joint_vel_pub_.publish(target_joint_vel);
00385                         }
00386                   watchdog_counter++;
00387                 }
00388     }
00389     
00390 };
00391 
00392 
00393 
00394 int main(int argc, char ** argv)
00395 {
00396     ros::init(argc, argv, "cob_trajectory_controller");
00397 
00398     cob_trajectory_controller_node tm;
00399 
00401     double frequency = tm.getFrequency();
00402 
00403     ros::Rate loop_rate(frequency);
00404     while (ros::ok())
00405     {
00406         tm.run();
00407         ros::spinOnce();
00408         loop_rate.sleep();
00409     }  
00410         
00411 }
00412 
00413 
00414 
00415 
00416 
00417 


cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Sun Oct 5 2014 23:02:42