gripper_action_controller.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_H
00031 #define GRIPPER_ACTION_CONTROLLER_GRIPPER_ACTION_CONTROLLER_H
00032 
00033 // C++ standard
00034 #include <cassert>
00035 #include <iterator>
00036 #include <stdexcept>
00037 #include <string>
00038 
00039 // Boost
00040 #include <boost/shared_ptr.hpp>
00041 #include <boost/scoped_ptr.hpp>
00042 
00043 // ROS
00044 #include <ros/node_handle.h>
00045 
00046 // URDF
00047 #include <urdf/model.h>
00048 
00049 // ROS messages
00050 #include <control_msgs/GripperCommandAction.h>
00051 
00052 // actionlib
00053 #include <actionlib/server/action_server.h>
00054 
00055 // ros_controls
00056 #include <realtime_tools/realtime_server_goal_handle.h>
00057 #include <controller_interface/controller.h>
00058 #include <hardware_interface/joint_command_interface.h>
00059 #include <hardware_interface/internal/demangle_symbol.h>
00060 #include <realtime_tools/realtime_buffer.h>
00061 
00062 // Project
00063 #include <gripper_action_controller/hardware_interface_adapter.h>
00064 
00065 namespace gripper_action_controller
00066 {
00067 
00074 template <class HardwareInterface>
00075 class GripperActionController : public controller_interface::Controller<HardwareInterface>
00076 {
00077 public:
00078 
00082   struct Commands
00083   {
00084     double position_; // Last commanded position
00085     double max_effort_; // Max allowed effort
00086   };
00087   
00088   GripperActionController();
00089   
00092   bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
00093   /*\}*/
00094   
00098   void starting(const ros::Time& time);
00099 
00101   void stopping(const ros::Time& time);
00102   
00103   void update(const ros::Time& time, const ros::Duration& period);
00104   /*\}*/
00105 
00106   realtime_tools::RealtimeBuffer<Commands> command_;
00107   Commands command_struct_, command_struct_rt_; // pre-allocated memory that is re-used to set the realtime buffer
00108 
00109 private:
00110 
00111   typedef actionlib::ActionServer<control_msgs::GripperCommandAction>                         ActionServer;
00112   typedef boost::shared_ptr<ActionServer>                                                     ActionServerPtr;
00113   typedef ActionServer::GoalHandle                                                            GoalHandle;
00114   typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::GripperCommandAction>        RealtimeGoalHandle;
00115   typedef boost::shared_ptr<RealtimeGoalHandle>                                               RealtimeGoalHandlePtr;
00116 
00117   typedef HardwareInterfaceAdapter<HardwareInterface> HwIfaceAdapter;
00118 
00119   bool                                          update_hold_position_; 
00120   
00121   bool                                         verbose_;            
00122   std::string                                  name_;               
00123   hardware_interface::JointHandle joint_;                          
00124   std::string                     joint_name_;                      
00125 
00126   HwIfaceAdapter                               hw_iface_adapter_;   
00127 
00128   RealtimeGoalHandlePtr                        rt_active_goal_;     
00129   control_msgs::GripperCommandResultPtr        pre_alloc_result_;
00130 
00131   ros::Duration action_monitor_period_;
00132 
00133   // ROS API
00134   ros::NodeHandle    controller_nh_;
00135   ActionServerPtr    action_server_;
00136 
00137   ros::Timer         goal_handle_timer_;
00138 
00139   void goalCB(GoalHandle gh);
00140   void cancelCB(GoalHandle gh);
00141   void preemptActiveGoal();
00142   void setHoldPosition(const ros::Time& time);
00143 
00144   ros::Time last_movement_time_;                                    
00145   double computed_command_;                                         
00146 
00147   double stall_timeout_, stall_velocity_threshold_;                 
00148   double default_max_effort_;                                       
00149   double goal_tolerance_;
00153   void checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity);
00154 
00155 };
00156 
00157 } // namespace
00158 
00159 #include <gripper_action_controller/gripper_action_controller_impl.h>
00160 
00161 #endif // header guard


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