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 * joint_movement_adapter.h 00020 * 00021 * Created on: 30.08.2011 00022 * Author: Martin Günther <mguenthe@uos.de> 00023 */ 00024 00025 #ifndef JOINT_MOVEMENT_ADAPTER_H_ 00026 #define JOINT_MOVEMENT_ADAPTER_H_ 00027 00028 #include <ros/ros.h> 00029 00030 #include <vector> 00031 00032 #include <actionlib/server/simple_action_server.h> 00033 #include <actionlib/client/simple_action_client.h> 00034 00035 #include <sensor_msgs/JointState.h> 00036 #include <katana_msgs/JointMovementAction.h> 00037 00038 #include <arm_navigation_msgs/JointLimits.h> 00039 00040 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00041 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h> 00042 00043 #include <angles/angles.h> 00044 #include <urdf/model.h> 00045 #include <urdf_interface/joint.h> 00046 00047 #include <joint_trajectory_generator/trajectory_generation.h> 00048 00049 namespace katana_joint_movement_adapter 00050 { 00051 00052 class JointMovementAdapter 00053 { 00054 typedef actionlib::SimpleActionServer<katana_msgs::JointMovementAction> JMAS; 00055 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JTAC; 00056 00057 public: 00058 JointMovementAdapter(std::string name); 00059 virtual ~JointMovementAdapter(); 00060 00061 void executeCB(const JMAS::GoalConstPtr &goal); 00062 void jointStateCb(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& state); 00063 00064 private: 00065 pr2_controllers_msgs::JointTrajectoryGoal makeRoughTrajectory(const sensor_msgs::JointState &jointGoal); 00066 pr2_controllers_msgs::JointTrajectoryGoal makeFullTrajectory(const pr2_controllers_msgs::JointTrajectoryGoal& goal); 00067 00068 sensor_msgs::JointState limitJointStates(const sensor_msgs::JointState &jointGoal); 00069 00070 JMAS as_; 00071 JTAC ac_; 00072 boost::mutex mutex_; 00073 std::map<std::string, double> current_state_; 00074 ros::Subscriber state_sub_; 00075 bool got_state_; 00076 double max_acc_, max_vel_; 00077 urdf::Model robot_model_; 00078 00079 // std::vector<std::string> gripper_joints_; 00080 00081 sensor_msgs::JointState movement_goal_; 00082 }; 00083 00084 } 00085 00086 #endif /* JOINT_MOVEMENT_ADAPTER_H_ */