示例#1
0
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;
}
示例#2
0
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;    
    
}
示例#3
0
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;        
}
示例#4
0
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));
 }
示例#7
0
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();
 }
示例#10
0
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.");
	}
示例#13
0
//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();
  }
示例#23
0
    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));
 }
示例#25
0
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;
}