Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <mtconnect_cnc_robot_example/move_arm_action_clients/MoveArmActionClient.h>
00019 #include <arm_navigation_msgs/utils.h>
00020
00021 using namespace mtconnect_cnc_robot_example;
00022
00023 MoveArmActionClient::MoveArmActionClient()
00024 :
00025 move_arm_client_ptr_()
00026 {
00027
00028 }
00029
00030 MoveArmActionClient::~MoveArmActionClient()
00031 {
00032
00033 }
00034
00035 void MoveArmActionClient::run()
00036 {
00037 using namespace arm_navigation_msgs;
00038
00039 if(!setup())
00040 {
00041 return;
00042 }
00043
00044 ros::AsyncSpinner spinner(2);
00045 spinner.start();
00046
00047
00048 geometry_msgs::PoseArray cartesian_poses;
00049 if(!getTrajectoryInArmSpace(cartesian_traj_,cartesian_poses))
00050 {
00051 ROS_ERROR_STREAM(ros::this_node::getName()<<": Cartesian Trajectory could not be transformed to arm space");
00052 return;
00053 }
00054
00055 while(ros::ok())
00056 {
00057 if(!moveArm(cartesian_poses))
00058 {
00059 ros::Duration(DURATION_LOOP_PAUSE).sleep();
00060 }
00061 }
00062 }
00063
00064
00065
00066
00067 bool MoveArmActionClient::moveArm(const geometry_msgs::PoseArray &cartesian_poses, bool wait_for_completion)
00068 {
00069 using namespace arm_navigation_msgs;
00070
00071 ROS_INFO_STREAM(ros::this_node::getName()<<": Sending Cartesian Goal with "<<cartesian_poses.poses.size()<<" via points");
00072 bool success = !cartesian_poses.poses.empty();
00073 std::vector<geometry_msgs::Pose>::const_iterator i;
00074 for(i = cartesian_poses.poses.begin(); i != cartesian_poses.poses.end();i++)
00075 {
00076
00077 move_arm_goal_.motion_plan_request.goal_constraints.position_constraints.clear();
00078 move_arm_goal_.motion_plan_request.goal_constraints.orientation_constraints.clear();
00079 move_arm_goal_.motion_plan_request.goal_constraints.joint_constraints.clear();
00080 move_arm_goal_.motion_plan_request.goal_constraints.visibility_constraints.clear();
00081
00082
00083 move_pose_constraint_.pose = *i;
00084 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(move_pose_constraint_,move_arm_goal_);
00085
00086
00087 move_arm_client_ptr_->sendGoal(move_arm_goal_);
00088
00089 if(!wait_for_completion)
00090 {
00091 return success;
00092 }
00093
00094 success = move_arm_client_ptr_->waitForResult(ros::Duration(DURATION_WAIT_RESULT));
00095 if(success)
00096 {
00097 success = actionlib::SimpleClientGoalState::SUCCEEDED == move_arm_client_ptr_->getState().state_;
00098 if(success)
00099 {
00100 ROS_INFO_STREAM(ros::this_node::getName()<<": Goal Achieved");
00101 }
00102 else
00103 {
00104 ROS_ERROR_STREAM(ros::this_node::getName()<<": Goal Rejected with error flag: "
00105 <<(unsigned int)move_arm_client_ptr_->getState().state_);
00106 break;
00107 }
00108 }
00109 else
00110 {
00111 move_arm_client_ptr_->cancelGoal();
00112 ROS_ERROR_STREAM(ros::this_node::getName()<<": Goal Failed with error flag: "
00113 <<(unsigned int)move_arm_client_ptr_->getState().state_);
00114 break;
00115 }
00116 }
00117
00118 return success;
00119 }
00120
00121 bool MoveArmActionClient::fetchParameters(std::string nameSpace)
00122 {
00123 ros::NodeHandle nh("~");
00124
00125 bool success = nh.getParam(PARAM_ARM_GROUP,arm_group_) && cartesian_traj_.fetchParameters();
00126 return success;
00127 }
00128
00129 void MoveArmActionClient::timerCallback(const ros::TimerEvent &evnt)
00130 {
00131
00132 cartesian_traj_.getMarker(path_msg_);
00133 if(!path_msg_.poses.empty())
00134 {
00135 path_pub_.publish(path_msg_);
00136 }
00137 }
00138
00139 bool MoveArmActionClient::getArmStartState(std::string group_name, arm_navigation_msgs::RobotState &robot_state)
00140 {
00141 using namespace arm_navigation_msgs;
00142
00143
00144 SetPlanningSceneDiff::Request planning_scene_req;
00145 SetPlanningSceneDiff::Response planning_scene_res;
00146 if(planning_scene_client_.call(planning_scene_req,planning_scene_res))
00147 {
00148 ROS_INFO_STREAM(ros::this_node::getName()<<": call to set planning scene succeeded");
00149 }
00150 else
00151 {
00152 ROS_ERROR_STREAM(ros::this_node::getName()<<": call to set planning scene failed");
00153 return false;
00154 }
00155
00156
00157 planning_models::KinematicState *st = collision_models_ptr_->setPlanningScene(planning_scene_res.planning_scene);
00158 if(st == NULL)
00159 {
00160 ROS_ERROR_STREAM(ros::this_node::getName()<<": Kinematic State for arm could not be retrieved from planning scene");
00161 return false;
00162 }
00163
00164 planning_environment::convertKinematicStateToRobotState(*st,
00165 ros::Time::now(),
00166 collision_models_ptr_->getWorldFrameId(),
00167 robot_state);
00168
00169 collision_models_ptr_->revertPlanningScene(st);
00170 return true;
00171 }
00172
00173 bool MoveArmActionClient::setup()
00174 {
00175 ros::NodeHandle nh;
00176 bool success = true;
00177
00178 if(!fetchParameters())
00179 {
00180 return false;
00181 }
00182
00183
00184 move_arm_client_ptr_ = MoveArmClientPtr(new MoveArmClient(DEFAULT_MOVE_ARM_ACTION,true));
00185 unsigned int attempts = 0;
00186 while(attempts++ < MAX_WAIT_ATTEMPTS)
00187 {
00188 ROS_WARN_STREAM(ros::this_node::getName()<<": waiting for "<<DEFAULT_MOVE_ARM_ACTION<<" server");
00189 success = move_arm_client_ptr_->waitForServer(ros::Duration(DURATION_WAIT_SERVER));
00190 if(success)
00191 {
00192 ROS_INFO_STREAM(ros::this_node::getName()<<": Found "<<DEFAULT_MOVE_ARM_ACTION<<" server");
00193 break;
00194 }
00195 }
00196
00197
00198 planning_scene_client_ = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(DEFAULT_PLANNING_SCENE_DIFF_SERVICE);
00199
00200
00201 path_pub_ = nh.advertise<nav_msgs::Path>(DEFAULT_PATH_MSG_TOPIC,1);
00202
00203
00204 publish_timer_ = nh.createTimer(ros::Duration(DURATION_TIMER_CYCLE),&MoveArmActionClient::timerCallback,this);
00205
00206
00207 collision_models_ptr_ = CollisionModelsPtr(new planning_environment::CollisionModels("robot_description"));
00208 getArmInfo(collision_models_ptr_.get(),arm_group_,base_link_frame_id_,tip_link_frame_id_);
00209
00210
00211 move_arm_goal_.motion_plan_request.group_name = arm_group_;
00212 move_arm_goal_.motion_plan_request.num_planning_attempts = DEFAULT_PATH_PLANNING_ATTEMPTS;
00213 move_arm_goal_.planner_service_name = DEFAULT_PATH_PLANNER;
00214 move_arm_goal_.motion_plan_request.planner_id = "";
00215 move_arm_goal_.motion_plan_request.allowed_planning_time = ros::Duration(DEFAULT_PATH_PLANNING_TIME);
00216
00217 move_pose_constraint_.header.frame_id = base_link_frame_id_;
00218 move_pose_constraint_.link_name = tip_link_frame_id_;
00219 move_pose_constraint_.absolute_position_tolerance.x = DEFAULT_POSITION_TOLERANCE;
00220 move_pose_constraint_.absolute_position_tolerance.y = DEFAULT_POSITION_TOLERANCE;
00221 move_pose_constraint_.absolute_position_tolerance.z = DEFAULT_POSITION_TOLERANCE;
00222 move_pose_constraint_.absolute_roll_tolerance = DEFAULT_ORIENTATION_TOLERANCE;
00223 move_pose_constraint_.absolute_pitch_tolerance = DEFAULT_ORIENTATION_TOLERANCE;
00224 move_pose_constraint_.absolute_yaw_tolerance = DEFAULT_ORIENTATION_TOLERANCE;
00225
00226 return success;
00227 }
00228
00229 bool MoveArmActionClient::getArmInfo(const planning_environment::CollisionModels *models,
00230 const std::string &arm_group,std::string &base_link, std::string &tip_link)
00231 {
00232 ros::NodeHandle nh;
00233 XmlRpc::XmlRpcValue group_param;
00234 std::stringstream ss;
00235 bool success = false;
00236
00237
00238 if(nh.getParam(DEFAULT_PLANNING_GROUPS_PARAMETER,group_param))
00239 {
00240 for(int i = 0; i < group_param.size(); i++)
00241 {
00242 XmlRpc::XmlRpcValue &element = group_param[i];
00243 if((element.getType() == XmlRpc::XmlRpcValue::TypeStruct) &&
00244 element.hasMember(PARAM_GROUP_KEY) &&
00245 (arm_group.compare(static_cast<std::string>(element[PARAM_GROUP_KEY])) == 0) &&
00246 element.hasMember(PARAM_BASE_KEY) &&
00247 element.hasMember(PARAM_TIP_KEY))
00248 {
00249 base_link = static_cast<std::string>(element[PARAM_BASE_KEY]);
00250 tip_link = static_cast<std::string>(element[PARAM_TIP_KEY]);
00251 success = true;
00252 break;
00253 }
00254 }
00255 }
00256 else
00257 {
00258 ROS_ERROR_STREAM("Did not find parameter '"<<DEFAULT_PLANNING_GROUPS_PARAMETER<<"', could not get arm information");
00259 return false;
00260 }
00261
00262
00263 const planning_models::KinematicModel::JointModelGroup *joint_model_group =
00264 models->getKinematicModel()->getModelGroup(arm_group);
00265
00266
00267 const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
00268 const std::vector<std::string> &link_names = joint_model_group->getGroupLinkNames();
00269 std::vector<std::string>::const_iterator i;
00270 ss<<"\nArm Base Link Frame: "<<base_link;
00271 ss<<"\nArm Tip Link Frame: "<<tip_link;
00272 ss<<"\nJoint Names in group '"<<arm_group<<"'";
00273 for(i = joint_names.begin(); i != joint_names.end() ; i++)
00274 {
00275 ss<<"\n\t"<<*i;
00276 }
00277
00278 ss<<"\nLink Names in group '"<<arm_group<<"'";
00279 for(i = link_names.begin(); i != link_names.end() ; i++)
00280 {
00281 ss<<"\n\t"<<*i;
00282 }
00283 ROS_INFO_STREAM(ros::this_node::getName()<<ss.str());
00284
00285 return success;
00286 }
00287
00288 bool MoveArmActionClient::getPoseInArmSpace(
00289 const CartesianGoal &cartesian_goal,geometry_msgs::Pose &base_to_tip_pose)
00290 {
00291
00292 static tf::StampedTransform base_to_parent_tf;
00293 static tf::StampedTransform tcp_to_tip_link_tf;
00294 tf::Transform base_to_tip_tf;
00295
00296 const std::string &parent_frame = boost::tuples::get<0>(cartesian_goal);
00297 const std::string &tcp_frame = boost::tuples::get<1>(cartesian_goal);
00298 const tf::Transform &parent_to_tcp_tf = boost::tuples::get<2>(cartesian_goal);
00299
00300
00301 try
00302 {
00303 tf_listener_.lookupTransform(base_link_frame_id_,parent_frame,ros::Time(0),base_to_parent_tf);
00304 tf_listener_.lookupTransform(tcp_frame,tip_link_frame_id_,ros::Time(0),tcp_to_tip_link_tf);
00305 }
00306 catch(tf::LookupException &e)
00307 {
00308 ROS_ERROR_STREAM(ros::this_node::getName()<<": Unable to lookup transforms");
00309 return false;
00310 }
00311
00312 base_to_tip_tf = ((tf::Transform)base_to_parent_tf) * parent_to_tcp_tf * ((tf::Transform)tcp_to_tip_link_tf);
00313 tf::poseTFToMsg(base_to_tip_tf,base_to_tip_pose);
00314
00315 return true;
00316 }
00317
00318 bool MoveArmActionClient::getTrajectoryInArmSpace(
00319 const CartesianTrajectory &cartesian_traj,geometry_msgs::PoseArray &base_to_tip_poses)
00320 {
00321
00322 static tf::StampedTransform base_to_parent_tf;
00323 static tf::StampedTransform tcp_to_tip_link_tf;
00324 tf::Transform base_to_tip_tf;
00325 geometry_msgs::Pose base_to_tip_pose;
00326
00327
00328 try
00329 {
00330 tf_listener_.lookupTransform(base_link_frame_id_,cartesian_traj.frame_id_,ros::Time(0),base_to_parent_tf);
00331 tf_listener_.lookupTransform(cartesian_traj.link_name_,tip_link_frame_id_,ros::Time(0),tcp_to_tip_link_tf);
00332 }
00333 catch(tf::LookupException &e)
00334 {
00335 ROS_ERROR_STREAM(ros::this_node::getName()<<": Unable to lookup transforms");
00336 return false;
00337 }
00338
00339 std::vector<tf::Transform>::const_iterator i;
00340 const std::vector<tf::Transform> &goal_array = cartesian_traj.cartesian_points_;
00341 for(i = goal_array.begin(); i != goal_array.end(); i++)
00342 {
00343 base_to_tip_tf = (static_cast<tf::Transform>(base_to_parent_tf)) * (*i) * (static_cast<tf::Transform>(tcp_to_tip_link_tf));
00344 tf::poseTFToMsg(base_to_tip_tf,base_to_tip_pose);
00345 base_to_tip_poses.poses.push_back(base_to_tip_pose);
00346 }
00347
00348 return true;
00349 }
00350
00351
00352