예제 #1
0
		/*!
		* \brief Executes the callback from the actionlib
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{			
			ROS_INFO("sdh: executeCB");
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}

			while (hasNewGoal_ == true ) usleep(10000);

			// \todo TODO: use joint_names for assigning values
			targetAngles_.resize(DOF_);
			targetAngles_[0] = goal->trajectory.points[0].positions[2]*180.0/pi_; // sdh_knuckle_joint
			targetAngles_[1] = goal->trajectory.points[0].positions[5]*180.0/pi_; // sdh_finger22_joint
			targetAngles_[2] = goal->trajectory.points[0].positions[6]*180.0/pi_; // sdh_finger23_joint
			targetAngles_[3] = goal->trajectory.points[0].positions[0]*180.0/pi_; // sdh_thumb2_joint
			targetAngles_[4] = goal->trajectory.points[0].positions[1]*180.0/pi_; // sdh_thumb3_joint
			targetAngles_[5] = goal->trajectory.points[0].positions[3]*180.0/pi_; // sdh_finger12_joint
			targetAngles_[6] = goal->trajectory.points[0].positions[4]*180.0/pi_; // sdh_finger13_joint
			ROS_INFO("received new position goal: [['sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']] = [%f,%f,%f,%f,%f,%f,%f,%f]",goal->trajectory.points[0].positions[0],goal->trajectory.points[0].positions[1],goal->trajectory.points[0].positions[2],goal->trajectory.points[0].positions[3],goal->trajectory.points[0].positions[4],goal->trajectory.points[0].positions[5],goal->trajectory.points[0].positions[6]);
		
			hasNewGoal_ = true;
			
			usleep(500000); // needed sleep until sdh starts to change status from idle to moving
			
			bool finished = false;
			while(finished == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
				for ( size_t i = 0; i < state_.size(); i++ )
		 		{
		 			ROS_DEBUG("state[%d] = %d",i,state_[i]);
		 			if (state_[i] == 0)
		 			{
		 				finished = true;
		 			}
		 			else
		 			{	
		 				finished = false;
		 			}
		 		}
		 		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeeded			
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			//result_.result.data = "succesfully received new goal";
			//result_.success = 1;
			//as_.setSucceeded(result_);
			as_.setSucceeded();
		}
예제 #2
0
void Navigator::executeCB(const actionlib::SimpleActionServer<navigator::navigatorAction>::GoalConstPtr& goal) {
    int destination_id = goal->location_code;
    geometry_msgs::PoseStamped destination_pose;
    int navigation_status;

    if (destination_id==navigator::navigatorGoal::COORDS) {
        destination_pose=goal->desired_pose;
    }
    
    switch(destination_id) {
        case navigator::navigatorGoal::HOME: 
              //specialized function to navigate to pre-defined HOME coords
               navigation_status = navigate_home(); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached home");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate home!");
                   navigator_as_.setAborted(result_);
               }
               break;
        case navigator::navigatorGoal::TABLE: 
              //specialized function to navigate to pre-defined TABLE coords
               navigation_status = navigate_to_table(); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached table");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate to table!");
                   navigator_as_.setAborted(result_);
               }
               break;
        case navigator::navigatorGoal::COORDS: 
              //more general function to navigate to specified pose:
              destination_pose=goal->desired_pose;
               navigation_status = navigate_to_pose(destination_pose); 
               if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
                   ROS_INFO("reached desired pose");
                   result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
                   navigator_as_.setSucceeded(result_);
               }
               else {
                   ROS_WARN("could not navigate to desired pose!");
                   navigator_as_.setAborted(result_);
               }
               break;               
               
        default:
             ROS_WARN("this location ID is not implemented");
             result_.return_code = navigator::navigatorResult::DESTINATION_CODE_UNRECOGNIZED; 
             navigator_as_.setAborted(result_);
            }
  
}
예제 #3
0
		/*!
		* \brief Executes the callback from the actionlib.
		*
		* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
		* \param goal JointTrajectoryGoal
		*/
		void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
		{
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			if (!isInitialized_)
			{
				ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
				as_.setAborted();
				return;
			}
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			finished_ = false;
			
			// stoping arm to prepare for new trajectory
			std::vector<double> VelZero;
			VelZero.resize(ModIds_param_.size());
			PCube_->MoveVel(VelZero);

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
			
			while(finished_ == false)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		}
      void setJointsCB( const staubliTX60::SetJointsGoalConstPtr &goal ) {
	 //staubli.ResetMotion();
	 ros::Rate rate(10);
	 bool success = true;
	 ROS_INFO("Set Joints Action Cmd received \n");
	 if( staubli.MoveJoints(goal->j,
				goal->params.movementType,
				goal->params.jointVelocity,
				goal->params.jointAcc,
				goal->params.jointDec,
				goal->params.endEffectorMaxTranslationVel, 
				goal->params.endEffectorMaxRotationalVel,
				goal->params.distBlendPrev,
				goal->params.distBlendNext
				) )
	   {
	    ROS_INFO("Cmd received, moving to desired joint angles.");
	    while(true){
	       if (as_.isPreemptRequested() || !ros::ok()) {
		  ROS_INFO("%s: Preempted", action_name_.c_str());
		  // set the action state to preempted
		  staubli.ResetMotion();
		  as_.setPreempted();
		  success = false;
		  break;
	       }
	       if( polling(goal->j) ) break;
	       rate.sleep();
	    }
	    if(success) as_.setSucceeded(result_);
	 }else {
	    as_.setAborted();
	    ROS_ERROR("Cannot move to specified joints' configuration.");
	 }
      }
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message 
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g.,  "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
    //ROS_INFO("in executeCB");
    //ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes
    
    //....

    // for illustration, populate the "result" message with two numbers:
    // the "input" is the message count, copied from goal->input (as sent by the client)
    // the "goal_stamp" is the server's count of how many goals it has serviced so far
    // if there is only one client, and if it is never restarted, then these two numbers SHOULD be identical...
    // unless some communication got dropped, indicating an error
    // send the result message back with the status of "success"

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.output = g_count; // we'll use the member variable result_, defined in our class
    result_.goal_stamp = goal->input;
    
    // the class owns the action server, so we can use its member methods here
   
    // DEBUG: if client and server remain in sync, all is well--else whine and complain and quit
    // NOTE: this is NOT generically useful code; server should be happy to accept new clients at any time, and
    // no client should need to know how many goals the server has serviced to date
    if (g_count != goal->input) {
        ROS_WARN("hey--mismatch!");
        ROS_INFO("g_count = %d; goal_stamp = %d", g_count, result_.goal_stamp);
        g_count_failure = true; //set a flag to commit suicide
        ROS_WARN("informing client of aborted goal");
        as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
    }
    else {
         as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message
    }
}
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <Action_Server::gazebo.action> customizes the simple action server to use our own "action" message 
// defined in our package, "Action_Server", in the subdirectory "action", called "Action.action"
// The name "Action" is prepended to other message types created automatically during compilation.
// e.g.,  "gazebo.action" is auto-generated from (our) base name "Action" and generic name "Action"
  void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<Action_Server::gazebo.action>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB");
    ROS_INFO("goal input is: %d", goal->input);
    //do work here: this is where your interesting code goes
    ros::Rate timer(1.0); // 1Hz timer
    countdown_val_ = goal->input;
    //implement a simple timer, which counts down from provided countdown_val to 0, in seconds
    while (countdown_val_>0) {
     ROS_INFO("countdown = %d",countdown_val_);
     
       // each iteration, check if cancellation has been ordered
     if (as_.isPreemptRequested()){	
      ROS_WARN("goal cancelled!");
      result_.output = countdown_val_;
          as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
          return; // done with callback
        }
        
 	   //if here, then goal is still valid; provide some feedback
 	   feedback_.fdbk = countdown_val_; // populate feedback message with current countdown value
 	   as_.publishFeedback(feedback_); // send feedback to the action client that requested this goal
       countdown_val_--; //decrement the timer countdown
       timer.sleep(); //wait 1 sec between loop iterations of this timer
     }
    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.output = countdown_val_; //value should be zero, if completed countdown
    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
  }
