Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef _DESCEND_RETREAT_PLACE_
00035 #define _DESCEND_RETREAT_PLACE_
00036
00037 #include <ros/ros.h>
00038
00039 #include <tf/transform_listener.h>
00040
00041 #include <object_manipulation_msgs/PlaceLocationResult.h>
00042 #include <object_manipulation_msgs/PlaceGoal.h>
00043
00044 #include <trajectory_msgs/JointTrajectory.h>
00045
00046 #include "object_manipulator/tools/grasp_marker_publisher.h"
00047
00048 namespace object_manipulator {
00049
00052 struct PlaceExecutionInfo {
00053 trajectory_msgs::JointTrajectory descend_trajectory_;
00054 trajectory_msgs::JointTrajectory retreat_trajectory_;
00055 geometry_msgs::PoseStamped gripper_place_pose_;
00056 object_manipulation_msgs::PlaceLocationResult result_;
00057 int marker_id_;
00058 };
00059
00060
00061
00064 class PlaceTester {
00065 protected:
00067 tf::TransformListener listener_;
00068
00070 virtual void testPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00071 const geometry_msgs::PoseStamped &place_location,
00072 PlaceExecutionInfo &execution_info) = 0;
00073
00075 GraspMarkerPublisher *marker_publisher_;
00076
00079 boost::function<void(size_t)> feedback_function_;
00080
00082 boost::function<bool()> interrupt_function_;
00083
00084 public:
00085 PlaceTester() : marker_publisher_(NULL) {}
00086
00088 virtual void testPlaces(const object_manipulation_msgs::PlaceGoal &place_goal,
00089 const std::vector<geometry_msgs::PoseStamped> &place_locations,
00090 std::vector<PlaceExecutionInfo> &execution_info,
00091 bool return_on_first_hit);
00092
00094 void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}
00095
00097 void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}
00098
00100 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00101
00103 object_manipulation_msgs::PlaceLocationResult Result(int result_code, bool continuation)
00104 {
00105 object_manipulation_msgs::PlaceLocationResult result;
00106 result.result_code = result_code;
00107 result.continuation_possible = continuation;
00108 return result;
00109 }
00110 };
00111
00114 class PlacePerformer {
00115 protected:
00117 virtual void performPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00118 const geometry_msgs::PoseStamped &place_location,
00119 PlaceExecutionInfo &execution_info) = 0;
00120
00122 GraspMarkerPublisher *marker_publisher_;
00123
00126 boost::function<void(size_t)> feedback_function_;
00127
00129 boost::function<bool()> interrupt_function_;
00130
00131 public:
00132 PlacePerformer() : marker_publisher_(NULL) {}
00133
00135 virtual void performPlaces(const object_manipulation_msgs::PlaceGoal &place_goal,
00136 const std::vector<geometry_msgs::PoseStamped> &place_locations,
00137 std::vector<PlaceExecutionInfo> &execution_info);
00138
00140 void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}
00141
00143 void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}
00144
00146 void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00147
00149 object_manipulation_msgs::PlaceLocationResult Result(int result_code, bool continuation)
00150 {
00151 object_manipulation_msgs::PlaceLocationResult result;
00152 result.result_code = result_code;
00153 result.continuation_possible = continuation;
00154 return result;
00155 }
00156 };
00157
00158
00159
00160 class StandardPlaceTester : public PlaceTester
00161 {
00162 protected:
00164 virtual void testPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00165 const geometry_msgs::PoseStamped &place_location,
00166 PlaceExecutionInfo &execution_info);
00167
00169 static float EPS;
00170 };
00171
00172
00173
00174 class StandardPlacePerformer : public PlacePerformer
00175 {
00177 object_manipulation_msgs::PlaceLocationResult
00178 retreat(const object_manipulation_msgs::PlaceGoal &place_goal);
00179
00181 bool constraintsUnderstandable(const arm_navigation_msgs::Constraints &constraints);
00182
00184 virtual object_manipulation_msgs::PlaceLocationResult
00185 placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00186 const PlaceExecutionInfo &execution_info);
00187
00189 virtual void performPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00190 const geometry_msgs::PoseStamped &place_location,
00191 PlaceExecutionInfo &execution_info);
00192 };
00193
00195
00198 class ReactivePlacePerformer : public StandardPlacePerformer
00199 {
00200 protected:
00202 virtual object_manipulation_msgs::PlaceLocationResult
00203 placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00204 const PlaceExecutionInfo &execution_info);
00205 };
00206
00207
00208 }
00209
00210
00211 #endif