cob_pick_place_action: cob_pick_place

Go to the documentation of this file.
00001 
00030 #ifndef COB_PICK_ACTION_H
00031 #define COB_PICK_ACTION_H
00032 
00033 #include <iostream>
00034 #include <fstream>
00035 #include <ros/ros.h>
00036 #include <ros/package.h>
00037 #include <actionlib/server/simple_action_server.h>
00038 #include <actionlib/client/simple_action_client.h>
00039 #include <actionlib/client/terminal_state.h>
00040 
00041 #include <tf/tf.h>
00042 #include <tf/transform_datatypes.h>
00043 //#include <geometry_msgs/Quaternion.h>
00044 
00045 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00046 #include <moveit/move_group_interface/move_group.h>
00047 #include <shape_tools/solid_primitive_dims.h>
00048 #include <moveit_msgs/Grasp.h>
00049 
00050 #include <cob_pick_place_action/CobPickAction.h>
00051 #include <cob_pick_place_action/CobPlaceAction.h>
00052 #include <cob_grasp_generation/QueryGraspsAction.h>
00053 #include <GraspTable.h>
00054 
00055 
00056 
00057 class CobPickPlaceActionServer
00058 {
00059 private:
00060         ros::NodeHandle nh_;
00061         
00062         ros::Publisher pub_co; //publisher for collision_objects
00063         ros::Publisher pub_ao; //publisher for attached_collision_objects
00064 
00065         boost::scoped_ptr<actionlib::SimpleActionServer<cob_pick_place_action::CobPickAction> > as_pick;
00066         boost::scoped_ptr<actionlib::SimpleActionServer<cob_pick_place_action::CobPlaceAction> > as_place;
00067         
00068         boost::scoped_ptr<actionlib::SimpleActionClient<cob_grasp_generation::QueryGraspsAction> > ac_grasps_or;
00069         
00070         moveit::planning_interface::MoveGroup group;
00071         
00072         char* GraspTableIniFile;
00073         GraspTable* m_GraspTable;
00074         
00075         bool last_grasp_valid;
00076         std::string last_object_name;
00077         tf::TransformListener tf_listener_;
00078         
00079         std::map<unsigned int,std::string> map_classid_to_classname;
00080         
00081 public:
00082         CobPickPlaceActionServer() : group("arm") {}
00083         ~CobPickPlaceActionServer();
00084         
00085         void initialize();
00086         void run();
00087 
00088         void pick_goal_cb(const cob_pick_place_action::CobPickGoalConstPtr &goal);
00089         void place_goal_cb(const cob_pick_place_action::CobPlaceGoalConstPtr &goal);
00090 
00091         void insertObject(std::string object_name, unsigned int object_class, geometry_msgs::PoseStamped object_pose);
00092         
00093         void fillAllGraspsKIT(unsigned int objectClassId, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00094         void fillSingleGraspKIT(unsigned int objectClassId, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00095         void convertGraspKIT(Grasp* current_grasp, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00096         
00097         void fillGraspsOR(unsigned int objectClassId, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00098         
00099         trajectory_msgs::JointTrajectory MapHandConfiguration(sensor_msgs::JointState table_config);
00100         tf::Transform transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_OBJECT, std::string object_frame_id);
00101         moveit_msgs::GripperTranslation calculateApproachDirection(geometry_msgs::Pose msg_pose_grasp_FOOTPRINT_from_ARM7, geometry_msgs::Pose msg_pose_pre_FOOTPRINT_from_ARM7);
00102 
00103 };
00104 #endif
00105