예제 #7
0
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");
    ROS_INFO("object code is: %d", goal->object_code);
    ROS_INFO("perception_source is: %d", goal->perception_source);
    g_status_code = 0; //coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    g_action_code = 1; //start with perceptual processing
    //do work here: this is where your interesting code goes
    while ((g_status_code != SUCCESS)&&(g_status_code != ABORTED)) { //coordinator::ManipTaskResult::MANIP_SUCCESS) {
        feedback_.feedback_status = g_status_code;
        as_.publishFeedback(feedback_);
        //ros::Duration(0.1).sleep();
        ROS_INFO("executeCB: g_status_code = %d",g_status_code);
        // each iteration, check if cancellation has been ordered

        if (as_.isPreemptRequested()) {
            ROS_WARN("goal cancelled!");
            result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
            g_action_code = 0;
            g_status_code = 0;
            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 (g_action_code) {

            case 1:  
                ROS_INFO("starting new task; should call object finder");
                g_status_code = 1; //
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 2;                
                break;

            case 2: // also do nothing...but maybe comment on status? set a watchdog?
                g_status_code = 2;
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                ros::Duration(2.0).sleep();
                g_action_code = 3;
                break;

            case 3:
                g_status_code = SUCCESS; //coordinator::ManipTaskResult::MANIP_SUCCESS; //code 0
                ROS_INFO("executeCB: g_action_code, status_code = %d, %d",g_action_code,g_status_code);
                g_action_code = 0; // back to waiting state--regardless
                break;
            default:
                ROS_WARN("executeCB: error--case not recognized");
                break;
        }        
    ros::Duration(0.5).sleep();    
        
    }
    ROS_INFO("executeCB: manip success; returning success");
        //if we survive to here, then the goal was successfully accomplished; inform the client
        result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
        as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
            g_action_code = 0;
            g_status_code = 0;
    return;
}
    void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result)
    {
        controlling_ = false;
        ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString());

        move_result_.result_state = result->result_state;
        move_result_.error_string = result->error_string;

        if (move_result_.result_state)
        {
            as_.setSucceeded(move_result_);
            ROS_INFO_NAMED(logger_name_, "Goal was successful :)");
        }
        else
        {
             ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)");

            // if is preempted => as_ was already set, cannot set again
            if (state.toString() != "PREEMPTED")
            {
                as_.setAborted(move_result_);
                ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted");
            }
            else
            {
                if (set_terminal_state_)
                {
                     as_.setPreempted(move_result_);
                     ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted");
                }
            }
        }
    }
예제 #9
0
	//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
	void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {	
		if(isInitialized_) {	
			ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
			// saving goal into local variables
			traj_ = goal->trajectory;
			traj_point_nr_ = 0;
			traj_point_ = traj_.points[traj_point_nr_];
			GoalPos_ = traj_point_.positions[0];
			finished_ = false;
			
			// stoping axis to prepare for new trajectory
			CamAxis_->Stop();

			// check that preempt has not been requested by the client
			if (as_.isPreemptRequested())
			{
				ROS_INFO("%s: Preempted", action_name_.c_str());
				// set the action state to preempted
				as_.setPreempted();
			}
			
			usleep(2000000); // needed sleep until drive starts to change status from idle to moving
			
			while (not finished_)
			{
				if (as_.isNewGoalAvailable())
				{
					ROS_WARN("%s: Aborted", action_name_.c_str());
					as_.setAborted();
					return;
				}
		   		usleep(10000);
				//feedback_ = 
				//as_.send feedback_
			}

			// set the action state to succeed			
			//result_.result.data = "executing trajectory";
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			// set the action state to succeeded
			as_.setSucceeded(result_);
		
		} else {
			as_.setAborted();
			ROS_WARN("Camera_axis not initialized yet!");
		}
	}
 void executeFollowTrajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) 
 {
     ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size());
     spawnTrajector(goal->trajectory);
     // only set to succeeded if component could reach position. this is currently not the care for e.g. by emergency stop, hardware error or exceeds limit.
     if(rejected_)
         as_follow_.setAborted(); //setRejected not implemented in simpleactionserver ?
     else
     {
         if(failure_)
             as_follow_.setAborted();
         else
             as_follow_.setSucceeded();
     }
     rejected_ = false;
     failure_ = false;
 }
예제 #11
0
    /**
     * @brief Done callback for the move_base client, checks for errors and aborts exploration task if necessary
     * @param state State from the move_base client
     * @param result Result from the move_base client
     */
    void doneMovingCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result){

        if (state == actionlib::SimpleClientGoalState::ABORTED){
            ROS_ERROR("Failed to move");
            as_.setAborted();
        }else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
            moving_ = false;
        }

    }
