00001 #ifndef __EE_CART_IMPED_ACTION_HH__ 00002 #define __EE_CART_IMPED_ACTION_HH__ 00003 00004 #include <ros/ros.h> 00005 #include <actionlib/server/action_server.h> 00006 00007 #include <ee_cart_imped_msgs/EECartImpedAction.h> 00008 #include <geometry_msgs/Pose.h> 00009 #include <tf/transform_listener.h> 00010 #include <string> 00011 00021 class EECartImpedExecuter { 00022 private: 00030 typedef actionlib::ActionServer<ee_cart_imped_msgs::EECartImpedAction> 00031 EECIAS; 00032 00040 typedef EECIAS::GoalHandle GoalHandle; 00041 00043 ros::NodeHandle node_; 00053 EECIAS action_server_; 00054 00056 ee_cart_imped_msgs::EECartImpedResult result_; 00058 ros::Publisher pub_controller_command_; 00060 ros::Subscriber sub_controller_state_; 00062 ros::Timer watchdog_timer_; 00064 tf::TransformListener tf_listener; 00065 00067 bool has_active_goal_; 00069 GoalHandle active_goal_; 00071 ee_cart_imped_msgs::EECartImpedGoal current_traj_; 00072 00077 ee_cart_imped_msgs::EECartImpedFeedbackConstPtr last_controller_state_; 00078 00085 geometry_msgs::Pose goal_constraints_; 00086 00094 double goal_effort_constraint_; 00095 00103 double goal_time_constraint_; 00104 00105 00112 geometry_msgs::Pose trajectory_constraints_; 00113 00121 double trajectory_effort_constraint_; 00122 00129 bool check_header_; 00130 00135 std::string controller_frame_id_; 00136 00152 void watchdog(const ros::TimerEvent &e); 00153 00166 void goalCB(GoalHandle gh); 00167 00174 void cancelCB(GoalHandle gh); 00175 00202 void controllerStateCB(const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr 00203 &msg); 00204 00224 bool checkConstraints 00225 (const ee_cart_imped_msgs::EECartImpedFeedbackConstPtr &msg, 00226 geometry_msgs::Pose pose_constraints, 00227 double effort_constraint); 00228 00229 public: 00230 00248 EECartImpedExecuter(ros::NodeHandle &n); 00249 00254 ~EECartImpedExecuter(); 00255 }; 00256 00257 #endif //ee_cart_imped_action.hh