Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #ifndef ROBOTNAVIGATOR_H_
00013 #define ROBOTNAVIGATOR_H_
00014
00015 #include <ros/ros.h>
00016 #include <algorithm>
00017 #include <trajectory_execution_monitor/joint_state_recorder.h>
00018 #include <trajectory_execution_monitor/follow_joint_trajectory_controller_handler.h>
00019 #include <trajectory_execution_monitor/trajectory_execution_monitor.h>
00020 #include <tabletop_object_detector/TabletopSegmentation.h>
00021 #include <tabletop_object_detector/TabletopObjectRecognition.h>
00022 #include <planning_environment/models/collision_models.h>
00023 #include <arm_navigation_msgs/PlanningScene.h>
00024 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00025 #include <planning_environment/models/model_utils.h>
00026 #include <arm_navigation_msgs/GetMotionPlan.h>
00027 #include <trajectory_msgs/JointTrajectory.h>
00028 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00029 #include <object_manipulator/grasp_execution/grasp_tester_fast.h>
00030 #include <object_manipulator/place_execution/place_tester_fast.h>
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <household_objects_database_msgs/GetModelMesh.h>
00033 #include <household_objects_database_msgs/GetModelDescription.h>
00034 #include <object_manipulation_msgs/GraspPlanning.h>
00035 #include <planning_environment/util/construct_object.h>
00036 #include <object_manipulation_tools/controller_utils/GraspPoseControllerHandler.h>
00037 #include <object_manipulation_tools/manipulation_utils/PlaceSequenceValidator.h>
00038 #include <object_manipulation_tools/manipulation_utils/GraspSequenceValidator.h>
00039 #include <tf/transform_listener.h>
00040
00041 typedef actionlib::SimpleActionClient<object_manipulation_msgs::GraspHandPostureExecutionAction> GraspActionServerClient;
00042
00043 typedef boost::shared_ptr<GraspSequenceValidator> GraspTesterPtr;
00044 typedef boost::shared_ptr<object_manipulator::GraspTesterFast> GraspTesterFastPtr;
00045 typedef boost::shared_ptr<PlaceSequenceValidator> PlaceSequencePtr;
00046
00047 using namespace trajectory_execution_monitor;
00048
00049
00050 namespace RobotNavigatorParameters
00051 {
00052 static const std::string DEFAULT_ARM_GROUP = "sia20d_arm";
00053 static const std::string DEFAULT_GRIPPER_GROUP = "end_effector";
00054 static const std::string DEFAULT_WRIST_LINK = "link_t";
00055 static const std::string DEFAULT_GRIPPER_LINK = "palm";
00056 static const std::string DEFAULT_GRASP_ACTION_SERVICE = "/grasp_execution_action";
00057 static const std::string DEFAULT_TRAJECTORY_ACTION_SERVICE = "/joint_trajectory_action";
00058 static const std::string DEFAULT_PATH_PLANNER_SERVICE = "/ompl_planning/plan_kinematic_path";
00059 static const std::string DEFAULT_SEGMENTATION_SERVICE = "/tabletop_segmentation";
00060 static const std::string DEFAULT_RECOGNITION_SERVICE = "/tabletop_object_recognition";
00061 static const std::string DEFAULT_TRAJECTORY_FILTER_SERVICE = "/trajectory_filter_server/filter_trajectory_with_constraints";
00062 static const std::string DEFAULT_GRASP_PLANNING_SERVICE = "/plan_point_cluster_grasp";
00063 static const std::string DEFAULT_MESH_DATABASE_SERVICE = "/objects_database_node/get_model_mesh";
00064 static const std::string DEFAULT_MODEL_DATABASE_SERVICE = "/objects_database_node/get_model_description";
00065 static const std::string DEFAULT_PLANNING_SCENE_SERVICE = "/environment_server/set_planning_scene_diff";
00066 static const std::string DEFAULT_IK_PLUGING = "SIA20D_Mesh_manipulator_kinematics/IKFastKinematicsPlugin";
00067 static const std::string DEFAULT_JOINT_STATES_TOPIC = "/joint_states";
00068 static const double DF_PICK_APPROACH_DISTANCE = 0.1f;
00069 static const double DF_PLACE_APPROACH_DISTANCE = 0.1f;
00070 static const double DF_PLACE_RETREAT_DISTANCE = 0.1f;
00071
00072
00073 static const std::string PARAM_NAME_ARM_GROUP = "arm_group";
00074 static const std::string PARAM_NAME_GRIPPER_GROUP = "gripper_group";
00075 static const std::string PARAM_NAME_WRIST_LINK = "wrist_link";
00076 static const std::string PARAM_NAME_GRIPPER_LINK = "gripper_link";
00077 static const std::string PARAM_NAME_GRASP_ACTION_SERVICE = "grasp_action_service_name";
00078 static const std::string PARAM_NAME_TRAJECTORY_ACTION_SERVICE = "joint_trajectory_service_name";
00079 static const std::string PARAM_NAME_PATH_PLANNER_SERVICE = "planner_service_name";
00080 static const std::string PARAM_NAME_TRAJECTORY_FILTER_SERVICE = "trajectory_filter_service_name";
00081 static const std::string PARAM_NAME_SEGMENTATION_SERVICE = "segmentation_service_name";
00082 static const std::string PARAM_NAME_RECOGNITION_SERVICE = "recognition_service_name";
00083 static const std::string PARAM_NAME_GRASP_PLANNING_SERVICE = "grasp_planning_service_name";
00084 static const std::string PARAM_NAME_MESH_DATABASE_SERVICE = "mesh_database_service_name";
00085 static const std::string PARAM_NAME_MODEL_DATABASE_SERVICE = "model_database_service_name";
00086 static const std::string PARAM_NAME_PLANNING_SCENE_SERVICE = "planning_scene_service_name";
00087 static const std::string PARAM_NAME_IK_PLUGING = "arm_inverse_kinematics_plugin";
00088 static const std::string PARAM_NAME_JOINT_STATES_TOPIC = "joint_state_topic";
00089 static const std::string PARAM_PICK_APPROACH_DISTANCE = "/pick_approach_distance";
00090 static const std::string PARAM_PLACE_APPROACH_DISTANCE = "/place_approach_distance";
00091 static const std::string PARAM_PLACE_RETREAT_DISTANCE = "/place_retreat_distance";
00092
00093 }
00094
00095
00096 class RobotNavigator
00097 {
00098 public:
00099
00100 typedef boost::shared_ptr<RobotNavigator> Ptr;
00101
00102 public:
00103 RobotNavigator();
00104 virtual ~RobotNavigator();
00105
00106
00107 virtual void run();
00108
00109 static std::string NODE_NAME;
00110 static std::string NAVIGATOR_NAMESPACE;
00111 static std::string MARKER_ARM_LINK;
00112 static std::string MARKER_ATTACHED_OBJECT;
00113 static std::string VISUALIZATION_TOPIC;
00114
00115
00116 static const tf::Transform PLACE_RECTIFICATION_TF;
00117
00118 protected:
00119
00120 virtual void setup();
00121
00122
00123 virtual void fetchParameters(std::string nameSpace = "");
00124
00125
00126
00127
00128
00129
00130
00131
00132 virtual bool performSegmentation();
00133 virtual bool performRecognition();
00134 virtual bool performPickGraspPlanning();
00135 virtual bool performPlaceGraspPlanning();
00136 virtual bool performGraspPlanning();
00137 virtual bool performTrajectoryFiltering(const std::string& group_name,trajectory_msgs::JointTrajectory& jt);
00138
00139
00140 virtual bool createCandidateGoalPoses(std::vector<geometry_msgs::PoseStamped> &placePoses) = 0;
00141
00142
00143 bool getMeshFromDatabasePose(const household_objects_database_msgs::DatabaseModelPose &model_pose,
00144 arm_navigation_msgs::CollisionObject& obj,const geometry_msgs::PoseStamped& pose);
00145
00146
00147 virtual bool moveArmToSide();
00148 virtual bool moveArm(const std::string& group_name,const std::vector<double>& joint_positions);
00149 virtual bool moveArmThroughPickSequence();
00150 virtual bool moveArmThroughPlaceSequence();
00151
00152
00153 virtual void callbackPublishMarkers(const ros::TimerEvent &evnt);
00154 virtual bool trajectoryFinishedCallback(bool storeLastTraj,TrajectoryExecutionDataVector tedv);
00155
00156
00157 void attachCollisionObjectCallback(const std::string& group_name);
00158 void detachCollisionObjectCallback(const std::string& group_name);
00159 bool attachCollisionObject(const std::string& group_name,const std::string& collision_object_name,
00160 const object_manipulation_msgs::Grasp& grasp);
00161 bool detachCollisionObject(const std::string& group_name,const geometry_msgs::PoseStamped& place_pose,
00162 const object_manipulation_msgs::Grasp& grasp);
00163 void revertPlanningScene();
00164 void updateCurrentJointStateToLastTrajectoryPoint(const trajectory_msgs::JointTrajectory& traj);
00165
00166
00167 bool updateChangesToPlanningScene();
00168
00169
00170
00171 void addDetectedTableToLocalPlanningScene(const tabletop_object_detector::Table &table);
00172 void addDetectedObjectToLocalPlanningScene(arm_navigation_msgs::CollisionObject &obj);
00173 void addDetectedObjectToLocalPlanningScene(const household_objects_database_msgs::DatabaseModelPoseList& model);
00174
00175
00176 virtual bool attemptGraspSequence(const std::string& group_name,const object_manipulator::GraspExecutionInfo& gei,bool performRecoveryMove = true);
00177 virtual bool attemptPlaceSequence(const std::string& group_name,const object_manipulator::PlaceExecutionInfo& pei);
00178
00179
00180 void startCycleTimer();
00181 void printTiming();
00182
00183
00184 static std::string makeCollisionObjectNameFromModelId(unsigned int model_id);
00185 const arm_navigation_msgs::CollisionObject* getCollisionObject(unsigned int model_id);
00186
00187
00188 virtual std::vector<std::string> getJointNames(const std::string& group);
00189 virtual trajectory_msgs::JointTrajectory getGripperTrajectory(int graspMove = object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP);
00190
00191
00192 void printJointTrajectory(const trajectory_msgs::JointTrajectory &jt);
00193 bool validateJointTrajectory(trajectory_msgs::JointTrajectory &jt);
00194
00195
00196 void collisionObjToMarker(const arm_navigation_msgs::CollisionObject &obj, visualization_msgs::Marker &marker);
00197 void addMarker(std::string name,visualization_msgs::Marker &marker);
00198 void addMarker(std::string name,visualization_msgs::MarkerArray &marker);
00199 bool hasMarker(std::string);
00200 visualization_msgs::Marker& getMarker(std::string name);
00201
00202
00203
00204
00205 virtual bool createPickMoveSequence(const object_manipulation_msgs::PickupGoal &pickupGoal,
00206 const std::vector<object_manipulation_msgs::Grasp> &grasps_candidates,
00207 std::vector<object_manipulator::GraspExecutionInfo> &grasp_sequence,
00208 std::vector<object_manipulation_msgs::Grasp> &valid_grasps);
00209 virtual bool createPlaceMoveSequence(const object_manipulation_msgs::PlaceGoal &placeGoal,
00210 const std::vector<geometry_msgs::PoseStamped> &place_poses,
00211 std::vector<object_manipulator::PlaceExecutionInfo> &place_sequence);
00212
00213
00214 protected:
00215
00216
00217 planning_environment::CollisionModels cm_;
00218
00219
00220 unsigned int current_planning_scene_id_;
00221 unsigned int last_mpr_id_;
00222 unsigned int max_mpr_id_;
00223 unsigned int max_trajectory_id_;
00224
00225 TrajectoryExecutionDataVector last_trajectory_execution_data_vector_;
00226
00227
00228 ros::ServiceClient seg_srv_;
00229 ros::ServiceClient rec_srv_;
00230 ros::ServiceClient planning_service_client_;
00231 ros::ServiceClient trajectory_filter_service_client_;
00232 ros::ServiceClient object_database_model_mesh_client_;
00233 ros::ServiceClient grasp_planning_client;
00234 ros::ServiceClient object_database_model_description_client_;
00235 ros::ServiceClient set_planning_scene_diff_client_;
00236
00237
00238 boost::shared_ptr<TrajectoryRecorder> joint_state_recorder_;
00239 boost::shared_ptr<TrajectoryControllerHandler> arm_controller_handler_;
00240 boost::shared_ptr<TrajectoryControllerHandler> gripper_controller_handler_;
00241
00242 trajectory_execution_monitor::TrajectoryExecutionMonitor trajectory_execution_monitor_;
00243 boost::function<bool(TrajectoryExecutionDataVector)> trajectories_finished_function_;
00244 boost::function<bool(TrajectoryExecutionDataVector)> grasp_action_finished_function_;
00245
00246
00247
00248
00249 boost::shared_ptr<GraspActionServerClient> grasp_exec_action_client_;
00250
00251
00252
00253
00254 boost::shared_ptr<GraspSequenceValidator> grasp_tester_;
00255 boost::shared_ptr<PlaceSequenceValidator> place_tester_;
00256
00257 arm_navigation_msgs::PlanningScene current_planning_scene_;
00258 arm_navigation_msgs::PlanningScene planning_scene_diff_;
00259 planning_models::KinematicState* current_robot_state_;
00260
00261 boost::condition_variable execution_completed_;
00262 boost::mutex execution_mutex_;
00263 bool trajectories_succeeded_;
00264
00265
00266 tabletop_object_detector::TabletopSegmentation::Response segmentation_results_;
00267 std::vector<sensor_msgs::PointCloud> segmented_clusters_;
00268 arm_navigation_msgs::CollisionObject table_;
00269
00270
00271 std::map<std::string, geometry_msgs::PoseStamped> recognized_obj_pose_map_;
00272 std::vector<household_objects_database_msgs::DatabaseModelPoseList> recognized_models_;
00273 household_objects_database_msgs::GetModelDescription::Response recognized_model_description_;
00274
00275
00276 std::map<std::string, bool> object_in_hand_map_;
00277 std::map<std::string, std::string> current_grasped_object_name_;
00278 std::map<std::string, object_manipulation_msgs::Grasp> current_grasp_map_;
00279 object_manipulation_msgs::PickupGoal grasp_pickup_goal_;
00280 object_manipulation_msgs::PlaceGoal grasp_place_goal_;
00281 std::vector<object_manipulation_msgs::Grasp> grasp_candidates_;
00282
00283
00284 std::vector<object_manipulator::GraspExecutionInfo> grasp_pick_sequence_;
00285 std::vector<object_manipulator::PlaceExecutionInfo> grasp_place_sequence_;
00286
00287
00288 geometry_msgs::PoseStamped current_place_location_;
00289
00290
00291 ros::WallTime cycle_start_time_;
00292 ros::WallDuration execution_duration_;
00293 ros::WallDuration perception_duration_;
00294 ros::WallDuration planning_scene_duration_;
00295 ros::WallDuration motion_planning_duration_;
00296 ros::WallDuration trajectory_filtering_duration_;
00297 ros::WallDuration grasp_planning_duration_;
00298
00299
00300 ros::Publisher attached_object_publisher_;
00301
00302
00303 ros::Publisher marker_array_publisher_;
00304 ros::Publisher marker_publisher_;
00305
00306
00307 ros::Timer marker_pub_timer_;
00308 std::map<std::string,visualization_msgs::Marker> marker_map_;
00309
00310 tf::TransformListener _TfListener;
00311
00312
00313
00314 std::string arm_group_name_;
00315 std::string gripper_group_name_;
00316
00317
00318 std::string wrist_link_name_;
00319 std::string gripper_link_name_;
00320
00321
00322 std::string grasp_action_service_;
00323 std::string trajectory_action_service_;
00324
00325
00326 std::string path_planner_service_;
00327 std::string trajectory_filter_service_;
00328 std::string segmentation_service_;
00329 std::string recognition_service_;
00330 std::string grasp_planning_service_;
00331 std::string mesh_database_service_;
00332 std::string model_database_service_;
00333 std::string planning_scene_service_;
00334 std::string joint_states_topic_;
00335
00336
00337 std::string ik_plugin_name_;
00338
00339
00340 double pick_approach_distance_;
00341 double place_approach_distance_;
00342 double place_retreat_distance_;
00343
00344
00345 };
00346
00347 #endif