예제 #12
0
 void as_cb(const cob_sound::SayGoalConstPtr &goal)
 {
   bool ret = say(goal->text.data);
   if (ret)
   {
       as_.setSucceeded();
   }
   else
   {
       as_.setAborted();
   }
 }
    /***********************************Callback**************************************************/
    void goalCallback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
        std::vector<std::string> jointNames = goal->trajectory.joint_names;
        if (checkIfValid(jointNames)) {
            size_t commandSize = goal->trajectory.points.size();
            for (int i = 0; i < commandSize - 1; ++i) {
                sensor_msgs::JointState command;
                command.name = goal->trajectory.joint_names;
                command.position = goal->trajectory.points[i].positions;
                command.velocity = goal->trajectory.points[i].velocities;
                command.effort = goal->trajectory.points[i].effort;


                _jointCommand.publish(command);

                ROS_INFO_STREAM(goal->trajectory.points[i]);
                clrFeedback();
                _feedback.joint_names = jointNames;
                _feedback.desired.effort = goal->trajectory.points[i].effort;
                _feedback.desired.velocities = goal->trajectory.points[i].velocities;
                _feedback.desired.positions = goal->trajectory.points[i].positions;
                waitForExecution();

            }
            sensor_msgs::JointState command;
            command.name = goal->trajectory.joint_names;
            command.position = goal->trajectory.points[commandSize - 1].positions;

            for(int i = 0; i < command.name.size(); ++i)
            {
                command.velocity.push_back(0.2);
            }

            command.effort = goal->trajectory.points[commandSize - 1].effort;
            rosInfo("Last");
            _jointCommand.publish(command);

            clrFeedback();
            _feedback.joint_names = jointNames;
            _feedback.desired.effort = goal->trajectory.points[commandSize - 1].effort;
            _feedback.desired.velocities = goal->trajectory.points[commandSize - 1].velocities;
            _feedback.desired.positions = goal->trajectory.points[commandSize - 1].positions;
            waitForExecution();

            _result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
            _result.error_string = "Cool";
            _actionServer.setSucceeded(_result);
        }
        else {
            _result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
            _result.error_string = "Not cool";
            _actionServer.setAborted(_result);
        }
    }
    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 goalCB()
 {
   // accept the new goal
   feedback_.forward_distance = 0.0;
   feedback_.turn_distance = 0.0;
   
   result_.forward_distance = 0.0;
   result_.turn_distance = 0.0;
   
   goal_ = as_.acceptNewGoal();
   
   if (!turnOdom(goal_->turn_distance))
   { 
     as_.setAborted(result_);
     return;
   }
   
   if (driveForwardOdom(goal_->forward_distance))
     as_.setSucceeded(result_);
   else
     as_.setAborted(result_);
 }
    void action_execute_stop_callback(const s8_motor_controller::StopGoalConstPtr & goal) {
        ROS_INFO("STOP");

        if(is_stopping) {
            ROS_FATAL("Stop action callback executed but is already stopping");
        }

        stop();
        is_stopping = true;

        const int timeout = 10; // 10 seconds.
        const int rate_hz = 10;

        ros::Rate rate(rate_hz);

        const int encoder_callback_ticks_treshold = 10;

        int ticks = 0;

        while(!is_still && ticks <= timeout * rate_hz && ticks_since_last_encoder_callback < encoder_callback_ticks_treshold) {
            rate.sleep();
            ticks++;
            ticks_since_last_encoder_callback++;
        }

        if(ticks_since_last_encoder_callback >= encoder_callback_ticks_treshold) {
            ROS_INFO("No encoder callback in %d ticks. Assuming still.", ticks_since_last_encoder_callback);
            is_still = true;
        }

        if(!is_still) {
            ROS_WARN("Unable to stop. Stop action failed.");
            s8_motor_controller::StopResult stop_action_result;
            stop_action_result.stopped = false;
            stop_action.setAborted(stop_action_result);
        } else {
            s8_motor_controller::StopResult stop_action_result;
            stop_action_result.stopped = true;
            stop_action.setSucceeded(stop_action_result);
            ROS_INFO("Stop action succeeded");
        }

        is_stopping = false;
        ignore_twist = true;
        ignored_twist_cnt = 0;
    }
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];
    }
}
예제 #18
0
//executeCB implementation: this is a member method that will get registered with the action server
// argument type is very long.  Meaning:
// actionlib is the package for action servers
// SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package)
// <example_action_server::demoAction> customizes the simple action server to use our own "action" message 
// defined in our package, "example_action_server", in the subdirectory "action", called "demo.action"
// The name "demo" is prepended to other message types created automatically during compilation.
// e.g.,  "demoAction" is auto-generated from (our) base name "demo" and generic name "Action"
void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<example_action_server::demoAction>::GoalConstPtr& goal) {
    ROS_INFO("callback activated");
    double yaw_desired, yaw_current, travel_distance, spin_angle;
    nav_msgs::Path paths;
    geometry_msgs::Pose pose_desired;
    paths = goal->input;

    int npts = paths.poses.size();
    ROS_INFO("received path request with %d poses",npts);    

	int move = 0;
	for (int i=0;i<npts;i++) { //visit each subgoal
		// odd notation: drill down, access vector element, drill some more to get pose
		pose_desired = paths.poses[i].pose; //get first pose from vector of poses        
        	get_yaw_and_dist(g_current_pose, pose_desired,travel_distance, yaw_desired);
        	ROS_INFO("pose %d: desired yaw = %f; desired (x,y) = (%f,%f)",i,yaw_desired, 		 	
				pose_desired.position.x,pose_desired.position.y); 
        	ROS_INFO("current (x,y) = (%f, %f)",g_current_pose.position.x,g_current_pose.position.y);
        	ROS_INFO("travel distance = %f",travel_distance); 
		ROS_INFO("pose %d: desired yaw = %f",i,yaw_desired);        
		yaw_current = convertPlanarQuat2Phi(g_current_pose.orientation); //our current yaw--should use a sensor
		spin_angle = yaw_desired - yaw_current; // spin this much
		//spin_angle = min_spin(spin_angle);// but what if this angle is > pi?  then go the other way
		do_spin(spin_angle); // carry out this incremental action
		// we will just assume that this action was successful--really should have sensor feedback here
		g_current_pose.orientation = pose_desired.orientation; // assumes got to desired orientation precisely
		move = pose_desired.position.x;
		
		ROS_INFO("Move:  %d", move);
		do_move(move);
		
		if(as_.isPreemptRequested()){
		   do_halt();
		   ROS_WARN("Srv: goal canceled.");
		   result_.output = -1;
		   as_.setAborted(result_);
		   return;
		}
        }
	result_.output = 0;
	as_.setSucceeded(result_);
	ROS_INFO("Move finished.");
}
예제 #19
0
void TaskActionServer::executeCB(const actionlib::SimpleActionServer<coordinator::ManipTaskAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB: received manipulation task");
    ROS_INFO("object code is: %d", goal->object_code);
    ROS_INFO("perception_source is: %d", goal->perception_source);
    //do work here: this is where your interesting code goes
    feedback_.feedback_status = coordinator::ManipTaskFeedback::RECEIVED_NEW_TASK;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing
    //get pickup pose:
    feedback_.feedback_status = coordinator::ManipTaskFeedback::PERCEPTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing

    feedback_.feedback_status = coordinator::ManipTaskFeedback::PICKUP_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing    

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_PLANNING_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      

    feedback_.feedback_status = coordinator::ManipTaskFeedback::DROPOFF_MOTION_BUSY;
    as_.publishFeedback(feedback_);
    ros::Duration(1.0).sleep(); // pretend we are processing      


    // each iteration, check if cancellation has been ordered
    if (as_.isPreemptRequested()) {
        ROS_WARN("goal cancelled!");
        result_.manip_return_code = coordinator::ManipTaskResult::ABORTED;
        as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well
        return; // done with callback
    }

    //if we survive to here, then the goal was successfully accomplished; inform the client
    result_.manip_return_code = coordinator::ManipTaskResult::MANIP_SUCCESS;
    as_.setSucceeded(result_); // return the "result" message to client, along with "success" status
}
    // 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 ArmMotionInterface::rescale_planned_trajectory_time(double time_stretch_factor) {
    if (!path_is_valid_) {
        cart_result_.return_code = cartesian_planner::cart_moveResult::PATH_NOT_VALID;
        ROS_WARN("do not have a valid path!");
        cart_move_as_.setAborted(cart_result_); // tell the client we have given up on this goal; send the result message as well
    }

    //given a trajectory, stretch out the arrival times by factor time_stretch_factor
    int npts = des_trajectory_.points.size();
    double arrival_time_sec, new_arrival_time_sec;
    for (int i = 0; i < npts; i++) {
        arrival_time_sec = des_trajectory_.points[i].time_from_start.toSec();
        new_arrival_time_sec = arrival_time_sec*time_stretch_factor;
        ros::Duration arrival_duration(new_arrival_time_sec); //convert time to a ros::Duration type
        des_trajectory_.points[i].time_from_start = arrival_duration;
    }
    computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
    cart_result_.computed_arrival_time = computed_arrival_time_;

    cart_result_.return_code = cartesian_planner::cart_moveResult::SUCCESS;
    cart_move_as_.setSucceeded(cart_result_);
}
  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_);
    }
  }
