00001 00002 /* 00003 * Copyright 2013 Southwest Research Institute 00004 00005 Licensed under the Apache License, Version 2.0 (the "License"); 00006 you may not use this file except in compliance with the License. 00007 You may obtain a copy of the License at 00008 00009 http://www.apache.org/licenses/LICENSE-2.0 00010 00011 Unless required by applicable law or agreed to in writing, software 00012 distributed under the License is distributed on an "AS IS" BASIS, 00013 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00014 See the License for the specific language governing permissions and 00015 limitations under the License. 00016 */ 00017 00018 #ifndef MOVEARMACTIONCLIENT_H_ 00019 #define MOVEARMACTIONCLIENT_H_ 00020 00021 #include <ros/ros.h> 00022 #include <arm_navigation_msgs/GetPlanningScene.h> 00023 #include <arm_navigation_msgs/SetPlanningSceneDiff.h> 00024 #include <planning_environment/models/collision_models.h> 00025 #include <planning_environment/models/model_utils.h> 00026 #include <actionlib/client/simple_action_client.h> 00027 #include <arm_navigation_msgs/MoveArmAction.h> 00028 #include <arm_navigation_msgs/SimplePoseConstraint.h> 00029 #include <nav_msgs/Path.h> 00030 #include <tf/tf.h> 00031 #include <tf/transform_listener.h> 00032 #include <geometry_msgs/PoseArray.h> 00033 #include <boost/tuple/tuple.hpp> 00034 #include <mtconnect_cnc_robot_example/utilities/utilities.h> 00035 00036 using namespace move_arm_utils; 00037 00038 namespace mtconnect_cnc_robot_example 00039 { 00040 // aliases 00041 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient; 00042 typedef boost::shared_ptr<MoveArmClient> MoveArmClientPtr; 00043 typedef boost::shared_ptr<planning_environment::CollisionModels> CollisionModelsPtr; 00044 typedef boost::shared_ptr<planning_models::KinematicState> KinematicStatePtr; 00045 typedef boost::tuple<std::string,std::string,tf::Transform> CartesianGoal; 00046 class MoveArmActionClient; 00047 typedef boost::shared_ptr<MoveArmActionClient> MoveArmActionClientPtr; 00048 00049 // defaults and constants 00050 static const std::string DEFAULT_PATH_MSG_TOPIC = "move_arm_path"; 00051 static const std::string DEFAULT_MOVE_ARM_ACTION = "move_arm_action"; 00052 static const std::string DEFAULT_PATH_PLANNER = "/ompl_planning/plan_kinematic_path"; 00053 static const std::string DEFAULT_PLANNING_SCENE_DIFF_SERVICE = "/environment_server/set_planning_scene_diff"; 00054 static const std::string DEFAULT_PLANNING_GROUPS_PARAMETER = "/robot_description_planning/groups"; 00055 static const int DEFAULT_PATH_PLANNING_ATTEMPTS = 2; 00056 static const double DEFAULT_PATH_PLANNING_TIME = 5.0f; 00057 static const double DEFAULT_ORIENTATION_TOLERANCE = 0.02f; //radians 00058 static const double DEFAULT_POSITION_TOLERANCE = 0.008f; // meters 00059 static const double DURATION_LOOP_PAUSE = 4.0f; // seconds 00060 static const double DURATION_WAIT_RESULT= 80.0f; 00061 static const double DURATION_TIMER_CYCLE = 2.0f; 00062 static const double DURATION_WAIT_SERVER = 5.0f; 00063 static const int MAX_WAIT_ATTEMPTS = 20; 00064 00065 // ros parameters 00066 static const std::string PARAM_ARM_GROUP = "arm_group"; 00067 static const std::string PARAM_GROUP_KEY = "name"; 00068 static const std::string PARAM_BASE_KEY = "base_link"; 00069 static const std::string PARAM_TIP_KEY = "tip_link"; 00070 00071 class MoveArmActionClient 00072 { 00073 public: 00074 MoveArmActionClient(); 00075 00076 virtual ~MoveArmActionClient(); 00077 00078 virtual void run(); 00079 00080 /* 00081 * Takes and array of poses where each pose is the desired tip link pose described in terms of the arm base 00082 */ 00083 virtual bool moveArm(const geometry_msgs::PoseArray &cartesian_poses,bool wait_for_completion = true); 00084 00085 virtual bool fetchParameters(std::string nameSpace = ""); 00086 00087 virtual void timerCallback(const ros::TimerEvent &evnt); 00088 00089 protected: 00090 00091 virtual bool getArmStartState(std::string group_name, arm_navigation_msgs::RobotState &robot_state); 00092 00093 virtual bool setup(); 00094 00095 bool getArmInfo(const planning_environment::CollisionModels *models, 00096 const std::string &arm_group,std::string &base_link, std::string &tip_link); 00097 00098 bool getPoseInArmSpace(const CartesianGoal &cartesian_goal,geometry_msgs::Pose &base_to_tip_pose); 00099 00100 bool getTrajectoryInArmSpace(const CartesianTrajectory &cartesian_traj,geometry_msgs::PoseArray &base_to_tip_poses); 00101 00102 protected: 00103 00104 // ros action clients 00105 MoveArmClientPtr move_arm_client_ptr_; 00106 00107 // ros service clients 00108 ros::ServiceClient planning_scene_client_; 00109 00110 // ros publishers 00111 ros::Publisher path_pub_; 00112 00113 // ros timers 00114 ros::Timer publish_timer_; 00115 00116 // ros messages 00117 nav_msgs::Path path_msg_; 00118 00119 // tf listener 00120 tf::TransformListener tf_listener_; 00121 00122 // arm info 00123 CollisionModelsPtr collision_models_ptr_; 00124 KinematicStatePtr arm_kinematic_state_ptr_; 00125 std::string base_link_frame_id_; 00126 std::string tip_link_frame_id_; 00127 00128 // ros parameters 00129 CartesianTrajectory cartesian_traj_; 00130 std::string arm_group_; 00131 00132 // move arm request members 00133 arm_navigation_msgs::MoveArmGoal move_arm_goal_; 00134 arm_navigation_msgs::SimplePoseConstraint move_pose_constraint_; 00135 00136 }; 00137 00138 } 00139 #endif /* MOVEARMACTIONCLIENT_H_ */