cob_pick_place_action.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016  
00017  
00018 #ifndef COB_PICK_ACTION_H
00019 #define COB_PICK_ACTION_H
00020 
00021 #include <iostream>
00022 #include <fstream>
00023 #include <ros/ros.h>
00024 #include <ros/package.h>
00025 #include <actionlib/server/simple_action_server.h>
00026 #include <actionlib/client/simple_action_client.h>
00027 #include <actionlib/client/terminal_state.h>
00028 
00029 #include <tf/tf.h>
00030 #include <tf/transform_broadcaster.h>
00031 #include <tf/transform_datatypes.h>
00032 //#include <geometry_msgs/Quaternion.h>
00033 
00034 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
00035 #include <moveit/move_group_interface/move_group.h>
00036 #include <shape_tools/solid_primitive_dims.h>
00037 #include <moveit_msgs/Grasp.h>
00038 
00039 #include <cob_pick_place_action/CobPickAction.h>
00040 #include <cob_pick_place_action/CobPlaceAction.h>
00041 #include <cob_grasp_generation/QueryGraspsAction.h>
00042 #include <GraspTable.h>
00043 
00044 
00045 
00046 class CobPickPlaceActionServer
00047 {
00048 private:
00049         ros::NodeHandle nh_;
00050 
00051         ros::Publisher pub_co; //publisher for collision_objects
00052         ros::Publisher pub_ao; //publisher for attached_collision_objects
00053 
00054         boost::scoped_ptr<actionlib::SimpleActionServer<cob_pick_place_action::CobPickAction> > as_pick;
00055         boost::scoped_ptr<actionlib::SimpleActionServer<cob_pick_place_action::CobPlaceAction> > as_place;
00056 
00057         boost::scoped_ptr<actionlib::SimpleActionClient<cob_grasp_generation::QueryGraspsAction> > ac_grasps_or;
00058 
00059         moveit::planning_interface::MoveGroup group;
00060 
00061         char* GraspTableIniFile;
00062         GraspTable* m_GraspTable;
00063 
00064         bool last_grasp_valid;
00065         std::string last_object_name;
00066         tf::TransformListener tf_listener_;
00067         tf::TransformBroadcaster tf_broadcaster_;
00068 
00069         std::map<unsigned int,std::string> map_classid_to_classname;
00070 
00071 public:
00072         CobPickPlaceActionServer(std::string group_name) : group(group_name) {}
00073         ~CobPickPlaceActionServer();
00074 
00075         void initialize();
00076         void run();
00077 
00078         void pick_goal_cb(const cob_pick_place_action::CobPickGoalConstPtr &goal);
00079         void place_goal_cb(const cob_pick_place_action::CobPlaceGoalConstPtr &goal);
00080 
00081         void insertObject(std::string object_name, unsigned int object_class, geometry_msgs::PoseStamped object_pose);
00082 
00083         void fillAllGraspsKIT(unsigned int objectClassId, std::string gripper_type, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00084         void fillSingleGraspKIT(unsigned int objectClassId, std::string gripper_type, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00085         void convertGraspKIT(Grasp* current_grasp, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00086 
00087         void fillGraspsOR(unsigned int objectClassId, std::string gripper_type, std::string gripper_side, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
00088 
00089         trajectory_msgs::JointTrajectory MapHandConfiguration(sensor_msgs::JointState table_config);
00090         tf::Transform transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_OBJECT, std::string object_frame_id);
00091         moveit_msgs::GripperTranslation calculateApproachDirection(geometry_msgs::Pose msg_pose_grasp_FOOTPRINT_from_ARM7, geometry_msgs::Pose msg_pose_pre_FOOTPRINT_from_ARM7);
00092 
00093 };
00094 #endif
00095 
00096 


cob_pick_place_action
Author(s): Felix Messmer
autogenerated on Thu Jun 6 2019 21:23:15