예제 #23
0
void TrajActionServer::executeCB(const actionlib::SimpleActionServer<davinci_traj_streamer::trajAction>::GoalConstPtr& goal) {
    double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
    int isegment;
    trajectory_msgs::JointTrajectoryPoint trajectory_point0;

    Eigen::VectorXd qvec, qvec0, qvec_prev, qvec_new;
    // TEST TEST TEST
    //Eigen::VectorXd q_vec;
    //q_vec<<0.1,0.2,0.15,0.4,0.5,0.6,0.7;    

    ROS_INFO("in executeCB");

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.return_val = g_count; // we'll use the member variable result_, defined in our class
    result_.traj_id = goal->traj_id;
    cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
    // copy trajectory to global var:
    new_trajectory = goal->trajectory; // 
    // insist that a traj have at least 2 pts
    int npts = new_trajectory.points.size();
    if (npts  < 2) {
        ROS_WARN("too few points; aborting goal");
        as_.setAborted(result_);
    } else { //OK...have a valid trajectory goal; execute it
        //got_new_goal = true;
        //got_new_trajectory = true;
        ROS_INFO("Cb received traj w/ npts = %d",npts);
        //cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
        //trajectory_msgs::JointTrajectoryPoint trajectory_point0;
        //trajectory_point0 = new_trajectory.points[0];  
        //trajectory_point0 =  tj_msg.points[0];   
        //cout<<new_trajectory.points[0].positions.size()<<" =  new_trajectory.points[0].positions.size()"<<endl;
        //cout<<"size of positions[]: "<<trajectory_point0.positions.size()<<endl;
        cout << "subgoals: " << endl;
        int njnts; 
        for (int i = 0; i < npts; i++) {
            njnts = new_trajectory.points[i].positions.size();
            cout<<"njnts: "<<njnts<<endl;
            for (int j = 0; j < njnts; j++) { //copy from traj point to 7x1 vector
                cout << new_trajectory.points[i].positions[j] << ", ";
            }
            cout<<endl;
            cout<<"time from start: "<<new_trajectory.points[i].time_from_start.toSec()<<endl;
            cout << endl;
        }

        as_.isActive();

        working_on_trajectory = true;
        //got_new_trajectory=false;
        traj_clock = 0.0; // initialize clock for trajectory;
        isegment = 0;
        trajectory_point0 = new_trajectory.points[0];
        njnts = new_trajectory.points[0].positions.size();
        int njnts_new;
        qvec_prev.resize(njnts);
        qvec_new.resize(njnts);
        ROS_INFO("populating qvec_prev: ");
        for (int i = 0; i < njnts; i++) { //copy from traj point to Eigen type vector
            qvec_prev[i] = trajectory_point0.positions[i];
        }
        //cmd_pose_right(qvec0); //populate and send out first command  
        //qvec_prev = qvec0;
        cout << "start pt: " << qvec_prev.transpose() << endl;
    }
    while (working_on_trajectory) {
        traj_clock += dt_traj;
        // update isegment and qvec according to traj_clock; 
        //if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false 
        //ROS_INFO("traj_clock = %f; updating qvec_new",traj_clock);
        working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
        //cmd_pose_right(qvec_new); // use qvec to populate object and send it to robot
        //ROS_INFO("publishing qvec_new as command");
        //davinciJointPublisher.pubJointStatesAll(qvec_new);
        command_joints(qvec_new);  //map these to all gazebo joints and publish as commands      
        qvec_prev = qvec_new;

        //cout << "traj_clock: " << traj_clock << "; vec:" << qvec_new.transpose() << endl;
        ros::spinOnce();
        ros::Duration(dt_traj).sleep();
    }
    ROS_INFO("completed execution of a trajectory" );
    as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message 
}
예제 #24
0
파일: node.cpp 프로젝트: uf-mil/SubjuGator
  void timer_callback(const ros::TimerEvent &)
  {
    mil_msgs::MoveToResult actionresult;

    // Handle disabled, killed, or no odom before attempting to produce trajectory
    std::string err = "";
    if (disabled)
      err = "c3 disabled";
    else if (kill_listener.isRaised())
      err = "killed";
    else if (!c3trajectory)
      err = "no odom";

    if (!err.empty())
    {
      if (c3trajectory)
        c3trajectory.reset();  // On revive/enable, wait for odom before station keeping

      // Cancel all goals while killed/disabled/no odom
      if (actionserver.isNewGoalAvailable())
        actionserver.acceptNewGoal();
      if (actionserver.isActive())
      {
        actionresult.error = err;
        actionresult.success = false;
        actionserver.setAborted(actionresult);
      }
      return;
    }

    ros::Time now = ros::Time::now();

    auto old_waypoint = current_waypoint;

    if (actionserver.isNewGoalAvailable())
    {
      boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
      current_waypoint =
          subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
                                             goal->speed, !goal->uncoordinated, !goal->blind);
      current_waypoint_t = now;
      this->linear_tolerance = goal->linear_tolerance;
      this->angular_tolerance = goal->angular_tolerance;

      waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);

      // Check if waypoint is valid
      std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
          Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
      actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
      actionresult.success = checkWPResult.first;
      if (checkWPResult.first == false && waypoint_check_)  // got a point that we should not move to
      {
        waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
        if (checkWPResult.second ==
            WAYPOINT_ERROR_TYPE::UNKNOWN)  // if unknown, check if there's a huge displacement with the new waypoint
        {
          auto a_point = Pose_from_Waypoint(current_waypoint);
          auto b_point = Pose_from_Waypoint(old_waypoint);
          // If moved more than .5m, then don't allow
          if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
          {
            ROS_ERROR("can't move there! - need to rotate");
            current_waypoint = old_waypoint;
          }
        }
        // if point is occupied, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
        {
          ROS_ERROR("can't move there! - waypoint is occupied");
          current_waypoint = old_waypoint;
        }
        // if point is above water, reject move
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
        {
          ROS_ERROR("can't move there! - waypoint is above water");
          current_waypoint = old_waypoint;
        }
        if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
        {
          ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
        }
      }
    }
    if (actionserver.isPreemptRequested())
    {
      current_waypoint = c3trajectory->getCurrentPoint();
      current_waypoint.do_waypoint_validation = false;
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
    }

    // Remember the previous trajectory
    auto old_trajectory = c3trajectory->getCurrentPoint();

    while (c3trajectory_t + traj_dt < now)
    {
      c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
      c3trajectory_t += traj_dt;
    }

    // Check if we will hit something while in trajectory the new trajectory
    geometry_msgs::Pose traj_point;  // Convert messages to correct type
    auto p = c3trajectory->getCurrentPoint();
    traj_point.position = vec2xyz<Point>(p.q.head(3));
    quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);

    std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
        waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);

    if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
    {  // New trajectory will hit an occupied goal, so reject
      ROS_ERROR("can't move there! - bad trajectory");
      current_waypoint = old_trajectory;
      current_waypoint.do_waypoint_validation = false;
      current_waypoint.r.qdot = subjugator::Vector6d::Zero();  // zero velocities
      current_waypoint_t = now;

      c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
      c3trajectory_t = now;
      actionresult.success = false;
      actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
    }

    PoseTwistStamped msg;
    msg.header.stamp = c3trajectory_t;
    msg.header.frame_id = fixed_frame;
    msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
    trajectory_pub.publish(msg);

    waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);

    PoseStamped msgVis;
    msgVis.header = msg.header;
    msgVis.pose = msg.posetwist.pose;
    trajectory_vis_pub.publish(msgVis);

    PoseStamped posemsg;
    posemsg.header.stamp = c3trajectory_t;
    posemsg.header.frame_id = fixed_frame;
    posemsg.pose = Pose_from_Waypoint(current_waypoint);
    waypoint_pose_pub.publish(posemsg);

    if (actionserver.isActive() &&
        c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance),
                                                         max(1e-3, angular_tolerance)) &&
        current_waypoint.r.qdot == subjugator::Vector6d::Zero())
    {
      actionresult.error = "";
      actionresult.success = true;
      actionserver.setSucceeded(actionresult);
    }
  }
