mtconnect_cnc_robot_example: state

Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Southwest Research Institute
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 #ifndef STATE_MACHINE_H_
00018 #define STATE_MACHINE_H_
00019 
00020 #include <boost/thread/mutex.hpp>
00021 #include <boost/thread/thread.hpp>
00022 #include <boost/assign/list_of.hpp>
00023 #include <boost/assign/list_inserter.hpp>
00024 #include <ros/ros.h>
00025 #include <arm_navigation_msgs/GetPlanningScene.h>
00026 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00027 #include <planning_environment/models/collision_models.h>
00028 #include <planning_environment/models/model_utils.h>
00029 #include <actionlib/client/simple_action_client.h>
00030 #include <arm_navigation_msgs/MoveArmAction.h>
00031 #include <arm_navigation_msgs/SimplePoseConstraint.h>
00032 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00033 #include <nav_msgs/Path.h>
00034 #include <tf/tf.h>
00035 #include <tf/transform_listener.h>
00036 #include <geometry_msgs/PoseArray.h>
00037 #include <boost/tuple/tuple.hpp>
00038 #include <mtconnect_cnc_robot_example/utilities/utilities.h>
00039 #include <mtconnect_cnc_robot_example/move_arm_action_clients/MoveArmActionClient.h>
00040 #include <mtconnect_cnc_robot_example/Command.h>
00041 #include <boost/enable_shared_from_this.hpp>
00042 #include <industrial_msgs/RobotStatus.h>
00043 #include <mtconnect_msgs/CloseChuckAction.h>
00044 #include <mtconnect_msgs/OpenChuckAction.h>
00045 #include <mtconnect_msgs/CloseDoorAction.h>
00046 #include <mtconnect_msgs/OpenDoorAction.h>
00047 #include <mtconnect_msgs/MaterialLoadAction.h>
00048 #include <mtconnect_msgs/MaterialUnloadAction.h>
00049 #include <object_manipulation_msgs/PickupAction.h>
00050 #include <object_manipulation_msgs/PlaceAction.h>
00051 #include <mtconnect_cnc_robot_example/utilities/utilities.h>
00052 #include <mtconnect_cnc_robot_example/state_machine/state_machine_interface.h>
00053 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
00054 #include <actionlib/client/simple_action_client.h>
00055 #include <actionlib/server/simple_action_server.h>
00056 #include <mtconnect_msgs/RobotSpindle.h>
00057 #include <mtconnect_msgs/RobotStates.h>
00058 #include <mtconnect_msgs/SetMTConnectState.h>
00059 #include <control_msgs/FollowJointTrajectoryAction.h>
00060 
00061 // aliases
00062 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00063 typedef actionlib::SimpleActionClient<object_manipulation_msgs::PickupAction> MovePickupClient;
00064 typedef actionlib::SimpleActionClient<object_manipulation_msgs::PlaceAction> MovePlaceClient;
00065 typedef actionlib::SimpleActionClient<mtconnect_msgs::OpenDoorAction> CncOpenDoorClient;
00066 typedef actionlib::SimpleActionClient<mtconnect_msgs::CloseDoorAction> CncCloseDoorClient;
00067 typedef actionlib::SimpleActionClient<mtconnect_msgs::OpenChuckAction> CncOpenChuckClient;
00068 typedef actionlib::SimpleActionClient<mtconnect_msgs::CloseChuckAction> CncCloseChuckClient;
00069 typedef actionlib::SimpleActionServer<mtconnect_msgs::MaterialLoadAction> MaterialLoadServer;
00070 typedef actionlib::SimpleActionServer<mtconnect_msgs::MaterialUnloadAction> MaterialUnloadServer;
00071 typedef actionlib::SimpleActionClient<object_manipulation_msgs::GraspHandPostureExecutionAction> GraspActionClient;
00072 typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> JointTractoryClient;
00073 typedef boost::shared_ptr<MoveArmClient> MoveArmClientPtr;
00074 typedef boost::shared_ptr<MovePickupClient> MovePickupClientPtr;
00075 typedef boost::shared_ptr<MovePlaceClient> MovePlaceClientPtr;
00076 typedef boost::shared_ptr<CncOpenDoorClient> CncOpenDoorClientPtr;
00077 typedef boost::shared_ptr<CncCloseDoorClient> CncCloseDoorClientPtr;
00078 typedef boost::shared_ptr<CncOpenChuckClient> CncOpenChuckClientPtr;
00079 typedef boost::shared_ptr<CncCloseChuckClient> CncCloseChuckClientPtr;
00080 typedef boost::shared_ptr<MaterialLoadServer> MaterialLoadServerPtr;
00081 typedef boost::shared_ptr<MaterialUnloadServer> MaterialUnloadServerPtr;
00082 typedef boost::shared_ptr<GraspActionClient> GraspActionClientPtr;
00083 typedef boost::shared_ptr<JointTractoryClient> JointTractoryClientPtr;
00084 typedef std::vector<int> MaterialHandlingSequence;
00085 
00086 namespace mtconnect_cnc_robot_example { namespace state_machine {
00087 
00088         namespace tasks
00089         {
00090                 enum Task
00091                 {
00092                         NO_TASK = int(0),
00093                         ROBOT_PLACE_WORKPIECE,
00094                         ROBOT_PICKUP_MATERIAL,
00095                         ROBOT_APPROACH_CNC,
00096                         ROBOT_ENTER_CNC,
00097                         ROBOT_MOVE_TO_CHUCK,
00098                         ROBOT_RETREAT_FROM_CHUCK,
00099                         ROBOT_EXIT_CNC,
00100                         ROBOT_MOVE_HOME,
00101                         ROBOT_MOVE_WAIT,
00102                         ROBOT_CARTESIAN_MOVE,
00103                         CNC_OPEN_DOOR,
00104                         CNC_CLOSE_DOOR,
00105                         CNC_OPEN_CHUCK,
00106                         CNC_CLOSE_CHUCK,
00107                         VISE_OPEN,
00108                         VISE_CLOSE,
00109                         GRIPPER_OPEN,
00110                         GRIPPER_CLOSE,
00111                         MATERIAL_LOAD_START,
00112                         MATERIAL_LOAD_END,
00113                         MATERIAL_UNLOAD_START,
00114                         MATERIAL_UNLOAD_END,
00115                         TEST_TASK_START,
00116                         TEST_TASK_END,
00117 
00118                         JM_HOME_TO_READY,
00119                         JM_READY_TO_APPROACH,
00120                         JM_APPROACH_TO_PICK,
00121                         JM_PICK_TO_DOOR,
00122                         JM_DOOR_TO_CHUCK,
00123                         JM_CHUCK_TO_READY,
00124                         JM_READY_TO_DOOR,
00125                         JM_PICK_TO_HOME
00126                 };
00127 
00128                 static std::map<int,std::string> TASK_MAP =
00129                                 boost::assign::map_list_of(NO_TASK,"NO TASK")
00130                 (ROBOT_PLACE_WORKPIECE,"ROBOT_PLACE_WORKPIECE")
00131                 (ROBOT_PICKUP_MATERIAL,"ROBOT_PICKUP_MATERIAL")
00132                 (ROBOT_APPROACH_CNC,"ROBOT_APPROACH_CNC")
00133                 (ROBOT_ENTER_CNC,"ROBOT_ENTER_CNC")
00134                 (ROBOT_MOVE_TO_CHUCK,"ROBOT_MOVE_TO_CHUCK")
00135                 (ROBOT_RETREAT_FROM_CHUCK,"ROBOT_RETREAT_FROM_CHUCK")
00136                 (ROBOT_EXIT_CNC,"ROBOT_EXIT_CNC")
00137                 (ROBOT_MOVE_HOME,"ROBOT_MOVE_HOME")
00138                 (ROBOT_CARTESIAN_MOVE,"ROBOT_CARTESIAN_MOVE")
00139                 (CNC_OPEN_DOOR,"CNC_OPEN_DOOR")
00140                 (CNC_CLOSE_DOOR,"CNC_CLOSE_DOOR")
00141                 (CNC_OPEN_CHUCK,"CNC_OPEN_CHUCK")
00142                 (CNC_CLOSE_CHUCK,"CNC_CLOSE_CHUCK")
00143                 (VISE_OPEN,"VISE_OPEN")
00144                 (VISE_CLOSE,"VISE_CLOSE")
00145                 (GRIPPER_OPEN,"GRIPPER_OPEN")
00146                 (GRIPPER_CLOSE,"GRIPPER_CLOSE")
00147                 (MATERIAL_LOAD_END,"MATERIAL_LOAD_END")
00148                 (MATERIAL_UNLOAD_END,"MATERIAL_UNLOAD_END")
00149                 (JM_HOME_TO_READY, "JM_HOME_TO_READY")
00150                 (JM_READY_TO_APPROACH, "JM_READY_TO_APPROACH")
00151                 (JM_APPROACH_TO_PICK, "JM_APPROACH_TO_PICK")
00152                 (JM_PICK_TO_DOOR, "JM_PICK_TO_DOOR")
00153                 (JM_DOOR_TO_CHUCK, "JM_DOOR_TO_CHUCK")
00154                 (JM_CHUCK_TO_READY, "JM_CHUCK_TO_READY")
00155                 (JM_READY_TO_DOOR, "JM_READY_TO_DOOR")
00156                 (JM_PICK_TO_HOME, "JM_PICK_TO_HOME");
00157         }
00158 
00159         class StateMachine : public StateMachineInterface , public MoveArmActionClient
00160         {
00161         public:
00162 
00163                 // callback types
00164                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const arm_navigation_msgs::MoveArmResultConstPtr& ) > MoveArmDoneCallback;
00165                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  object_manipulation_msgs::PickupResultConstPtr& ) > MovePickupDoneCallback;
00166                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  object_manipulation_msgs::PlaceResultConstPtr& ) > MovePlaceDoneCallback;
00167                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  object_manipulation_msgs::GraspHandPostureExecutionResultConstPtr& ) > GraspActionDoneCallback;
00168                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  mtconnect_msgs::OpenDoorResultConstPtr & ) > OpenDoorDoneCallback;
00169                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  mtconnect_msgs::CloseDoorResultConstPtr & ) > CloseDoorDoneCallback;
00170                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  mtconnect_msgs::OpenChuckResultConstPtr & ) > OpenChuckDoneCallback;
00171                 typedef boost::function<void (const actionlib::SimpleClientGoalState& ,const  mtconnect_msgs::CloseChuckResultConstPtr & ) > CloseChuckDoneCallback;
00172 
00173         public:
00174 
00175                 StateMachine();
00176                 virtual ~StateMachine();
00177 
00178                 virtual void run();
00179 
00180         protected:
00181 
00182                 bool fetch_parameters(std::string name_space = "");
00183                 bool fetchParameters(std::string name_space = ""){return true;} // override inherited from move arm client
00184                 bool setup();
00185 
00186                 // ros timer callbaks
00187                 void publish_robot_topics_timercb(const ros::TimerEvent &evnt);
00188 
00189                 // action goal callbacks
00190                 void material_load_goalcb(/*const MaterialLoadServer::GoalConstPtr &gh*/);
00191                 void material_unload_goalcb(/*const MaterialUnloadServer::GoalConstPtr &gh*/);
00192 
00193                 // wrappers for sending a goal to a move arm server
00194                 bool moveArm(const geometry_msgs::PoseArray &cartesian_poses)
00195                 {
00196                         MoveArmActionClient::moveArm(cartesian_poses,false);
00197                 }
00198 
00199                 bool moveArm(move_arm_utils::JointStateInfo &joint_info);
00200                 bool moveArm(std::string & move_name);
00201 
00202                 // material load/unload specific
00203                 bool run_task(int task_id);
00204                 bool run_next_task();
00205 
00206                 // task test
00207                 void print_task_list()
00208                 {
00209                         using namespace mtconnect_cnc_robot_example::state_machine::tasks;
00210                         std::map<int,std::string>::iterator i;
00211                         std::cout<<"\tTasks List\n";
00212                         for(i = TASK_MAP.begin(); i != TASK_MAP.end(); i++)
00213                         {
00214                                 std::cout<<"\t- ("<<i->first<<") \t"<< i->second<<"\n";
00215                         }
00216                 }
00217 
00218                 // fault related methods
00219                 void cancel_active_material_requests();
00220                 void cancel_active_action_goals();
00221                 void get_param_force_fault_flags();
00222                 bool get_param_fault_on_task_check(int task_id);
00223                 bool all_action_servers_connected();
00224                 bool check_arm_at_position(sensor_msgs::JointState &joints, double tolerance);
00225 
00226                 // subscriber callback
00227                 void ros_status_subs_cb(const industrial_msgs::RobotStatusConstPtr &msg);
00228 
00229                 // service callbacks
00230                 bool external_command_cb(mtconnect_cnc_robot_example::Command::Request &req,
00231                                 mtconnect_cnc_robot_example::Command::Response &res);
00232 
00233                 // transition actions
00234                 virtual bool on_startup();
00235                 virtual bool on_ready();
00236                 virtual bool on_robot_reset();
00237                 virtual bool on_material_load_started();
00238                 virtual bool on_material_load_completed();
00239                 virtual bool on_material_unload_started();
00240                 virtual bool on_material_unload_completed();
00241                 virtual bool on_cnc_reset();
00242                 virtual bool on_peripherals_reset();
00243                 virtual bool on_robot_fault();
00244                 virtual bool on_cnc_fault();
00245                 virtual bool on_gripper_fault();
00246                 virtual bool on_robot_moving();
00247                 virtual bool on_cnc_moving();
00248                 virtual bool on_gripper_moving();
00249                 virtual bool on_test_task_started();
00250                 virtual bool on_test_task_completed();
00251                 virtual bool on_display_tasks()
00252                 {
00253                         print_task_list();
00254                         return true;
00255                 }
00256 
00257         protected:
00258                 // material load/unload task sequence
00259                 std::vector<int> material_load_task_sequence_;
00260                 std::vector<int> jm_material_load_task_sequence_;
00261                 std::vector<int> material_unload_task_sequence_;
00262                 std::vector<int> jm_material_unload_task_sequence_;
00263                 std::vector<int> current_task_sequence_; // will take the value of the load or unload task sequence
00264                 int current_task_index_;
00265 
00266                 // test tasks
00267                 int test_task_id_;
00268 
00269                 // task definitions
00270                 std::string task_desc_;
00271                 std::map<std::string, trajectory_msgs::JointTrajectoryPtr> joint_paths_;
00272                 bool use_task_desc_motion;  //if true, will use task defined motion (instead of planners)
00273 
00274                 // action servers
00275                 MaterialLoadServerPtr material_load_server_ptr_;
00276                 MaterialUnloadServerPtr material_unload_server_ptr_;
00277 
00278                 // action clients
00279                 MovePickupClientPtr move_pickup_client_ptr_;
00280                 MovePlaceClientPtr move_place_client_ptr_;
00281                 CncOpenDoorClientPtr open_door_client_ptr_;
00282                 CncCloseDoorClientPtr close_door_client_ptr_;
00283                 CncOpenChuckClientPtr open_chuck_client_ptr_;
00284                 CncCloseChuckClientPtr close_chuck_client_ptr_;
00285                 GraspActionClientPtr grasp_action_client_ptr_;
00286                 GraspActionClientPtr vise_action_client_ptr_;
00287                 JointTractoryClientPtr joint_traj_client_ptr_;
00288 
00289                 // topic publishers (ros bridge components wait for these topics)
00290                 ros::Publisher robot_states_pub_;
00291                 ros::Publisher robot_spindle_pub_;
00292 
00293                 // topic subscribers
00294                 ros::Subscriber robot_status_sub_;
00295 
00296                 // service servers
00297                 ros::ServiceServer external_command_srv_;
00298 
00299                 // server clients
00300                 ros::ServiceClient material_load_set_state_client_;
00301                 ros::ServiceClient material_unload_set_state_client_;
00302                 ros::ServiceClient trajectory_filter_client_;
00303 
00304 
00305 
00306                 // robot state messages
00307                 mtconnect_msgs::RobotStates robot_state_msg_;
00308                 mtconnect_msgs::RobotSpindle robot_spindle_msg_;
00309 
00310                 // service req/res
00311                 mtconnect_msgs::SetMTConnectState mat_load_set_state_;
00312                 mtconnect_msgs::SetMTConnectState mat_unload_set_state_;
00313                 arm_navigation_msgs::FilterJointTrajectoryWithConstraints trajectory_filter_;
00314 
00315                 // timers
00316                 ros::Timer robot_topics_timer_;
00317 
00318                 // robot moves initialized from parameters
00319                 move_arm_utils::CartesianTrajectory traj_arbitrary_move_;
00320                 move_arm_utils::CartesianTrajectory traj_approach_cnc_;
00321                 move_arm_utils::CartesianTrajectory traj_enter_cnc_;
00322                 move_arm_utils::CartesianTrajectory traj_move_to_chuck_;
00323                 move_arm_utils::CartesianTrajectory traj_retreat_from_chuck_;
00324                 move_arm_utils::CartesianTrajectory traj_exit_cnc_;
00325 
00326                 move_arm_utils::PickupGoalInfo material_load_pickup_goal_;
00327                 move_arm_utils::PlaceGoalInfo material_load_place_goal_;
00328                 move_arm_utils::PickupGoalInfo material_unload_pickup_goal_;
00329                 move_arm_utils::PlaceGoalInfo material_unload_place_goal_;
00330                 move_arm_utils::JointStateInfo joint_home_pos_;
00331                 move_arm_utils::JointStateInfo joint_wait_pos_;
00332 
00333                 // function handle placeholders
00334                 MoveArmDoneCallback move_arm_done_cb_;
00335                 MovePickupDoneCallback move_pickup_done_cb_;
00336                 MovePlaceDoneCallback move_place_done_cb_;
00337                 GraspActionDoneCallback grasp_action_done_cb_;
00338                 OpenDoorDoneCallback open_door_done_cb_;
00339                 CloseDoorDoneCallback close_door_done_cb_;
00340                 OpenChuckDoneCallback open_chuck_done_cb_;
00341                 CloseChuckDoneCallback close_chuck_done_cb_;
00342 
00343                 // move arm members
00344                 geometry_msgs::PoseArray cartesian_poses_;
00345                 arm_navigation_msgs::MoveArmGoal move_arm_joint_goal_;
00346 
00347                 // joint trajectory members
00348                 control_msgs::FollowJointTrajectoryGoal joint_traj_goal_;
00349 
00350         };
00351 
00352 }       }
00353 
00354 #endif /* STATE_MACHINE_H_ */