gripper_action_controller_impl.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2014, SRI International
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of SRI International nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #ifndef GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
00031 #define GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_IMPL_H
00032 
00033 namespace gripper_action_controller
00034 {
00035 namespace internal
00036 {
00037 std::string getLeafNamespace(const ros::NodeHandle& nh)
00038 {
00039   const std::string complete_ns = nh.getNamespace();
00040   std::size_t id   = complete_ns.find_last_of("/");
00041   return complete_ns.substr(id + 1);
00042 }  
00043 
00044 boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
00045 {
00046   boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
00047 
00048   std::string urdf_str;
00049   // Check for robot_description in proper namespace
00050   if (nh.getParam(param_name, urdf_str))
00051   {
00052     if (!urdf->initString(urdf_str))
00053     {
00054       ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
00055         nh.getNamespace() << ").");
00056       return boost::shared_ptr<urdf::Model>();
00057     }
00058   }
00059   // Check for robot_description in root
00060   else if (!urdf->initParam("robot_description"))
00061   {
00062     ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
00063     return boost::shared_ptr<urdf::Model>();
00064   }
00065   return urdf;
00066 }
00067 
00068 typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
00069 std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
00070 {
00071   std::vector<UrdfJointConstPtr> out;
00072   for (unsigned int i = 0; i < joint_names.size(); ++i)
00073   {
00074     UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
00075     if (urdf_joint)
00076     {
00077       out.push_back(urdf_joint);
00078     }
00079     else
00080     {
00081       ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
00082       return std::vector<UrdfJointConstPtr>();
00083     }
00084   }
00085   return out;
00086 }
00087 
00088 } // namespace
00089 
00090 template <class HardwareInterface>
00091 inline void GripperActionController<HardwareInterface>::
00092 starting(const ros::Time& time)
00093 {  
00094   command_struct_rt_.position_ = joint_.getPosition();
00095   command_struct_rt_.max_effort_ = default_max_effort_;
00096   command_.initRT(command_struct_rt_);
00097 
00098   // Hardware interface adapter
00099   hw_iface_adapter_.starting(ros::Time(0.0));
00100   last_movement_time_ = time;
00101 }
00102  
00103 template <class HardwareInterface>
00104 inline void GripperActionController<HardwareInterface>::
00105 stopping(const ros::Time& time)
00106 {
00107   preemptActiveGoal();
00108 }
00109 
00110 template <class HardwareInterface>
00111 inline void GripperActionController<HardwareInterface>::
00112 preemptActiveGoal()
00113 {
00114   RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00115    
00116   // Cancels the currently active goal
00117   if (current_active_goal)
00118   {
00119     // Marks the current goal as canceled
00120     rt_active_goal_.reset();
00121     if(current_active_goal->gh_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
00122       current_active_goal->gh_.setCanceled();
00123   }
00124 }
00125 
00126 template <class HardwareInterface>
00127 GripperActionController<HardwareInterface>::
00128 GripperActionController()
00129 : verbose_(false) // Set to true during debugging
00130 {}
00131 
00132 template <class HardwareInterface>
00133 bool GripperActionController<HardwareInterface>::init(HardwareInterface* hw,
00134                                                       ros::NodeHandle&   root_nh,
00135                                                       ros::NodeHandle&   controller_nh)
00136 {
00137   using namespace internal;
00138   
00139   // Cache controller node handle
00140   controller_nh_ = controller_nh;
00141   
00142   // Controller name
00143   name_ = getLeafNamespace(controller_nh_);
00144   
00145   // Action status checking update rate
00146   double action_monitor_rate = 20.0;
00147   controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
00148   action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
00149   ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");
00150   
00151   // Controlled joint
00152   controller_nh_.getParam("joint", joint_name_);
00153   if (joint_name_.empty()) 
00154   {
00155     ROS_ERROR_STREAM_NAMED(name_, "Could not find joint name on param server");
00156     return false;
00157   }
00158   
00159   // URDF joints
00160   boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
00161   if (!urdf) 
00162   {
00163     return false;
00164   }
00165   
00166   std::vector<std::string> joint_names;
00167   joint_names.push_back(joint_name_);
00168   std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names);
00169   if (urdf_joints.empty()) 
00170   {
00171     return false;
00172   }
00173   
00174   // Initialize members
00175   // Joint handle
00176   try 
00177   {
00178     joint_ = hw->getHandle(joint_name_);
00179   }
00180   catch (...)
00181   {
00182     ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_name_ << "' in '" <<
00183                            this->getHardwareInterfaceType() << "'.");
00184     return false;
00185   }
00186 
00187   ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
00188                          "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'" <<
00189                          "\n");
00190   
00191   // Default tolerances
00192   controller_nh_.param<double>("goal_tolerance", goal_tolerance_, 0.01);
00193   goal_tolerance_ = fabs(goal_tolerance_);
00194   // Max allowable effort 
00195   controller_nh_.param<double>("max_effort", default_max_effort_, 0.0);
00196   default_max_effort_ = fabs(default_max_effort_);
00197   // Stall - stall velocity threshold, stall timeout
00198   controller_nh_.param<double>("stall_velocity_threshold", stall_velocity_threshold_, 0.001);
00199   controller_nh_.param<double>("stall_timeout", stall_timeout_, 1.0);
00200   
00201   // Hardware interface adapter
00202   hw_iface_adapter_.init(joint_, controller_nh_);
00203 
00204   // Command - non RT version
00205   command_struct_.position_ = joint_.getPosition();
00206   command_struct_.max_effort_ = default_max_effort_;
00207 
00208   // Result
00209   pre_alloc_result_.reset(new control_msgs::GripperCommandResult());
00210   pre_alloc_result_->position = command_struct_.position_;
00211   pre_alloc_result_->reached_goal = false;
00212   pre_alloc_result_->stalled = false;
00213 
00214   // ROS API: Action interface
00215   action_server_.reset(new ActionServer(controller_nh_, "gripper_cmd",
00216                                         boost::bind(&GripperActionController::goalCB,   this, _1),
00217                                         boost::bind(&GripperActionController::cancelCB, this, _1),
00218                                         false));
00219   action_server_->start();    
00220   return true;
00221 }
00222 
00223 template <class HardwareInterface>
00224 void GripperActionController<HardwareInterface>::
00225 update(const ros::Time& time, const ros::Duration& period)
00226 {
00227   command_struct_rt_ = *(command_.readFromRT());
00228 
00229   double current_position = joint_.getPosition();
00230   double current_velocity = joint_.getVelocity();
00231 
00232   double error_position = command_struct_rt_.position_ - current_position;
00233   double error_velocity = - current_velocity;
00234     
00235   checkForSuccess(time, error_position, current_position, current_velocity);
00236 
00237   // Hardware interface adapter: Generate and send commands
00238   computed_command_ = hw_iface_adapter_.updateCommand(time, period,
00239                                                       command_struct_rt_.position_, 0.0, 
00240                                                       error_position, error_velocity, command_struct_rt_.max_effort_);
00241 }
00242 
00243 template <class HardwareInterface>
00244 void GripperActionController<HardwareInterface>::
00245 goalCB(GoalHandle gh)
00246 {
00247   ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
00248   
00249   // Precondition: Running controller
00250   if (!this->isRunning())
00251   {
00252     ROS_ERROR_NAMED(name_, "Can't accept new action goals. Controller is not running.");
00253     control_msgs::GripperCommandResult result;
00254     gh.setRejected(result);
00255     return;
00256   }
00257 
00258   // Try to update goal
00259   RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
00260 
00261   // Accept new goal
00262   preemptActiveGoal();
00263   gh.setAccepted();
00264 
00265   // This is the non-realtime command_struct
00266   // We use command_ for sharing
00267   command_struct_.position_ = gh.getGoal()->command.position;
00268   command_struct_.max_effort_ = gh.getGoal()->command.max_effort;
00269   command_.writeFromNonRT(command_struct_);
00270 
00271   pre_alloc_result_->reached_goal = false;
00272   pre_alloc_result_->stalled = false;
00273 
00274   last_movement_time_ = ros::Time::now();
00275     
00276   // Setup goal status checking timer
00277   goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
00278                                                   &RealtimeGoalHandle::runNonRealtime,
00279                                                   rt_goal);
00280   goal_handle_timer_.start();
00281   rt_active_goal_ = rt_goal;
00282 }
00283 
00284 template <class HardwareInterface>
00285 void GripperActionController<HardwareInterface>::
00286 cancelCB(GoalHandle gh)
00287 {
00288   RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00289   
00290   // Check that cancel request refers to currently active goal (if any)
00291   if (current_active_goal && current_active_goal->gh_ == gh)
00292   {
00293     // Reset current goal
00294     rt_active_goal_.reset();
00295     
00296     // Enter hold current position mode
00297     setHoldPosition(ros::Time(0.0));
00298     ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");
00299     
00300     // Mark the current goal as canceled
00301     current_active_goal->gh_.setCanceled();
00302   }
00303 }
00304 
00305 template <class HardwareInterface>
00306 void GripperActionController<HardwareInterface>::
00307 setHoldPosition(const ros::Time& time)
00308 {
00309   command_struct_.position_ = joint_.getPosition();
00310   command_struct_.max_effort_ = default_max_effort_;
00311   command_.writeFromNonRT(command_struct_);
00312 }
00313 
00314 template <class HardwareInterface>
00315 void GripperActionController<HardwareInterface>::
00316 checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity)
00317 {
00318   if(!rt_active_goal_)
00319     return;
00320 
00321   if(rt_active_goal_->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
00322     return;
00323 
00324   if(fabs(error_position) < goal_tolerance_)
00325   {
00326     pre_alloc_result_->effort = computed_command_;
00327     pre_alloc_result_->position = current_position;
00328     pre_alloc_result_->reached_goal = true;
00329     pre_alloc_result_->stalled = false;
00330     rt_active_goal_->setSucceeded(pre_alloc_result_);
00331   }
00332   else
00333   {
00334     if(fabs(current_velocity) > stall_velocity_threshold_)
00335     {
00336       last_movement_time_ = time;
00337     }
00338     else if( (time - last_movement_time_).toSec() > stall_timeout_)
00339     {
00340       pre_alloc_result_->effort = computed_command_;
00341       pre_alloc_result_->position = current_position;
00342       pre_alloc_result_->reached_goal = false;
00343       pre_alloc_result_->stalled = true;
00344       rt_active_goal_->setAborted(pre_alloc_result_);
00345     }
00346   }
00347 }
00348 
00349 } // namespace
00350 
00351 #endif // header guard


gripper_action_controller
Author(s): Sachin Chitta
autogenerated on Fri Aug 28 2015 12:37:03