int ArmMotionCommander::rt_arm_request_tool_pose_wrt_torso(void) { // debug: compare this to output of: //rosrun tf tf_echo torso yale_gripper_frame ROS_INFO("requesting right-arm tool pose"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_TOOL_POSE; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0)); if (!finished_before_timeout_) { ROS_WARN("did not respond within timeout"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("move did not return success; code = %d",cart_result_.return_code); return (int) cart_result_.return_code; } tool_pose_stamped_ = cart_result_.current_pose_gripper_right; ROS_INFO("move returned success; right arm tool pose: "); ROS_INFO("origin w/rt torso = %f, %f, %f ",tool_pose_stamped_.pose.position.x, tool_pose_stamped_.pose.position.y,tool_pose_stamped_.pose.position.z); ROS_INFO("quaternion x,y,z,w: %f, %f, %f, %f",tool_pose_stamped_.pose.orientation.x, tool_pose_stamped_.pose.orientation.y,tool_pose_stamped_.pose.orientation.z, tool_pose_stamped_.pose.orientation.w); return (int) cart_result_.return_code; }
int ArmMotionCommander::rt_arm_plan_jspace_path_current_to_qgoal(Eigen::VectorXd q_des_vec) { ROS_INFO("requesting a joint-space motion plan"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL; cart_goal_.q_goal_right.resize(7); for (int i=0;i<7;i++) cart_goal_.q_goal_right[i] = q_des_vec[i]; //specify the goal js pose cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0)); ROS_INFO("return code: %d",cart_result_.return_code); if (!finished_before_timeout_) { ROS_WARN("giving up waiting on result"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } ROS_INFO("finished before timeout"); if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) { ROS_WARN("right arm plan not valid"); return (int) cart_result_.return_code; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("unknown return code... not SUCCESS"); return (int) cart_result_.return_code; } //here if success return code ROS_INFO("returned SUCCESS from planning request"); computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time(); ROS_INFO("computed move time: %f",computed_arrival_time_); return (int) cart_result_.return_code; }
int ArmMotionCommander::rt_arm_plan_path_current_to_goal_pose(geometry_msgs::PoseStamped des_pose) { ROS_INFO("requesting a cartesian-space motion plan"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_POSE; cart_goal_.des_pose_gripper_right = des_pose; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0)); ROS_INFO("return code: %d",cart_result_.return_code); if (!finished_before_timeout_) { ROS_WARN("giving up waiting on result"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } ROS_INFO("finished before timeout"); if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) { ROS_WARN("right arm plan not valid"); return (int) cart_result_.return_code; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("unknown return code... not SUCCESS"); return (int) cart_result_.return_code; } //here if success return code ROS_INFO("returned SUCCESS from planning request"); computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time(); ROS_INFO("computed move time: %f",computed_arrival_time_); return (int) cart_result_.return_code; }
int ArmMotionCommander::rt_arm_plan_path_current_to_goal_dp_xyz(Eigen::Vector3d dp_displacement) { ROS_INFO("requesting a cartesian-space motion plan along vector"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_DP_XYZ; //must fill in desired vector displacement cart_goal_.arm_dp_right.resize(3); for (int i=0;i<3;i++) cart_goal_.arm_dp_right[i] = dp_displacement[i]; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0)); ROS_INFO("return code: %d",cart_result_.return_code); if (!finished_before_timeout_) { ROS_WARN("giving up waiting on result"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } ROS_INFO("finished before timeout"); if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) { ROS_WARN("right arm plan not valid"); return (int) cart_result_.return_code; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("unknown return code... not SUCCESS"); return (int) cart_result_.return_code; } //here if success return code ROS_INFO("returned SUCCESS from planning request"); computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time(); ROS_INFO("computed move time: %f",computed_arrival_time_); return (int) cart_result_.return_code; }
BlockManipulationAction() : block_detection_action_("block_detection", true), interactive_manipulation_action_("interactive_manipulation", true), pick_and_place_action_("pick_and_place", true), reset_arm_action_("reset_arm", true) { // Load parameters nh_.param<std::string>("/block_manipulation_action_demo/arm_link", arm_link, "/base_link"); nh_.param<double>("/block_manipulation_action_demo/gripper_open", gripper_open, 0.042); nh_.param<double>("/block_manipulation_action_demo/gripper_closed", gripper_closed, 0.024); nh_.param<double>("/block_manipulation_action_demo/z_up", z_up, 0.12); nh_.param<double>("/block_manipulation_action_demo/table_height", z_down, 0.01); nh_.param<double>("/block_manipulation_action_demo/block_size", block_size, 0.03); nh_.param<bool>("once", once, false); ROS_INFO("Block size %f", block_size); ROS_INFO("Table height %f", z_down); // Initialize goals block_detection_goal_.frame = arm_link; block_detection_goal_.table_height = z_down; block_detection_goal_.block_size = block_size; pick_and_place_goal_.frame = arm_link; pick_and_place_goal_.z_up = z_up; pick_and_place_goal_.gripper_open = gripper_open; pick_and_place_goal_.gripper_closed = gripper_closed; pick_and_place_goal_.topic = pick_and_place_topic; interactive_manipulation_goal_.block_size = block_size; interactive_manipulation_goal_.frame = arm_link; ROS_INFO("Finished initializing, waiting for servers..."); block_detection_action_.waitForServer(); ROS_INFO("Found block detection server."); interactive_manipulation_action_.waitForServer(); ROS_INFO("Found interactive manipulation."); pick_and_place_action_.waitForServer(); ROS_INFO("Found pick and place server."); ROS_INFO("Found servers."); reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal()); ROS_INFO("Reseted arm action."); detectBlocks(); }
void pickAndPlace(const actionlib::SimpleClientGoalState& state, const InteractiveBlockManipulationResultConstPtr& result) { ROS_INFO("Got interactive marker callback. Picking and placing."); if (state == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Succeeded!"); else { ROS_INFO("Did not succeed! %s", state.toString().c_str()); ros::shutdown(); } pick_and_place_action_.sendGoal(pick_and_place_goal_, boost::bind( &BlockManipulationAction::finish, this, _1, _2)); }
void ArmMotionCommander::send_test_goal(void) { ROS_INFO("sending a test goal"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::ARM_TEST_MODE; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0)); //bool finished_before_timeout = action_client.waitForResult(); // wait forever... if (!finished_before_timeout_) { ROS_WARN("giving up waiting on result"); } else { ROS_INFO("finished before timeout"); ROS_INFO("return code: %d",cart_result_.return_code); } }
void addBlocks(const actionlib::SimpleClientGoalState& state, const BlockDetectionResultConstPtr& result) { ROS_INFO("Got block detection callback. Adding blocks."); geometry_msgs::Pose block; if (state == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Succeeded!"); else { ROS_INFO("Did not succeed! %s", state.toString().c_str()); ros::shutdown(); } interactive_manipulation_action_.sendGoal(interactive_manipulation_goal_, boost::bind( &BlockManipulationAction::pickAndPlace, this, _1, _2)); }
void finish(const actionlib::SimpleClientGoalState& state, const PickAndPlaceResultConstPtr& result) { ROS_INFO("Got pick and place callback. Finished!"); if (state == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Succeeded!"); else ROS_INFO("Did not succeed! %s", state.toString().c_str()); reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal()); if (once) ros::shutdown(); else detectBlocks(); }
int ArmMotionCommander::rt_arm_execute_planned_path(void) { ROS_INFO("requesting execution of planned path"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_EXECUTE_PLANNED_PATH; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(computed_arrival_time_+2.0)); if (!finished_before_timeout_) { ROS_WARN("did not complete move in expected time"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("move did not return success; code = %d",cart_result_.return_code); return (int) cart_result_.return_code; } ROS_INFO("move returned success"); return (int) cart_result_.return_code; }
void ArmMotionInterface::execute_planned_move(void) { if (!path_is_valid_) { cart_result_.return_code = cartesian_planner::cart_moveResult::PATH_NOT_VALID; ROS_WARN("attempted to execute invalid path!"); cart_move_as_.setAborted(cart_result_); // tell the client we have given up on this goal; send the result message as well return; } // convert path to a trajectory: //baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message js_goal_.trajectory = des_trajectory_; //computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec(); ROS_INFO("sending action request to traj streamer node"); ROS_INFO("computed arrival time is %f", computed_arrival_time_); busy_working_on_a_request_ = true; g_js_doneCb_flag = 0; traj_streamer_action_client_.sendGoal(js_goal_, boost::bind(&ArmMotionInterface::js_doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired ROS_INFO("waiting on trajectory streamer..."); while (g_js_doneCb_flag == 0) { ROS_INFO("..."); ros::Duration(1.0).sleep(); ros::spinOnce(); } //finished_before_timeout_ = traj_streamer_action_client_.waitForResult(ros::Duration(computed_arrival_time_ + 2.0)); /* if (!finished_before_timeout_) { ROS_WARN("EXECUTE_PLANNED_PATH: giving up waiting on result"); cart_result_.return_code = cartesian_planner::cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; cart_move_as_.setSucceeded(cart_result_); //could say "aborted" } else { * */ ROS_INFO("finished before timeout"); cart_result_.return_code = cartesian_planner::cart_moveResult::SUCCESS; cart_move_as_.setSucceeded(cart_result_); //} path_is_valid_ = false; // reset--require new path before next move busy_working_on_a_request_ = false; //save the last point commanded, for future reference std::vector <double> last_pt; last_pt = des_trajectory_.points.back().positions; int njnts = last_pt.size(); for (int i = 0; i < njnts; i++) { last_arm_jnt_cmd_[i] = last_pt[i]; } }
void turn() { turtlebot_actions::TurtlebotMoveGoal goal; goal.forward_distance = 0; goal.turn_distance = M_PI; action_client.waitForServer(); action_client.sendGoal(goal); //wait for the action to return bool finished_before_timeout = action_client.waitForResult( ros::Duration(30.0)); if (finished_before_timeout) { actionlib::SimpleClientGoalState state = action_client.getState(); ROS_INFO("Action finished: %s", state.toString().c_str()); } else ROS_INFO("Action did not finish before the time out."); }
//send goal command to request right-arm joint angles; these will be stored in internal variable int ArmMotionCommander::rt_arm_request_q_data(void) { ROS_INFO("requesting right-arm joint angles"); cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_Q_DATA; cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(computed_arrival_time_+2.0)); if (!finished_before_timeout_) { ROS_WARN("did not respond within timeout"); return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT; } if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) { ROS_WARN("move did not return success; code = %d",cart_result_.return_code); return (int) cart_result_.return_code; } q_vec_ = cart_result_.q_arm_right; ROS_INFO("move returned success; right arm angles: "); ROS_INFO("%f; %f; %f; %f; %f; %f; %f",q_vec_[0],q_vec_[1],q_vec_[2],q_vec_[3],q_vec_[4],q_vec_[5],q_vec_[6]); return (int) cart_result_.return_code; }
// move platform server receives a new goal void moveGoalCB() { set_terminal_state_ = true; move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal; ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq); if (as_.isPreemptRequested() ||!ros::ok()) { ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq); if (planning_) ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now()); if (controlling_) ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now()); move_result_.result_state = 0; move_result_.error_string = "Preempt Requested!!!"; as_.setPreempted(move_result_); return; } // Convert move goal to plan goal pose_goal_.pose_goal = move_goal_.nav_goal; if (planner_state_sub.getNumPublishers()==0) { ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down"); planning_ = false; move_result_.result_state = 0; move_result_.error_string = "Planner is down"; as_.setAborted(move_result_); } else { ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2), actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(), actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback()); planning_ = true; ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner"); } return; }
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result) { planning_ = false; ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString()); move_result_.result_state = result->result_state; if (move_result_.result_state) //if plan OK { planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner if (ctrl_state_sub.getNumPublishers()==0) { ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down"); controlling_ = false; move_result_.result_state = 0; move_result_.error_string = "Controller is down!!!"; as_.setAborted(move_result_); } else { ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2), actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(), actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1)); controlling_ = true; ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller"); } } else //if plan NOT OK { ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now()); move_result_.error_string = "Planning Failed: " + result->error_string; ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string); as_.setAborted(move_result_); } return; }
void feedback_callback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP) { geometry_msgs::Pose identity_pose; identity_pose.orientation.w = 1.0; server_->setPose( feedback->marker_name, identity_pose); server_->applyChanges(); if(feedback->marker_name.compare(left_ee_semantics_.get_name()) == 0) { // TODO: turn this into a separate function, too goal_.command.left_ee.goal_pose.header = feedback->header; goal_.command.left_ee.goal_pose.pose = feedback->pose; goal_.command.left_ee.type = giskard_msgs::ArmCommand::CARTESIAN_GOAL; goal_.command.left_ee.convergence_thresholds = left_ee_semantics_.get_thresholds(); goal_.command.right_ee = create_identity_goal(right_ee_semantics_); } else if (feedback->marker_name.compare(right_ee_semantics_.get_name()) == 0) { // TODO: turn this into a separate function, too goal_.command.right_ee.goal_pose.header = feedback->header; goal_.command.right_ee.goal_pose.pose = feedback->pose; goal_.command.right_ee.type = giskard_msgs::ArmCommand::CARTESIAN_GOAL; goal_.command.right_ee.convergence_thresholds = right_ee_semantics_.get_thresholds(); goal_.command.left_ee = create_identity_goal(left_ee_semantics_); } else { ROS_ERROR("Could not associate marker with name '%s' to any bodypart.", feedback->marker_name.c_str()); return; } client_.sendGoal(goal_); } }
void actualizeGoal(ros::NodeHandle& nh, actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction>& client, arm_navigation_msgs::MoveArmGoal goal) { if (nh.ok()) { bool finished_within_time = false; client.sendGoal(goal); finished_within_time = client.waitForResult(ros::Duration(200.0)); if (!finished_within_time) { client.cancelGoal(); ROS_INFO("Timed out achieving goal"); } else { actionlib::SimpleClientGoalState state = client.getState(); bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); if(success) ROS_INFO("Action finished: %s",state.toString().c_str()); else ROS_INFO("Action failed: %s",state.toString().c_str()); } } }
void move_straight() { float current_angle = 0; float desired_angle = 0; /* if (map->frames.size() > 0) { Sophus::SE3f position = map->frames[map->frames.size() - 1]->get_pos(); Eigen::Vector3f current_heading = position.unit_quaternion() * Eigen::Vector3f::UnitZ(); current_angle = std::atan2(-current_heading(1), current_heading(0)); desired_angle = std::asin(position.translation()(1)/10.0); }*/ turtlebot_actions::TurtlebotMoveGoal goal; goal.forward_distance = 25.0; goal.turn_distance = current_angle - desired_angle; action_client.waitForServer(); action_client.sendGoal(goal); //wait for the action to return bool finished_before_timeout = action_client.waitForResult( ros::Duration(500.0)); if (finished_before_timeout) { actionlib::SimpleClientGoalState state = action_client.getState(); ROS_INFO("Action finished: %s", state.toString().c_str()); } else ROS_INFO("Action did not finish before the time out."); map->save("corridor_map"); }
void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose) { ROS_INFO("[pick and place] Picking. Also placing."); simple_arm_server::MoveArmGoal goal; simple_arm_server::ArmAction action; simple_arm_server::ArmAction grip; /* open gripper */ grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER; grip.move_time.sec = 1.0; grip.command = gripper_open; goal.motions.push_back(grip); /* arm straight up */ tf::Quaternion temp; temp.setRPY(0,1.57,0); action.goal.orientation.x = temp.getX(); action.goal.orientation.y = temp.getY(); action.goal.orientation.z = temp.getZ(); action.goal.orientation.w = temp.getW(); /* hover over */ action.goal.position.x = start_pose.position.x; action.goal.position.y = start_pose.position.y; action.goal.position.z = z_up; action.move_time.sec = 0.25; goal.motions.push_back(action); /* go down */ action.goal.position.z = start_pose.position.z; action.move_time.sec = 1.5; goal.motions.push_back(action); /* close gripper */ grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER; grip.command = gripper_closed; grip.move_time.sec = 1.0; goal.motions.push_back(grip); /* go up */ action.goal.position.z = z_up; action.move_time.sec = 1.0; goal.motions.push_back(action); /* hover over */ action.goal.position.x = end_pose.position.x; action.goal.position.y = end_pose.position.y; action.goal.position.z = z_up; action.move_time.sec = 1.0; goal.motions.push_back(action); /* go down */ action.goal.position.z = end_pose.position.z; action.move_time.sec = 1.5; goal.motions.push_back(action); /* open gripper */ grip.command = gripper_open; goal.motions.push_back(grip); /* go up */ action.goal.position.z = z_up; action.move_time.sec = 1.0; goal.motions.push_back(action); goal.header.frame_id = arm_link; client_.sendGoal(goal); ROS_INFO("[pick and place] Sent goal. Waiting."); client_.waitForResult(ros::Duration(30.0)); ROS_INFO("[pick and place] Received result."); if (client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) as_.setSucceeded(result_); else { ROS_INFO("Actual state: %s", client_.getState().toString().c_str()); as_.setAborted(result_); } }
void CartMoveActionServer::executeCB(const actionlib::SimpleActionServer<cwru_action::cart_moveAction>::GoalConstPtr& goal) { ROS_INFO("in executeCB of CartMoveActionServer"); cart_result_.err_code=0; cart_move_as_.isActive(); //unpack the necessary info: gripper_ang1_ = goal->gripper_jaw_angle1; gripper_ang2_ = goal->gripper_jaw_angle2; arrival_time_ = goal->move_time; // interpret the desired gripper poses: geometry_msgs::PoseStamped des_pose_gripper1 = goal->des_pose_gripper1; geometry_msgs::PoseStamped des_pose_gripper2 = goal->des_pose_gripper2; // convert the above to affine objects: des_gripper1_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper1.pose); cout<<"gripper1 desired pose; "<<endl; cout<<des_gripper1_affine_wrt_lcamera_.linear()<<endl; cout<<"origin: "<<des_gripper1_affine_wrt_lcamera_.translation().transpose()<<endl; des_gripper2_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper2.pose); cout<<"gripper2 desired pose; "<<endl; cout<<des_gripper2_affine_wrt_lcamera_.linear()<<endl; cout<<"origin: "<<des_gripper2_affine_wrt_lcamera_.translation().transpose()<<endl; //do IK to convert these to joint angles: //Eigen::VectorXd q_vec1,q_vec2; Vectorq7x1 q_vec1,q_vec2; q_vec1.resize(7); q_vec2.resize(7); trajectory_msgs::JointTrajectory des_trajectory; // an empty trajectory des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs des_trajectory.joint_names.clear(); //could put joint names in...but I assume a fixed order and fixed size, so this is unnecessary // if using wsn's trajectory streamer action server des_trajectory.header.stamp = ros::Time::now(); trajectory_msgs::JointTrajectoryPoint trajectory_point; //,trajectory_point2; trajectory_point.positions.resize(14); ROS_INFO("\n"); ROS_INFO("stored previous command to gripper one: "); cout<<gripper1_affine_last_commanded_pose_.linear()<<endl; cout<<"origin: "<<gripper1_affine_last_commanded_pose_.translation().transpose()<<endl; // first, reiterate previous command: // this could be easier, if saved previous joint-space trajectory point... des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*gripper1_affine_last_commanded_pose_; //previous pose ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements q_vec1 = ik_solver_.get_soln(); q_vec1(6) = last_gripper_ang1_; // include desired gripper opening angle des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*gripper2_affine_last_commanded_pose_; //previous pose ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements q_vec2 = ik_solver_.get_soln(); cout<<"q_vec1 of stored pose: "<<endl; for (int i=0;i<6;i++) { cout<<q_vec1[i]<<", "; } cout<<endl; q_vec2(6) = last_gripper_ang2_; // include desired gripper opening angle for (int i=0;i<7;i++) { trajectory_point.positions[i] = q_vec1(i); trajectory_point.positions[i+7] = q_vec2(i); } cout<<"start traj pt: "<<endl; for (int i=0;i<14;i++) { cout<<trajectory_point.positions[i]<<", "; } cout<<endl; trajectory_point.time_from_start = ros::Duration(0.0); // start time set to 0 // PUSH IN THE START POINT: des_trajectory.points.push_back(trajectory_point); // compute and append the goal point, in joint space trajectory: des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*des_gripper1_affine_wrt_lcamera_; ROS_INFO("desired gripper one location in base frame: "); cout<<"gripper1 desired pose; "<<endl; cout<<des_gripper_affine1_.linear()<<endl; cout<<"origin: "<<des_gripper_affine1_.translation().transpose()<<endl; ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements q_vec1 = ik_solver_.get_soln(); q_vec1(6) = gripper_ang1_; // include desired gripper opening angle des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*des_gripper2_affine_wrt_lcamera_; ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements q_vec2 = ik_solver_.get_soln(); cout<<"q_vec1 of goal pose: "<<endl; for (int i=0;i<6;i++) { cout<<q_vec1[i]<<", "; } cout<<endl; q_vec2(6) = gripper_ang2_; for (int i=0;i<7;i++) { trajectory_point.positions[i] = q_vec1(i); trajectory_point.positions[i+7] = q_vec2(i); } trajectory_point.time_from_start = ros::Duration(arrival_time_); cout<<"goal traj pt: "<<endl; for (int i=0;i<14;i++) { cout<<trajectory_point.positions[i]<<", "; } cout<<endl; des_trajectory.points.push_back(trajectory_point); js_goal_.trajectory = des_trajectory; // Need boost::bind to pass in the 'this' pointer // see example: http://library.isr.ist.utl.pt/docs/roswiki/actionlib_tutorials%282f%29Tutorials%282f%29Writing%2820%29a%2820%29Callback%2820%29Based%2820%29Simple%2820%29Action%2820%29Client.html // ac.sendGoal(goal, // boost::bind(&MyNode::doneCb, this, _1, _2), // Client::SimpleActiveCallback(), // Client::SimpleFeedbackCallback()); js_action_client_.sendGoal(js_goal_, boost::bind(&CartMoveActionServer::js_doneCb_,this,_1,_2)); // we could also name additional callback functions here, if desired // action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //alt--more callback funcs possible double t_timeout=arrival_time_+2.0; //wait 2 sec longer than expected duration of move bool finished_before_timeout = js_action_client_.waitForResult(ros::Duration(t_timeout)); //bool finished_before_timeout = action_client.waitForResult(); // wait forever... if (!finished_before_timeout) { ROS_WARN("joint-space interpolation result is overdue "); } else { ROS_INFO("finished before timeout"); } ROS_INFO("completed callback" ); cart_move_as_.setSucceeded(cart_result_); // tell the client that we were successful acting on the request, and return the "result" message //let's remember the last pose commanded, so we can use it as start pose for next move gripper1_affine_last_commanded_pose_ = des_gripper1_affine_wrt_lcamera_; gripper2_affine_last_commanded_pose_ = des_gripper2_affine_wrt_lcamera_; //and the jaw opening angles: last_gripper_ang1_=gripper_ang1_; last_gripper_ang2_=gripper_ang2_; }
bool executeGrasps(const std::vector<manipulation_msgs::Grasp>& possible_grasps, const geometry_msgs::Pose& block_pose) { ROS_INFO_STREAM_NAMED("pick_place_moveit","Creating Pickup Goal"); // --------------------------------------------------------------------------------------------- // Create PlanningOptions moveit_msgs::PlanningOptions options; // The diff to consider for the planning scene (optional) //PlanningScene planning_scene_diff // If this flag is set to true, the action // returns an executable plan in the response but does not attempt execution options.plan_only = false; // If this flag is set to true, the action of planning & // executing is allowed to look around (move sensors) if // it seems that not enough information is available about // the environment options.look_around = false; // If this value is positive, the action of planning & executing // is allowed to look around for a maximum number of attempts; // If the value is left as 0, the default value is used, as set // with dynamic_reconfigure //int32 look_around_attempts // If set and if look_around is true, this value is used as // the maximum cost allowed for a path to be considered executable. // If the cost of a path is higher than this value, more sensing or // a new plan needed. If left as 0.0 but look_around is true, then // the default value set via dynamic_reconfigure is used //float64 max_safe_execution_cost // If the plan becomes invalidated during execution, it is possible to have // that plan recomputed and execution restarted. This flag enables this // functionality options.replan = false; // The maximum number of replanning attempts //int32 replan_attempts // --------------------------------------------------------------------------------------------- // Create and populate the goal moveit_msgs::PickupGoal goal; // An action for picking up an object // The name of the object to pick up (as known in the planning scene) goal.target_name = chosen_block_object_.id; // which group should be used to plan for pickup goal.group_name = PLANNING_GROUP_NAME; // which end-effector to be used for pickup (ideally descpending from the group above) goal.end_effector = EE_NAME; // a list of possible grasps to be used. At least one grasp must be filled in goal.possible_grasps.resize(possible_grasps.size()); goal.possible_grasps = possible_grasps; // the name that the support surface (e.g. table) has in the collision map // can be left empty if no name is available //string collision_support_surface_name // whether collisions between the gripper and the support surface should be acceptable // during move from pre-grasp to grasp and during lift. Collisions when moving to the // pre-grasp location are still not allowed even if this is set to true. goal.allow_gripper_support_collision = true; // The names of the links the object to be attached is allowed to touch; // If this is left empty, it defaults to the links in the used end-effector //string[] attached_object_touch_links // Optional constraints to be imposed on every point in the motion plan //Constraints path_constraints // an optional list of obstacles that we have semantic information about // and that can be touched/pushed/moved in the course of grasping; // CAREFUL: If the object name 'all' is used, collisions with all objects are disabled during the approach & lift. //string[] allowed_touch_objects // The maximum amount of time the motion planner is allowed to plan for goal.allowed_planning_time = 10.0; // seconds? // Planning options goal.planning_options = options; //ROS_INFO_STREAM_NAMED("pick_place_moveit","Pause"); //ros::Duration(5.0).sleep(); // --------------------------------------------------------------------------------------------- // Send the grasp to move_group/Pickup ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending pick action to move_group/Pickup"); movegroup_action_.sendGoal(goal); if(!movegroup_action_.waitForResult(ros::Duration(20.0))) { ROS_INFO_STREAM_NAMED("pick_place_moveit","Returned early?"); return false; } if (movegroup_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO_STREAM_NAMED("pick_place_moveit","Plan successful!"); } else { ROS_ERROR_STREAM_NAMED("pick_place_moveit","FAILED: " << movegroup_action_.getState().toString() << ": " << movegroup_action_.getState().getText()); return false; } return true; }
// Constructor PickPlaceMoveItServer(const std::string name): nh_("~"), movegroup_action_("pickup", true), clam_arm_client_("clam_arm", true), ee_marker_is_loaded_(false), block_published_(false), action_server_(name, false) { base_link_ = "/base_link"; // ----------------------------------------------------------------------------------------------- // Adding collision objects collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1); // ----------------------------------------------------------------------------------------------- // Connect to move_group/Pickup action server while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server"); } // --------------------------------------------------------------------------------------------- // Connect to ClamArm action server while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server"); } // --------------------------------------------------------------------------------------------- // Create planning scene monitor planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION)); if (planning_scene_monitor_->getPlanningScene()) { //planning_scene_monitor_->startWorldGeometryMonitor(); //planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene"); //planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object"); } else { ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured"); } // --------------------------------------------------------------------------------------------- // Load the Robot Viz Tools for publishing to Rviz rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_)); // --------------------------------------------------------------------------------------------- // Register the goal and preempt callbacks action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this)); action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this)); action_server_.start(); // Announce state ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready."); ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command..."); // --------------------------------------------------------------------------------------------- // Send home ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home"); clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET; clam_arm_client_.sendGoal(clam_arm_goal_); while(!clam_arm_client_.getState().isDone() && ros::ok()) ros::Duration(0.1).sleep(); // --------------------------------------------------------------------------------------------- // Send fake command fake_goalCB(); }
void executeCb(const frontier_exploration::ExploreTaskGoalConstPtr &goal) { success_ = false; moving_ = false; explore_costmap_ros_->resetLayers(); //create costmap services ros::ServiceClient updateBoundaryPolygon = private_nh_.serviceClient<frontier_exploration::UpdateBoundaryPolygon>("explore_costmap/explore_boundary/update_boundary_polygon"); ros::ServiceClient getNextFrontier = private_nh_.serviceClient<frontier_exploration::GetNextFrontier>("explore_costmap/explore_boundary/get_next_frontier"); //wait for move_base and costmap services if(!move_client_.waitForServer() || !updateBoundaryPolygon.waitForExistence() || !getNextFrontier.waitForExistence()){ as_.setAborted(); return; } //set region boundary on costmap if(ros::ok() && as_.isActive()){ frontier_exploration::UpdateBoundaryPolygon srv; srv.request.explore_boundary = goal->explore_boundary; if(updateBoundaryPolygon.call(srv)){ ROS_INFO("Region boundary set"); }else{ ROS_ERROR("Failed to set region boundary"); as_.setAborted(); return; } } //loop until all frontiers are explored ros::Rate rate(frequency_); while(ros::ok() && as_.isActive()){ frontier_exploration::GetNextFrontier srv; //placeholder for next goal to be sent to move base geometry_msgs::PoseStamped goal_pose; //get current robot pose in frame of exploration boundary tf::Stamped<tf::Pose> robot_pose; explore_costmap_ros_->getRobotPose(robot_pose); //provide current robot pose to the frontier search service request tf::poseStampedTFToMsg(robot_pose,srv.request.start_pose); //evaluate if robot is within exploration boundary using robot_pose in boundary frame geometry_msgs::PoseStamped eval_pose = srv.request.start_pose; if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){ tf_listener_.transformPose(goal->explore_boundary.header.frame_id, srv.request.start_pose, eval_pose); } //check if robot is not within exploration boundary and needs to return to center of search area if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){ //check if robot has explored at least one frontier, and promote debug message to warning if(success_){ ROS_WARN("Robot left exploration boundary, returning to center"); }else{ ROS_DEBUG("Robot not initially in exploration boundary, traveling to center"); } //get current robot position in frame of exploration center geometry_msgs::PointStamped eval_point; eval_point.header = eval_pose.header; eval_point.point = eval_pose.pose.position; if(eval_point.header.frame_id != goal->explore_center.header.frame_id){ geometry_msgs::PointStamped temp = eval_point; tf_listener_.transformPoint(goal->explore_center.header.frame_id, temp, eval_point); } //set goal pose to exploration center goal_pose.header = goal->explore_center.header; goal_pose.pose.position = goal->explore_center.point; goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(eval_point.point, goal->explore_center.point) ); }else if(getNextFrontier.call(srv)){ //if in boundary, try to find next frontier to search ROS_DEBUG("Found frontier to explore"); success_ = true; goal_pose = feedback_.next_frontier = srv.response.next_frontier; retry_ = 5; }else{ //if no frontier found, check if search is successful ROS_DEBUG("Couldn't find a frontier"); //search is succesful if(retry_ == 0 && success_){ ROS_WARN("Finished exploring room"); as_.setSucceeded(); boost::unique_lock<boost::mutex> lock(move_client_lock_); move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now()); return; }else if(retry_ == 0 || !ros::ok()){ //search is not successful ROS_ERROR("Failed exploration"); as_.setAborted(); return; } ROS_DEBUG("Retrying..."); retry_--; //try to find frontier again, without moving robot continue; } //if above conditional does not escape this loop step, search has a valid goal_pose //check if new goal is close to old goal, hence no need to resend if(!moving_ || !pointsNearby(move_client_goal_.target_pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)){ ROS_DEBUG("New exploration goal"); move_client_goal_.target_pose = goal_pose; boost::unique_lock<boost::mutex> lock(move_client_lock_); if(as_.isActive()){ move_client_.sendGoal(move_client_goal_, boost::bind(&FrontierExplorationServer::doneMovingCb, this, _1, _2),0,boost::bind(&FrontierExplorationServer::feedbackMovingCb, this, _1)); moving_ = true; } lock.unlock(); } //check if continuous goal updating is enabled if(frequency_ > 0){ //sleep for specified frequency and then continue searching rate.sleep(); }else{ //wait for movement to finish before continuing while(ros::ok() && as_.isActive() && moving_){ ros::WallDuration(0.1).sleep(); } } } //goal should never be active at this point ROS_ASSERT(!as_.isActive()); }
void detectBlocks() { block_detection_action_.sendGoal(block_detection_goal_, boost::bind( &BlockManipulationAction::addBlocks, this, _1, _2)); }
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) { ROS_INFO("in executeCB: received manipulation task"); goal_action_code_ = goal->action_code; //verbatim from received goal action_code_ = goal->action_code; //init: this value changes as state machine advances through steps ROS_INFO("requested action code is: %d", goal_action_code_); //if action code is "MANIP_OBJECT", need to go through a sequence of action codes //otherwise, action code is a simple action code, and can use it as-is /* if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) { //if command is for manip, then we can expect an object code, perception source and dropoff pose object_code_ = goal->object_code; //what type of object is this? perception_source_ = goal->perception_source; //name sensor or provide coords dropoff_pose_ = goal->dropoff_frame; ROS_INFO("object code is: %d", object_code_); ROS_INFO("perception_source is: %d", goal->perception_source); //if (object_code_ == coordinator::ManipTaskGoal::TOY_BLOCK) { if (object_code_ == ObjectIdCodes::TOY_BLOCK_ID) { vision_object_code_ = object_code_; //same code; ROS_INFO("using object-finder object code %d", vision_object_code_); pickup_action_code_ = object_grabber::object_grabberGoal::GRAB_TOY_BLOCK; dropoff_action_code_ = object_grabber::object_grabberGoal::PLACE_TOY_BLOCK; //start the state machine with perceptual processing task: action_code_ = coordinator::ManipTaskGoal::GET_PICKUP_POSE; } else { ROS_WARN("unknown object type in manipulation action"); as_.setAborted(result_); } } else if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) { object_code_ = goal->object_code; //what type of object is this? */ if (goal_action_code_ == coordinator::ManipTaskGoal::DROPOFF_OBJECT) { object_code_ = goal->object_code; //what type of object is this? } else if (goal_action_code_ == coordinator::ManipTaskGoal::GRAB_OBJECT) { object_code_ = goal->object_code; //what type of object is this? ROS_INFO("object code is: %d", object_code_); if (goal->perception_source == coordinator::ManipTaskGoal::BLIND_MANIP) { ROS_INFO("blind manipulation; using provided pick-up pose"); pickup_pose_ = goal->pickup_frame; } } else if (goal_action_code_ == coordinator::ManipTaskGoal::GET_PICKUP_POSE) { ROS_INFO("object code is: %d", object_code_); ROS_INFO("perception_source is: %d", goal->perception_source); object_code_ = goal->object_code; //what type of object is this? perception_source_ = goal->perception_source; //name sensor or provide coords vision_object_code_ = object_code_; //same code } status_code_ = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK; working_on_task_ = true; //do work here while (working_on_task_) { //coordinator::ManipTaskResult::MANIP_SUCCESS) { feedback_.feedback_status = status_code_; as_.publishFeedback(feedback_); //ROS_INFO("executeCB: status_code = %d", status_code_); // each iteration, check if cancellation has been ordered if (as_.isPreemptRequested()) { ROS_WARN("goal cancelled!"); result_.manip_return_code = coordinator::ManipTaskResult::ABORTED; action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::ABORTED; working_on_task_ = false; as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well return; // done with callback } //here is where we step through states: switch (action_code_) { case coordinator::ManipTaskGoal::FIND_TABLE_SURFACE: ROS_INFO("serving request to find table surface"); found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY; object_finder_goal_.object_id = ObjectIdCodes::TABLE_SURFACE; //vision_object_code_; object_finder_goal_.known_surface_ht = false; //require find table height //object_finder_goal_.surface_ht = 0.05; //this is ignored for known_surface_ht=false object_finder_ac_.sendGoal(object_finder_goal_, boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2)); action_code_ = coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE; ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_); ROS_INFO("waiting on perception"); break; case coordinator::ManipTaskGoal::WAIT_FIND_TABLE_SURFACE: if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) { ROS_INFO("surface-finder success"); surface_height_ = pickup_pose_.pose.position.z; // table-top height, as found by object_finder found_surface_height_ = true; ROS_INFO("found table ht = %f", surface_height_); as_.setSucceeded(result_); // return the "result" message to client, along with "success" status return; //done w/ callback } else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) { //ROS_INFO("waiting on perception"); //do nothing } else { ROS_WARN("object-finder failure; aborting"); action_code_ = coordinator::ManipTaskGoal::ABORT; result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION; found_surface_height_ = false; } break; case coordinator::ManipTaskGoal::GET_PICKUP_POSE: ROS_INFO("establishing pick-up pose"); if (perception_source_ == coordinator::ManipTaskGoal::BLIND_MANIP) { ROS_INFO("blind manipulation; using provided pick-up pose"); pickup_pose_ = goal->pickup_frame; result_.object_pose = pickup_pose_; //done with perception, but do fake waiting anyway //declare victor on finding object found_object_code_ = object_finder::objectFinderResult::OBJECT_FOUND; action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER; status_code_ = coordinator::ManipTaskFeedback::PERCEPTION_BUSY; } else if (perception_source_ == coordinator::ManipTaskGoal::PCL_VISION) { ROS_INFO("invoking object finder"); found_object_code_ = object_finder::objectFinderResult::OBJECT_FINDER_BUSY; ROS_INFO("instructing finder to locate object %d", vision_object_code_); object_finder_goal_.object_id = vision_object_code_; if (found_surface_height_) { object_finder_goal_.known_surface_ht = true; object_finder_goal_.surface_ht = surface_height_; ROS_INFO("using surface ht = %f", surface_height_); } else { object_finder_goal_.known_surface_ht = false; //require find table height //object_finder_goal_.surface_ht = 0.05; //not needed } ROS_INFO("sending object-finder goal: "); object_finder_ac_.sendGoal(object_finder_goal_, boost::bind(&TaskActionServer::objectFinderDoneCb_, this, _1, _2)); action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_FINDER; ROS_INFO("waiting on perception"); } else { ROS_WARN("unrecognized perception mode; quitting"); action_code_ = coordinator::ManipTaskGoal::ABORT; result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION; } ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_); break; case coordinator::ManipTaskGoal::WAIT_FOR_FINDER: if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FOUND) { ROS_INFO("object-finder success"); //next step: use the pose to grab object: /* if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) { action_code_ = coordinator::ManipTaskGoal::GRAB_OBJECT; status_code_ = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY; } else*/ { working_on_task_ = false; // test--set to goal achieved action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE; result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS; //object pose is in result message as_.setSucceeded(result_); // return the "result" message to client, along with "success" status return; //done w/ callback } //will later test for result code of object grabber, so initialize it to PENDING //(next step in state machine) object_grabber_return_code_ = object_grabber::object_grabberResult::PENDING; } else if (found_object_code_ == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) { //ROS_INFO("waiting on perception"); //continue waiting } else { ROS_WARN("object-finder failure; aborting"); action_code_ = coordinator::ManipTaskGoal::ABORT; result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PERCEPTION; } break; case coordinator::ManipTaskGoal::GRAB_OBJECT: status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY; ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_); //ros::Duration(2.0).sleep(); //if here, then presumably have a valid pose for object of interest; grab it! //send object-grabber action code to grab specified object object_grabber_goal_.action_code = object_grabber::object_grabberGoal::GRAB_OBJECT;//pickup_action_code_; //specify the object to be grabbed object_grabber_goal_.object_frame = pickup_pose_; //and the object's current pose object_grabber_goal_.object_id = object_code_; // = goal->object_code; object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above ROS_INFO("sending goal to grab object: "); object_grabber_ac_.sendGoal(object_grabber_goal_, boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2)); action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT; status_code_ = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY; //will inspect this status to see if object grasp is eventually successful object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY; break; case coordinator::ManipTaskGoal::WAIT_FOR_GRAB_OBJECT: if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_ACQUIRED) { //success! ROS_INFO("acquired object"); /*if (goal_action_code_ == coordinator::ManipTaskGoal::MANIP_OBJECT) { //for manip command, have more steps to complete: action_code_ = coordinator::ManipTaskGoal::DROPOFF_OBJECT; status_code_ = coordinator::ManipTaskFeedback::PICKUP_SUCCESSFUL; } else*/ { //if just a grab command, we are now done working_on_task_ = false; // test--set to goal achieved action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE; result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS; //object pose is in result message as_.setSucceeded(result_); // return the "result" message to client, along with "success" status return; //done w/ callback } } else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) { // do nothing--just wait patiently //ROS_INFO("waiting for object grab"); } else { ROS_WARN("trouble with acquiring object"); action_code_ = coordinator::ManipTaskGoal::ABORT; result_.manip_return_code = coordinator::ManipTaskResult::FAILED_PICKUP; } break; case coordinator::ManipTaskGoal::DROPOFF_OBJECT: status_code_ = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0 ROS_INFO("executeCB: action_code, status_code = %d, %d", action_code_, status_code_); object_grabber_goal_.action_code = object_grabber::object_grabberGoal::DROPOFF_OBJECT;//dropoff_action_code_; //specify the object to be grabbed object_grabber_goal_.object_id = object_code_; // = goal->object_code; dropoff_pose_= goal->dropoff_frame; object_grabber_goal_.object_frame = dropoff_pose_; //and the object's current pose object_grabber_goal_.grasp_option = object_grabber::object_grabberGoal::DEFAULT_GRASP_STRATEGY; //from above ROS_INFO("sending goal to drop off object: "); object_grabber_ac_.sendGoal(object_grabber_goal_, boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2)); action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT; //will inspect this status to see if object grasp is eventually successful object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY; break; case coordinator::ManipTaskGoal::WAIT_FOR_DROPOFF_OBJECT: //ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_); if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success! ROS_INFO("switch/case happiness! dropped off object; manip complete"); working_on_task_ = false; // test--set to goal achieved action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::COMPLETED_DROPOFF; result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS; as_.setSucceeded(result_); // return the "result" message to client, along with "success" status return; //done w/ callback } else if (object_grabber_return_code_ == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) { // do nothing--just wait patiently //ROS_INFO("waiting for object dropoff"); } else { ROS_WARN("trouble with acquiring object"); action_code_ = coordinator::ManipTaskGoal::ABORT; result_.manip_return_code = coordinator::ManipTaskResult::FAILED_DROPOFF; } break; case coordinator::ManipTaskGoal::MOVE_TO_PRE_POSE: status_code_ = coordinator::ManipTaskFeedback::PREPOSE_MOVE_BUSY; object_grabber_goal_.action_code = object_grabber::object_grabberGoal::MOVE_TO_WAITING_POSE; //specify the object to be grabbed ROS_INFO("sending goal to move to pre-pose: "); object_grabber_ac_.sendGoal(object_grabber_goal_, boost::bind(&TaskActionServer::objectGrabberDoneCb_, this, _1, _2)); action_code_ = coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE; //will inspect this status to see if object grasp is eventually successful object_grabber_return_code_ = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY; break; case coordinator::ManipTaskGoal::WAIT_FOR_MOVE_TO_PREPOSE: if (object_grabber_return_code_ == object_grabber::object_grabberResult::SUCCESS) { //success! ROS_INFO("completed move to pre-pose"); working_on_task_ = false; // test--set to goal achieved action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::COMPLETED_MOVE; result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS; as_.setSucceeded(result_); // return the "result" message to client, along with "success" status return; //done w/ callback } else { ROS_INFO("object_grabber_return_code_ = %d",object_grabber_return_code_); ros::Duration(0.5).sleep(); } break; case coordinator::ManipTaskGoal::ABORT: ROS_WARN("aborting goal..."); //retain reason for failure to report back to client //result_.manip_return_code = coordinator::ManipTaskResult::ABORTED; action_code_ = coordinator::ManipTaskGoal::NO_CURRENT_TASK; status_code_ = coordinator::ManipTaskFeedback::ABORTED; working_on_task_ = false; as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well return; // done with callback default: ROS_WARN("executeCB: error--case not recognized"); working_on_task_ = false; break; } } ROS_INFO("executeCB: I should not be here..."); //if we survive to here, then the goal was successfully accomplished; inform the client result_.manip_return_code = coordinator::ManipTaskResult::ABORTED; as_.setAborted(result_); // return the "result" message to client, along with "success" status return; }