$search
00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2011 University of Osnabrück 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software 00017 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * katana_gripper_joint_trajectory_controller.h 00020 * 00021 * Created on: 20.10.2011 00022 * Author: Karl Glatz <glatz@hs-weingarten.de> 00023 * Ravensburg-Weingarten, University of Applied Sciences 00024 * 00025 * based on joint_trajectory_action/src/joint_trajectory_action.cpp 00026 * 00027 */ 00028 #ifndef KATANA_GRIPPER_JOINT_TRAJECTORY_CONTROLLER_H_ 00029 #define KATANA_GRIPPER_JOINT_TRAJECTORY_CONTROLLER_H_ 00030 00031 #include <ros/ros.h> 00032 #include <actionlib/server/action_server.h> 00033 #include <spline_smoother/splines.h> 00034 00035 #include <trajectory_msgs/JointTrajectory.h> 00036 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00037 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h> 00038 00039 #include <katana_gazebo_plugins/gazebo_ros_katana_gripper_action_interface.h> 00040 00041 namespace katana_gazebo_plugins 00042 { 00043 00047 static const double GRIPPER_ANGLE_THRESHOLD = 0.005; 00048 00052 class KatanaGripperJointTrajectoryController : public IGazeboRosKatanaGripperAction 00053 { 00054 00055 private: 00056 typedef actionlib::ActionServer<pr2_controllers_msgs::JointTrajectoryAction> JTAS; 00057 typedef JTAS::GoalHandle GoalHandle; 00058 00059 public: 00060 00061 KatanaGripperJointTrajectoryController(ros::NodeHandle pn); 00062 virtual ~KatanaGripperJointTrajectoryController(); 00063 00064 private: 00065 00066 JTAS *action_server_; 00067 00068 bool has_active_goal_; 00069 GoalHandle active_goal_; 00070 trajectory_msgs::JointTrajectory current_traj_; 00071 bool trajectory_finished_; 00072 00073 // the internal state of the gripper 00074 GRKAPoint current_point_; 00075 GRKAPoint last_desired_point_; 00076 00077 std::vector<std::string> joint_names_; 00078 std::map<std::string, double> goal_constraints_; 00079 std::map<std::string, double> trajectory_constraints_; 00080 double goal_time_constraint_; 00081 double stopped_velocity_tolerance_; 00082 00083 // call-back methods 00084 void goalCB(GoalHandle gh); 00085 void cancelCB(GoalHandle gh); 00086 00087 // helper methods 00088 static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b); 00089 void checkGoalStatus(); 00090 bool currentIsDesiredAngle(); 00091 void setCurrentTrajectory(trajectory_msgs::JointTrajectory traj); 00092 bool isTrajectoryFinished(); 00093 00094 public: 00095 // public methods defined by interface IGazeboRosKatanaGripperAction 00096 00097 GRKAPoint getNextDesiredPoint(ros::Time time); 00098 void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00099 00100 void setCurrentPoint(GRKAPoint point) 00101 { 00102 this->current_point_ = point; 00103 this->checkGoalStatus(); 00104 } 00105 00106 void cancelGoal() 00107 { 00108 cancelCB(active_goal_); 00109 } 00110 00114 bool hasActiveGoal() const 00115 { 00116 return has_active_goal_; 00117 } 00118 00119 }; 00120 00121 } 00122 00123 #endif /* KATANA_GRIPPER_JOINT_TRAJECTORY_CONTROLLER_H_ */