예제 #25
0
//this is where the bulk of the work is done, interpolating between potentially coarse joint-space poses
// using the specified arrival times
void trajActionServer::executeCB(const actionlib::SimpleActionServer<baxter_trajectory_streamer::trajAction>::GoalConstPtr& goal) {
    double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
    int isegment;
    trajectory_msgs::JointTrajectoryPoint trajectory_point0;

    Vectorq7x1 qvec, qvec0, qvec_prev, qvec_new;

    //ROS_INFO("in executeCB");
    //ROS_INFO("goal input is: %d", goal->input);

    g_count++; // keep track of total number of goals serviced since this server was started
    result_.return_val = g_count; // we'll use the member variable result_, defined in our class
    //result_.traj_id = goal->traj_id;
    //cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
    // copy trajectory to global var:
    new_trajectory = goal->trajectory; // 
    // insist that a traj have at least 2 pts
    int npts = new_trajectory.points.size();
    if (npts  < 2) {
        ROS_WARN("too few points; aborting goal");
        as_.setAborted(result_);
    } else { //OK...have a valid trajectory goal; execute it
        //got_new_goal = true;
        //got_new_trajectory = true;
        ROS_INFO("Cb received traj w/ npts = %d",npts);
        //cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
        //debug output...
        /*
        cout << "subgoals: " << endl;
        for (int i = 0; i < npts; i++) {
            for (int j = 0; j < 7; j++) { //copy from traj point to 7x1 vector
                cout << new_trajectory.points[i].positions[j] << ", ";
            }
            cout << endl;
        }
        */
        as_.isActive();

        working_on_trajectory = true;

        traj_clock = 0.0; // initialize clock for trajectory;
        isegment = 0; //initialize the segment count
        trajectory_point0 = new_trajectory.points[0]; //start trajectory from first point...should be at least close to
          //current state of system; SHOULD CHECK THIS
        for (int i = 0; i < 7; i++) { //copy from traj point to 7x1 Eigen-type vector
            qvec0[i] = trajectory_point0.positions[i];
        }
        cmd_pose_left(qvec0); //populate and send out first command  
        qvec_prev = qvec0;
        cout << "start pt: " << qvec0.transpose() << endl;
    }
    while (working_on_trajectory) {
        traj_clock += dt_traj;
        // update isegment and qvec according to traj_clock; 
        //if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false          
        working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
        cmd_pose_left(qvec_new); // use qvec to populate object and send it to robot
        qvec_prev = qvec_new;
        //cout << "traj_clock: " << traj_clock << "; vec:" << qvec_new.transpose() << endl;
        ros::spinOnce();
        ros::Duration(dt_traj).sleep(); //update the outputs at time-step resolution specified by dt_traj
    }
    ROS_INFO("completed execution of a trajectory" );
    as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message 
}
	void executeCB(const lasa_action_planners::PLAN2CTRLGoalConstPtr &goal)
	{

		std::string desired_action = goal->action_type;
		ROS_INFO_STREAM( "Desired Action is " << desired_action);

		isOkay = false, isFTOkay = false;
		ros::Rate r(10);
		ROS_INFO("Waiting for EE pose/ft topic...");
		while(ros::ok() && (!isOkay || !isFTOkay)) {
			r.sleep();
		}
		if(!ros::ok()) {
			result_.success = 0;
			ROS_INFO("%s: Failed", action_name_.c_str());
			as_.setAborted(result_);
			return;
		}

		// initialize action progress as null
		feedback_.progress = 0;
		bool success = false;
		///////////////////////////////////////////////
		/////----- EXECUTE REQUESTED ACTION ------/////
		///////////////////////////////////////////////

		if (goal->action_type=="HOME"){
			// TODO: do something meaningful here
			tf::Pose pose(ee_pose);
			success = go_home(pose);
		} else if(goal->action_type == "HOME_POUR") {
			// Home for pouring with lasa robot
			tf::Pose pose(ee_pose);
			pose.setOrigin(tf::Vector3(-0.65, -0.11, 0.474));
			pose.setRotation(tf::Quaternion(-0.006, 0.907, -0.420, -0.114));
			success = go_home(pose);
		}
		if(goal->action_type=="FIND_TABLE"){
			// Go down until table found
			tf::Pose pose(ee_pose);
			success = go_home(ee_pose);
			if(success) {
				success = find_table_for_rolling(goal->height, 0.03, goal->threshold);
			}
		}
		if(goal->action_type=="ROLL_TEST"){

			/**** For now use this instead **/
			tf::Pose p(ee_pose);
			/*********************************/

			success = go_home(p);
			if(success) {
				success = find_table_for_rolling(0.15, 0.03, 5);
				if(success) {
					success = rolling(goal->force, goal->speed, 0.1);
				}
			}
		}
		// Use learned models to do shit
		if(goal->action_type=="LEARNED_MODEL"){
			DoughTaskPhase phase;
			if(goal->action_name == "roll") {
				phase = PHASEROLL;
			} else if(goal->action_name == "reach") {
				phase = PHASEREACH;
			} else if(goal->action_name == "back") {
				phase = PHASEBACK;
			} else if(goal->action_name == "home") {
				phase = PHASEHOME;
			} else {
				ROS_ERROR_STREAM("Unidentified action name "<<goal->action_name.c_str());
				result_.success = 0;
				as_.setAborted(result_);
				return;
			}

			//TODO: update these from action
			double reachingThreshold = 0.02, model_dt = 0.01;
			//CDSController::DynamicsType masterType = CDSController::LINEAR_DYNAMICS;
			CDSController::DynamicsType masterType = CDSController::MODEL_DYNAMICS;
			//CDSController::DynamicsType slaveType = CDSController::LINEAR_DYNAMICS;
			CDSController::DynamicsType slaveType = CDSController::UTHETA;
			tf::Transform trans_obj, trans_att;

			//Little hack for using reaching phase for HOMING				
			if (phase==PHASEHOME){
				     phase=PHASEREACH;
				     homing = true;
			}

			switch (action_mode) {
			case ACTION_BOXY_FIXED:
				/*if(phase == PHASEREACH) {
					trans_obj.setOrigin(tf::Vector3(0.7, -0.43, 0.64)); // TODO: remember to add 0.15 to tf values as well
					trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				} else if(phase == PHASEROLL) {
					trans_obj.setOrigin(tf::Vector3(0.85, -0.43, ee_pose.getOrigin().z()));
					trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				} else if(phase == PHASEBACK) {
					trans_obj.setOrigin(tf::Vector3(0.73, -0.63, 0.84));
					trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				}
				trans_att.setIdentity();*/
				
				trans_obj.setIdentity();
				trans_obj.setOrigin(tf::Vector3(0.8, -0.43, 0.64)); 

				if(phase == PHASEREACH) {
				        trans_att.setOrigin(tf::Vector3(-0.05, 0,0)); // TODO: remember to add 0.15 to tf values as well
					trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				} else if(phase == PHASEROLL) {
					trans_att.setOrigin(tf::Vector3(0.05, 0, 0));
					trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				} else if(phase == PHASEBACK) {
					trans_att.setOrigin(tf::Vector3(-0.15, -0.2, 0.15));
					trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
				}
				break;
			case ACTION_LASA_FIXED:
				if(phase == PHASEREACH) {
					trans_obj.setOrigin(tf::Vector3(-0.55, -0.10, 0.3)); // TODO: remember to add 0.15 to tf values as well (z was 0.15)
					trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
				} else if(phase == PHASEROLL) {
					trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, ee_pose.getOrigin().z()));
					trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
				} else if(phase == PHASEBACK) {
					trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, 0.3)); //(z was 0.15)
					trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
				}
				trans_att.setIdentity();
				break;
			case ACTION_VISION:
				trans_obj.setRotation(tf::Quaternion(goal->object_frame.rotation.x,goal->object_frame.rotation.y,
						goal->object_frame.rotation.z,goal->object_frame.rotation.w));
				trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,
						goal->object_frame.translation.z));

				trans_att.setRotation(tf::Quaternion(goal->attractor_frame.rotation.x,goal->attractor_frame.rotation.y,
						goal->attractor_frame.rotation.z,goal->attractor_frame.rotation.w));
				trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,
						goal->attractor_frame.translation.z));


				if (bWaitForForces){ //HACK FOR BOXY
					// Hack! For setting heights for reach and add offset of roller 
					if(phase == PHASEREACH) {
						trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,goal->attractor_frame.translation.z + 0.1));
					}

					// Hack! safety for rolling
					if(phase == PHASEROLL) {
						trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,ee_pose.getOrigin().getZ()));
						trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y, 0.0));				
					}
				}

				/*if (bWaitForForces){ //HACK FOR LASA
					// Hack! For setting heights for reach and back
					if(phase == PHASEREACH || phase == PHASEBACK) {
						trans_obj.getOrigin().setZ(0.15);
						trans_att.getOrigin().setZ(0.0);
					}

					// Hack! safety for rolling
					if(phase == PHASEROLL) {
						trans_obj.getOrigin().setZ(ee_pose.getOrigin().getZ());
						trans_att.getOrigin().setZ(0.0);
					}
				}*/

				break;
			default:
				break;
			}


			std::string force_gmm_id = goal->force_gmm_id;
			success = learned_model_execution(phase, masterType, slaveType, reachingThreshold,
					model_dt, trans_obj, trans_att, base_path, force_gmm_id);


		}

		result_.success = success;

                homing=false;
		if(success)
		{

			if (!homing){
					ROS_WARN_STREAM("STORE PLOT");
					plot_dyn = 2;
					plot_dyn_msg.data = plot_dyn;	
					pub_plot_dyn_.publish(plot_dyn_msg);
					ros::Rate r(1);
					r.sleep();
			}

			//Wait for message of "plot stored"
			/*ros::Rate wait(1000);
			while(ros::ok() && (plot_published!=1)) {
				ros::spinOnce();
				wait.sleep();
			}*/						
			ROS_INFO("%s: Succeeded", action_name_.c_str());
			as_.setSucceeded(result_);
				

		} else {
			ROS_INFO("%s: Failed", action_name_.c_str());
			as_.setAborted(result_);
		}




	}
