RobotNavigator.h
Go to the documentation of this file.
00001 /*
00002  * RobotNavigator.h
00003  *
00004  *  Created on: Oct 18, 2012
00005  *      Author: jnicho
00006  *  Description:
00007  *      This class provides the basic functionality for robot arm navigation tasks.  These task include calling various
00008  *      services through service clients for each step in the navigation sequence such as segmentation, recognition, grasp
00009  *      planning, path planning, trajectory filtering, and grasp and trajectory executions.
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 //typedef boost::shared_ptr<object_manipulator::GraspTesterFast> GraspTesterPtr;
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 // ros param default values
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         // ros param names
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         // demo start
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         // others
00116         static const tf::Transform PLACE_RECTIFICATION_TF; // used to orient the tcp's z vector in the normal direction for all place moves
00117 
00118 protected:
00119         // setup
00120         virtual void setup();
00121 
00122         // ros parameters
00123         virtual void fetchParameters(std::string nameSpace = "");
00124 
00125         /* Service methods
00126          * these methods perform the following:
00127          *      -  Build request objects,
00128          *      -  Check results
00129          *      -  Store results needed for subsequent steps in the manipulation
00130          *      -  Perform additional post processing stuff such as updating the planning scene or publish data
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         // goal position: this should be implemented by each specific specialization of the class
00140         virtual bool createCandidateGoalPoses(std::vector<geometry_msgs::PoseStamped> &placePoses) = 0;
00141 
00142         // database comm
00143         bool getMeshFromDatabasePose(const household_objects_database_msgs::DatabaseModelPose &model_pose,
00144                         arm_navigation_msgs::CollisionObject& obj,const geometry_msgs::PoseStamped& pose);
00145 
00146         // move arm methods
00147         virtual bool moveArmToSide(); // it is preferable to implement this in order to tell the robot arm where "side" is
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         // callbacks
00153         virtual void callbackPublishMarkers(const ros::TimerEvent &evnt);
00154         virtual bool trajectoryFinishedCallback(bool storeLastTraj,TrajectoryExecutionDataVector tedv);
00155 
00156         // planning scene
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         // adds or removes objects into planning scene through a service call, service returns a copy of the scene in its current state;
00167         bool updateChangesToPlanningScene();
00168 
00169         // add objects to local copy of the planning scene, however the planning scene service needs to be called in order to push
00170         // any changes into the actual planning scene
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         // grasp execution
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         // demo monitoring
00180         void startCycleTimer();
00181         void printTiming();
00182 
00183         // general utilities
00184         static std::string makeCollisionObjectNameFromModelId(unsigned int model_id);
00185         const arm_navigation_msgs::CollisionObject* getCollisionObject(unsigned int model_id);
00186 
00187         // trajectory utilities
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         //virtual trajectory_msgs::JointTrajectory getGripperTrajectory(const std::string& arm_name,bool open);
00191 
00192         void printJointTrajectory(const trajectory_msgs::JointTrajectory &jt);
00193         bool validateJointTrajectory(trajectory_msgs::JointTrajectory &jt); // checks for null arrays and fill with zeros as needed
00194 
00195         // visual utilities
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         /* move sequence creation methods
00203          * These methods generate all the necessary move steps corresponding to each manipulation sequence
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         // planning environemt
00217         planning_environment::CollisionModels cm_;
00218 
00219         // ids
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         // service clients
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         // trajectory handlers and recorders
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         // action services
00247         // will use grasp execution client to request pre-grasp action since the default gripper controller handler
00248         // ignores this step.
00249         boost::shared_ptr<GraspActionServerClient> grasp_exec_action_client_;
00250 
00251         // grasp move sequence generators
00252         //object_manipulator::PlaceTesterFast* place_tester_;
00253         //boost::shared_ptr<object_manipulator::GraspTesterFast> grasp_tester_;
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         // segmentation results
00266         tabletop_object_detector::TabletopSegmentation::Response segmentation_results_;
00267         std::vector<sensor_msgs::PointCloud> segmented_clusters_;
00268         arm_navigation_msgs::CollisionObject table_;
00269 
00270         // recognition results
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         // grasp planning results
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         // pick/place move execution data
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         // timing results
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         // object publisher
00300         ros::Publisher attached_object_publisher_;
00301 
00302         // marker publishers
00303         ros::Publisher marker_array_publisher_;
00304         ros::Publisher marker_publisher_;
00305 
00306         // timer to publish markers periodically
00307         ros::Timer marker_pub_timer_;
00308         std::map<std::string,visualization_msgs::Marker> marker_map_;
00309 
00310         tf::TransformListener _TfListener;
00311 
00312         // ################################## ros parameters ##################################
00313         // group names
00314         std::string arm_group_name_;
00315         std::string gripper_group_name_;
00316 
00317         // link/gripper names
00318         std::string wrist_link_name_;
00319         std::string gripper_link_name_;
00320 
00321         // action services
00322         std::string grasp_action_service_;// = "/grasp_execution_action";
00323         std::string trajectory_action_service_;// = "/joint_trajectory_action";
00324 
00325         // topic names
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         // plugins
00337         std::string ik_plugin_name_;
00338 
00339         // pick/place
00340         double pick_approach_distance_;
00341         double place_approach_distance_;
00342         double place_retreat_distance_;
00343 
00344         // ################################## end of ros parameters ##################################
00345 };
00346 
00347 #endif /* ROBOTNAVIGATOR_H_ */


object_manipulation_tools
Author(s): Jnicho
autogenerated on Mon Oct 6 2014 00:56:17