00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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
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
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;}
00184 bool setup();
00185
00186
00187 void publish_robot_topics_timercb(const ros::TimerEvent &evnt);
00188
00189
00190 void material_load_goalcb();
00191 void material_unload_goalcb();
00192
00193
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
00203 bool run_task(int task_id);
00204 bool run_next_task();
00205
00206
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
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
00227 void ros_status_subs_cb(const industrial_msgs::RobotStatusConstPtr &msg);
00228
00229
00230 bool external_command_cb(mtconnect_cnc_robot_example::Command::Request &req,
00231 mtconnect_cnc_robot_example::Command::Response &res);
00232
00233
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
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_;
00264 int current_task_index_;
00265
00266
00267 int test_task_id_;
00268
00269
00270 std::string task_desc_;
00271 std::map<std::string, trajectory_msgs::JointTrajectoryPtr> joint_paths_;
00272 bool use_task_desc_motion;
00273
00274
00275 MaterialLoadServerPtr material_load_server_ptr_;
00276 MaterialUnloadServerPtr material_unload_server_ptr_;
00277
00278
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
00290 ros::Publisher robot_states_pub_;
00291 ros::Publisher robot_spindle_pub_;
00292
00293
00294 ros::Subscriber robot_status_sub_;
00295
00296
00297 ros::ServiceServer external_command_srv_;
00298
00299
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
00307 mtconnect_msgs::RobotStates robot_state_msg_;
00308 mtconnect_msgs::RobotSpindle robot_spindle_msg_;
00309
00310
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
00316 ros::Timer robot_topics_timer_;
00317
00318
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
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
00344 geometry_msgs::PoseArray cartesian_poses_;
00345 arm_navigation_msgs::MoveArmGoal move_arm_joint_goal_;
00346
00347
00348 control_msgs::FollowJointTrajectoryGoal joint_traj_goal_;
00349
00350 };
00351
00352 } }
00353
00354 #endif