예제 #27
0
	void executeCB(const segbot_arm_manipulation::TabletopGraspGoalConstPtr  &goal)
	{
		if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::GRASP){
		
			if (goal->cloud_clusters.size() == 0){
				ROS_INFO("[segbot_tabletop_grap_as.cpp] No object point clouds received...aborting");
				as_.setAborted(result_);
				return;
			}
		
			
			ROS_INFO("[segbot_tabletop_grap_as.cpp] Received action request...proceeding.");
			listenForArmData(40.0);
			
			//the result
			segbot_arm_manipulation::TabletopGraspResult result;
		
			//get the data out of the goal
			Eigen::Vector4f plane_coef_vector;
			for (int i = 0; i < 4; i ++)
				plane_coef_vector(i)=goal->cloud_plane_coef[i];
			int selected_object = goal->target_object_cluster_index;
			
			PointCloudT::Ptr target_object (new PointCloudT);
			pcl::PCLPointCloud2 target_object_pc2;
			pcl_conversions::toPCL(goal->cloud_clusters.at(goal->target_object_cluster_index),target_object_pc2);
			pcl::fromPCLPointCloud2(target_object_pc2,*target_object);
			
			ROS_INFO("[segbot_tabletop_grasp_as.cpp] Publishing point cloud...");
			cloud_grasp_pub.publish(goal->cloud_clusters.at(goal->target_object_cluster_index));
			
			//wait for response at 5 Hz
			listenForGrasps(40.0);
			
			ROS_INFO("[segbot_tabletop_grasp_as.cpp] Heard %i grasps",(int)current_grasps.grasps.size());
		
			//next, compute approach and grasp poses for each detected grasp
			
			//wait for transform from visual space to arm space
			
			std::string sensor_frame_id = goal->cloud_clusters.at(goal->target_object_cluster_index).header.frame_id;
			
			listener.waitForTransform(sensor_frame_id, "mico_link_base", ros::Time(0), ros::Duration(3.0));
			
			
			//here, we'll store all grasp options that pass the filters
			std::vector<GraspCartesianCommand> grasp_commands;
			
			
			for (unsigned int i = 0; i < current_grasps.grasps.size(); i++){
				
							
				GraspCartesianCommand gc_i = segbot_arm_manipulation::grasp_utils::constructGraspCommand(current_grasps.grasps.at(i),HAND_OFFSET_APPROACH,HAND_OFFSET_GRASP, sensor_frame_id);
				
				
				
				//filter 1: if the grasp is too close to plane, reject it
				bool ok_with_plane = segbot_arm_manipulation::grasp_utils::checkPlaneConflict(gc_i,plane_coef_vector,MIN_DISTANCE_TO_PLANE);
				
				//for filter 2, the grasps need to be in the arm's frame of reference
				listener.transformPose("mico_link_base", gc_i.approach_pose, gc_i.approach_pose);
				listener.transformPose("mico_link_base", gc_i.grasp_pose, gc_i.grasp_pose);

				
				//filter 2: apply grasp filter method in request
				bool passed_filter = passesFilter(goal->grasp_filter_method,gc_i);
				
				if (passed_filter && ok_with_plane){
					ROS_INFO("Found grasp fine with filter and plane");
					
					
					
					//filter two -- if IK fails
					moveit_msgs::GetPositionIK::Response  ik_response_approach = segbot_arm_manipulation::computeIK(nh_,gc_i.approach_pose);
					
					if (ik_response_approach.error_code.val == 1){
						moveit_msgs::GetPositionIK::Response  ik_response_grasp = segbot_arm_manipulation::computeIK(nh_,gc_i.grasp_pose);
				
						if (ik_response_grasp.error_code.val == 1){
							
							ROS_INFO("...grasp fine with IK");
							
							
							//now check to see how close the two sets of joint angles are -- if the joint configurations for the approach and grasp poses differ by too much, the grasp will not be accepted
							std::vector<double> D = segbot_arm_manipulation::getJointAngleDifferences(ik_response_approach.solution.joint_state, ik_response_grasp.solution.joint_state);
							
							double sum_d = 0;
							for (int p = 0; p < D.size(); p++){
								sum_d += D[p];
							}
						
							
							if (sum_d < ANGULAR_DIFF_THRESHOLD){
								//ROS_INFO("Angle diffs for grasp %i: %f, %f, %f, %f, %f, %f",(int)grasp_commands.size(),D[0],D[1],D[2],D[3],D[4],D[5]);
								
								//ROS_INFO("Sum diff: %f",sum_d);
							
								//store the IK results
								gc_i.approach_q = ik_response_approach.solution.joint_state;
								gc_i.grasp_q = ik_response_grasp.solution.joint_state;
								
								grasp_commands.push_back(gc_i);
								
								ROS_INFO("...fine with continuity");
							}
						}
					}
				}
			}
			
			//check to see if all potential grasps have been filtered out
			if (grasp_commands.size() == 0){
				ROS_WARN("[segbot_tabletop_grasp_as.cpp] No feasible grasps found. Aborting.");
				as_.setAborted(result_);
				return;
			}
			
			//make sure we're working with the correct tool pose
			listenForArmData(30.0);
			
			int selected_grasp_index = -1;
			
			
			
			if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_ORIENTATION_SELECTION){
				//find the grasp with closest orientatino to current pose
				double min_diff = 1000000.0;
				for (unsigned int i = 0; i < grasp_commands.size(); i++){
					double d_i = segbot_arm_manipulation::grasp_utils::quat_angular_difference(grasp_commands.at(i).approach_pose.pose.orientation, current_pose.pose.orientation);
					
					ROS_INFO("Distance for pose %i:\t%f",(int)i,d_i);
					if (d_i < min_diff){
						selected_grasp_index = (int)i;
						min_diff = d_i;
					}
				}
			}
			else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::RANDOM_SELECTION){

				srand (time(NULL));
				selected_grasp_index = rand() % grasp_commands.size(); 
				ROS_INFO("Randomly selected grasp = %i",selected_grasp_index);     
			}
			else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_JOINTSPACE_SELECTION){
				
				double min_diff = 1000000.0;
				for (unsigned int i = 0; i < grasp_commands.size(); i++){
					std::vector<double> D_i = segbot_arm_manipulation::getJointAngleDifferences(grasp_commands.at(i).approach_q, current_state);
					
					double sum_d = 0;
					for (int p = 0; p < D_i.size(); p++)
						sum_d += D_i[p];
					
					if (sum_d < min_diff){
						selected_grasp_index = (int)i;
						min_diff = sum_d;
					}
				}
				
				
				
			}
			
			if (selected_grasp_index == -1){
				ROS_WARN("[segbot_tabletop_grasp_as.cpp] Grasp selection failed. Aborting.");
				as_.setAborted(result_);
				return;
			}
			
			//compute RPY for target pose
			ROS_INFO("Selected approach pose:");
			ROS_INFO_STREAM(grasp_commands.at(selected_grasp_index).approach_pose);

			//publish individual pose for visualization purposes
			pose_pub.publish(grasp_commands.at(selected_grasp_index).approach_pose);
			
			//close fingers while moving
			segbot_arm_manipulation::closeHand();
			
			//move to approach pose -- do it twice to correct 
			segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
			segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
			
			//open fingers
			segbot_arm_manipulation::openHand();
		
			//move to grasp pose
			segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).grasp_pose);
		
			//close hand
			segbot_arm_manipulation::closeHand();
			
			result_.success = true;
			as_.setSucceeded(result_);
			return;
		}
		else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER){
			//TO DO: move to handover position
			
			ROS_INFO("Starting handover action...");
			
			//listen for haptic feedback
			
			double rate = 40;
			
			ros::Rate r(rate);

			double total_grav_free_effort = 0;
			double total_delta;
			double delta_effort[6];

			listenForArmData(40.0);
			sensor_msgs::JointState prev_effort_state = current_effort;


			double elapsed_time = 0;

			while (ros::ok()){
		
				ros::spinOnce();
				
				total_delta=0.0;
				for (int i = 0; i < 6; i ++){
					delta_effort[i] = fabs(current_effort.effort[i]-prev_effort_state.effort[i]);
					total_delta+=delta_effort[i];
					ROS_INFO("Total delta=%f",total_delta);
				}
				
				if (total_delta > fabs(FORCE_HANDOVER_THRESHOLD)){
					ROS_INFO("[segbot_tabletop_grasp_as.cpp] Force detected");
					
					//now open the hand
					segbot_arm_manipulation::openHand();
					
					result_.success = true;
					as_.setSucceeded(result_);
					return;
					
				}
				
				r.sleep();
				elapsed_time+=(1.0)/rate;
				
				if (goal->timeout_seconds > 0 && elapsed_time > goal->timeout_seconds){
					
					ROS_WARN("Handover action timed out...");
					
					result_.success = false;
					as_.setAborted(result_);
					
					return;
				}
			}
			
			
			
			result_.success = true;
			as_.setSucceeded(result_);
		}
		else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER_FROM_HUMAN){
			//open fingers
			segbot_arm_manipulation::openHand();
			
			//update readings
			listenForArmData(40.0);
			
			bool result = waitForForce(FORCE_HANDOVER_THRESHOLD,goal->timeout_seconds);
		
			if (result){
				segbot_arm_manipulation::closeHand();
				result_.success = true;
				as_.setSucceeded(result_);
			}
			else {
				result_.success = false;
				as_.setAborted(result_);
			}
		
		}
	
	
	}
    void executeActionCB(const OrientToBaseGoalConstPtr& goal) {

        BaseScanLinearRegression srv;

        double min_angle = -M_PI/8.0, max_angle=M_PI/8.0, min_dist=0.02, max_dist=0.8;
        nh_.getParamCached("/laser_linear_regression/min_angle", min_angle);
        nh_.getParamCached("/laser_linear_regression/max_angle", max_angle);
        nh_.getParamCached("/laser_linear_regression/min_dist", min_dist);
        nh_.getParamCached("/laser_linear_regression/max_dist", max_dist);

        srv.request.filter_minAngle = angles::from_degrees(min_angle);
        srv.request.filter_maxAngle = angles::from_degrees(max_angle);
        srv.request.filter_minDistance = min_dist;
        srv.request.filter_maxDistance = max_dist;
        std::cout << "min_dist " << min_dist << ", max_dist " << max_dist << std::endl;
        //srv.request.filter_minAngle = -M_PI / 8.0;
        //srv.request.filter_maxAngle = M_PI / 8.0;
        //srv.request.filter_minDistance = 0.02;
        //srv.request.filter_maxDistance = 0.80;

        target_distance = goal->distance;

        ros::Duration max_time(50.0);
        ros::Time stamp = ros::Time::now();
        OrientToBaseResult result;
        bool oriented = false;
        int  iterator = 0;

        while (ros::ok()) {
            ROS_INFO("Call service Client");

            if(client.call(srv)) {
                ROS_INFO("Called service LaserScanLinearRegressionService");
                //std::cout << "result: " << srv.response.center << ", " << srv.response.a << ", " << srv.response.b << std::endl;


                geometry_msgs::Twist cmd = calculateVelocityCommand(srv.response.center, srv.response.a, srv.response.b,oriented,iterator);
                cmd_pub.publish(cmd);

                std::cout << "cmd x:" << cmd.linear.x << ", y: "  << cmd.linear.y << ", z: " << cmd.angular.z << std::endl;

                if ((fabs(cmd.angular.z)  + fabs(cmd.linear.x) ) < 0.001) {

                    ROS_INFO("Point reached");
                    result.succeed = true;
                    as_.setSucceeded(result);
                    geometry_msgs::Twist zero_vel;
                    cmd_pub.publish(zero_vel);
                    break;

                }

                if  (stamp + max_time < ros::Time::now()) {
                    result.succeed = false;
                    as_.setAborted(result);
                    geometry_msgs::Twist zero_vel;
                    cmd_pub.publish(zero_vel);
                    break;
                }

            } else {
                ROS_ERROR("Failed to call service LaserScanLinearRegressionService");

                if  (stamp + max_time < ros::Time::now()) {
                    result.succeed = false;
                    as_.setAborted(result);
                    break;
                }
            }
        }

    }
