katana_simple_grasp_planner: katana_simple_grasp_planner: grasp

00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2012 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  * grasp_planner.h
00020  *
00021  *  Created on: 17.11.2012
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #ifndef GRASP_PLANNER_H_
00026 #define GRASP_PLANNER_H_
00027 
00028 #include <ros/ros.h>
00029 #include <tf/transform_broadcaster.h>
00030 #include <pluginlib/class_loader.h>
00031 #include <kinematics_base/kinematics_base.h>
00032 
00033 #include <actionlib/server/simple_action_server.h>
00034 #include <sensor_msgs/JointState.h>
00035 #include <object_manipulation_msgs/GraspPlanningAction.h>
00036 #include <object_manipulation_msgs/GraspPlanningErrorCode.h>
00037 
00038 namespace katana_simple_grasp_planner
00039 {
00040 
00041 class GraspPlanner
00042 {
00043 public:
00044   GraspPlanner();
00045   virtual ~GraspPlanner();
00046 
00047   void main_loop();
00048 
00049 private:
00050   tf::TransformBroadcaster tf_broadcaster_;
00051 
00052   pluginlib::ClassLoader<kinematics::KinematicsBase> kinematics_loader_;
00053   boost::shared_ptr<kinematics::KinematicsBase> kinematics_solver_;
00054 
00055   ros::NodeHandle nh_;
00056   actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction> as_;
00057 
00058   sensor_msgs::JointState pre_grasp_joint_state_;
00059   sensor_msgs::JointState grasp_joint_state_;
00060 
00061 
00062   std::vector<tf::Transform> generate_grasps(double x, double y, double z);
00063   std::vector<double> get_ik(tf::Transform grasp_tf);
00064 
00065   void execute_cb(const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal);
00066 };
00067 
00068 } /* namespace katana_simple_grasp_planner */
00069 #endif /* GRASP_PLANNER_H_ */