void ArmMotionInterface::executeCB(const actionlib::SimpleActionServer<cwru_action::cwru_baxter_cart_moveAction>::GoalConstPtr& goal) {
    ROS_INFO("in executeCB of ArmMotionInterface");
    cart_goal_ = *goal; // copy of goal held in member var
    command_mode_ = goal->command_code;
    ROS_INFO_STREAM("received command mode " << command_mode_);
    int njnts;

    switch (command_mode_) {
        case cwru_action::cwru_baxter_cart_moveGoal::ARM_TEST_MODE:
            ROS_INFO("responding to request TEST_MODE: ");
            cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
            cart_move_as_.setSucceeded(cart_result_);
            break;
        //looks up current right-arm joint angles and returns them to client
        case cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_Q_DATA:
            ROS_INFO("responding to request RT_ARM_GET_Q_DATA");
            get_right_arm_joint_angles(); //will update q_vec_right_arm_Xd_
            cart_result_.q_arm_right.resize(7);
            for (int i=0;i<7;i++) {
                cart_result_.q_arm_right[i] = q_vec_right_arm_Xd_[i];
            }
                cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
                cart_move_as_.setSucceeded(cart_result_);            
            break;
            
        case  cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_TOOL_POSE:  
            ROS_INFO("responding to request RT_ARM_GET_TOOL_POSE");
            compute_right_tool_stamped_pose();
            cart_result_.current_pose_gripper_right = current_gripper_stamped_pose_right_;
            cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
            cart_move_as_.setSucceeded(cart_result_);            
           break;
        //prepares a trajectory plan to move arm from current pose to pre-defined pose
        case cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_PRE_POSE:
            ROS_INFO("responding to request RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_PRE_POSE");
            q_start_Xd_ = get_jspace_start_right_arm_();
            //q_start=q_start_Xd; // convert to fixed-size vector;
            plan_jspace_path_qstart_to_qend(q_start_Xd_, q_pre_pose_Xd_);
            busy_working_on_a_request_ = false;
            break;

        case cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL:
            ROS_INFO("responding to request RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL");
            q_start_Xd_ = get_jspace_start_right_arm_();
            q_goal_pose_Xd_.resize(7);
            njnts = goal->q_goal_right.size();
            if (njnts!=7) {
                ROS_WARN("joint-space goal is wrong dimension");
                cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
            }
            else {
                for (int i=0;i<7;i++) q_goal_pose_Xd_[i] = goal->q_goal_right[i];
                //q_start=q_start_Xd; // convert to fixed-size vector;
                plan_jspace_path_qstart_to_qend(q_start_Xd_, q_goal_pose_Xd_);
                busy_working_on_a_request_ = false;   
            }
            break;
        case cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_POSE:
            rt_arm_plan_path_current_to_goal_pose();
            break;
            
        case  cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_DP_XYZ:
            rt_arm_plan_path_current_to_goal_dp_xyz();
            break;
            
        //consults a pre-computed trajectory and invokes execution;
        case cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_EXECUTE_PLANNED_PATH: //assumes there is a valid planned path in optimal_path_
            ROS_INFO("responding to request RT_ARM_EXECUTE_PLANNED_PATH");
            execute_planned_move();
            break;

        default:
            ROS_WARN("this command mode is not defined: %d", command_mode_);
            cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::COMMAND_CODE_NOT_RECOGNIZED;
            cart_move_as_.setAborted(cart_result_); // tell the client we have given up on this goal; send the result message as well
    }
}
  void executeCB(const corobot_face_recognition::FaceRecognitionGoalConstPtr &goal)
  {
    if (_as.isPreemptRequested() || !ros::ok()) {
      ROS_INFO("%s: Preempted", _action_name.c_str());
      // set the action state to preempted
      _as.setPreempted();
      // success = false;
      // break;
    }
    
    // helper variables
    ros::Rate r(60);
    bool success = true;

    _goal_id = goal->order_id;
    _goal_argument = goal->order_argument;

    _feedback.order_id = _goal_id;
    _feedback.names.clear();
    _feedback.confidence.clear();
    
    _result.order_id = _goal_id;
    _result.names.clear();
    _result.confidence.clear();

    // publish info to the console for the user
    ROS_INFO("%s: Executing. order_id: %i, order_argument: %s ", _action_name.c_str(), _goal_id, _goal_argument.c_str());

    switch (_goal_id) {
      // RECOGNIZE ONCE
      case 0:
        // cout << "case 0" << endl;
        // _fr = new FaceRec(true);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();

        break;
      
      // RECOGNIZE CONTINUOUSLY
      case 1:
        //cout << "case 1" << endl;
        // _fr = new FaceRec(true);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::recognizeCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();
        
        break;
      
      // ADD TRAINING IMAGES
      case 2:
        // cout << "case 2" << endl;
        _fc = new FaceImgCapturer(_goal_argument);
        _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::addTrainingImagesCb, this);
        
        while( _as.isActive() && !_as.isPreemptRequested() && !ros::isShuttingDown() )
          r.sleep();

        break;
      
      // TRAIN DATABASE
      case 3:
        // call training function and check if successful
        // bool trainingSuccessful = true;
        // cout << "case 3" << endl;
        _fr->train(_preprocessing);
        if (true) 
          _as.setSucceeded(_result);
        else 
          _as.setAborted(_result);
        break;
      
      // STOP
      case 4:
        if (_as.isActive()) {
          _as.setPreempted();
          // _image_sub.shutdown();
        }
        break;

      // EXIT
      case 5:
        // cout << "case 4" << endl;
        ROS_INFO("%s: Exit request.", _action_name.c_str());
        _as.setSucceeded(_result);
        r.sleep();
        ros::shutdown();
        break;
    }

  }