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");
                }
            }
        }
    }
예제 #2
0
void FaceClient::doneCb(const actionlib::SimpleClientGoalState& state,
	    const face_recognition::FaceRecognitionResultConstPtr& result)
{
  ROS_INFO("Goal [%i] Finished in state [%s]", result->order_id,state.toString().c_str());
  if(state.toString() != "SUCCEEDED") return;
  if( result->order_id==0)
    ROS_INFO("%s was recognized with confidence %f", result->names[0].c_str(),result->confidence[0]);
}
void doneCb(const actionlib::SimpleClientGoalState& state, const asctec_hl_comm::WaypointResultConstPtr & result)
{
  if (state.state_ == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Finished in state [%s]", state.toString().c_str());
  else
    ROS_WARN("Finished in state [%s]", state.toString().c_str());

  const geometry_msgs::Point32 & wp = result->result_pos;
  ROS_INFO("Reached waypoint: %fm %fm %fm %f°",wp.x, wp.y, wp.z, result->result_yaw*180/M_PI);
}
예제 #4
0
// Called once when the goal completes
void done2Cb(const actionlib::SimpleClientGoalState& state,
            const bbauv_msgs::ControllerResultConstPtr& result)
{
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  //ROS_INFO("Answer: %i", result->sequence.back());
  ros::shutdown();
}
 void doneCb(const actionlib::SimpleClientGoalState& state,
             const FibonacciResultConstPtr& result)
 {
   ROS_INFO("Finished in state [%s]", state.toString().c_str());
   ROS_INFO("Answer: %i", result->sequence.back());
   ros::shutdown();
 }
void doneCb(const actionlib::SimpleClientGoalState& state,
		const watermellon::wm_navigation_alResultConstPtr& result)
{
	ROS_INFO("Finished in state [%s]", state.toString().c_str());
	ROS_INFO("Answer: %s", result->finished? "True": "False");
	ros::shutdown();
}
void rightArmDoneCb(const actionlib::SimpleClientGoalState& state,
        const control_msgs::FollowJointTrajectoryResultConstPtr& result) {
    ROS_INFO(" rtArmDoneCb: server responded with state [%s]", state.toString().c_str());
    g_done_move = true;
    //ROS_INFO("got return val = %d", result->return_val);
    g_done_count++;
}
// This function will be called once when the goal completes
void ArmMotionCommander::doneCb_(const actionlib::SimpleClientGoalState& state,
                                 const cwru_action::cwru_baxter_cart_moveResultConstPtr& result)
{
    ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
    ROS_INFO("got return value= %d", result->return_code);
    cart_result_=*result;
}
void doneCb(const actionlib::SimpleClientGoalState& state,
        const coordinator::ManipTaskResultConstPtr& result) {
    ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
    g_goal_done = true;
    g_result = *result;
    g_callback_status = result->manip_return_code;

    switch (g_callback_status) {
        case coordinator::ManipTaskResult::MANIP_SUCCESS:
            ROS_INFO("returned MANIP_SUCCESS");
            
            break;
            
        case coordinator::ManipTaskResult::FAILED_PERCEPTION:
            ROS_WARN("returned FAILED_PERCEPTION");
            g_object_finder_return_code = result->object_finder_return_code;
            break;
        case coordinator::ManipTaskResult::FAILED_PICKUP:
            ROS_WARN("returned FAILED_PICKUP");
            g_object_grabber_return_code= result->object_grabber_return_code;
            g_object_pose = result->object_pose;
            //g_des_flange_pose_stamped_wrt_torso = result->des_flange_pose_stamped_wrt_torso;
            break;
        case coordinator::ManipTaskResult::FAILED_DROPOFF:
            ROS_WARN("returned FAILED_DROPOFF");
            //g_des_flange_pose_stamped_wrt_torso = result->des_flange_pose_stamped_wrt_torso;          
            break;
    }
}
예제 #10
0
// This function will be called once when the goal completes
// this is optional, but it is a convenient way to get access to the "result" message sent by the server
void doneCb(const actionlib::SimpleClientGoalState& state,
        const example_action_server::demoResultConstPtr& result) {
    ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
    ROS_INFO("got result output = %d",result->output);
    g_result_output= result->output;
    g_goal_active=false;
}
예제 #11
0
void doneCb(const actionlib::SimpleClientGoalState& status,
            const kobuki_msgs::AutoDockingResultConstPtr& result)
{

	ROS_INFO("Finished in state [%s]", status.toString().c_str());
	ROS_INFO("Answer: %s", result->text.c_str());
	//ros::shutdown();
}
			// Called once when the goal completes
			virtual void doneCb(const actionlib::SimpleClientGoalState& state, const typename Result::ConstPtr& result)
			{
				if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
				{
					ROS_ERROR("Finished in state [%s]", state.toString().c_str());
					publishEventString("/PRIMITIVE_FINISHED");
				}
			}
예제 #13
0
// Called once when the goal completes
void FaceDetectionClient::doneCb(const actionlib::SimpleClientGoalState &state,
                const face_detection::identifyResultConstPtr &result)
	{
		ROS_INFO("Finished in state [%s]", state.toString().c_str());
		user = result->users_identified;
		xPos = result->x;
		yPos = result->y;
		goalCompleted = true;
		ros::shutdown();
	}
예제 #14
0
void doneCb(const actionlib::SimpleClientGoalState& state,
        const gazebo_action_motion_ctrl::pathResultConstPtr& result) {
    ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
    g_curr_pose=result->final_pose;
    g_is_done=true;
    was_canceled=false;
    if(g_curr_pose.position.x>blocked_x){
        blocked_x=g_curr_pose.position.x;
        was_blocked_y=false;
    }
}
예제 #15
0
void TaskActionServer::objectGrabberDoneCb_(const actionlib::SimpleClientGoalState& state,
        const object_grabber::object_grabberResultConstPtr& result) {
    ROS_INFO(" objectGrabberDoneCb: server responded with state [%s]", state.toString().c_str());
        
    ROS_INFO("got result output = %d; ", result->return_code);

    //result_.des_flange_pose_stamped_wrt_torso = result->des_flange_pose_stamped_wrt_torso;
    
    //pass return code back to the client    
    result_.object_grabber_return_code = object_grabber_return_code_;
    object_grabber_return_code_ = result->return_code; //put this last, to avoid race condition
}
 void pickAndPlace(const actionlib::SimpleClientGoalState& state, const InteractiveBlockManipulationResultConstPtr& result)
 {
   ROS_INFO("Got interactive marker callback. Picking and placing.");
   
   if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
     ROS_INFO("Succeeded!");
   else
   {
     ROS_INFO("Did not succeed! %s",  state.toString().c_str());
     ros::shutdown();
   }
   pick_and_place_action_.sendGoal(pick_and_place_goal_, boost::bind( &BlockManipulationAction::finish, this, _1, _2));
 }
 void 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 navigatorDoneCb(const actionlib::SimpleClientGoalState& state,
        const navigator::navigatorResultConstPtr& result) {
    ROS_INFO(" navigatorDoneCb: server responded with state [%s]", state.toString().c_str());
    g_navigator_rtn_code=result->return_code;
    ROS_INFO("got object code response = %d; ",g_navigator_rtn_code);
    if (g_navigator_rtn_code==navigator::navigatorResult::DESTINATION_CODE_UNRECOGNIZED) {
        ROS_WARN("destination code not recognized");
    }
    else if (g_navigator_rtn_code==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
        ROS_INFO("reached desired location!");
    }
    else {
        ROS_WARN("desired pose not reached!");
    }
}
 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();
 }
void InteractiveObjectDetectionFrame::userCmdDone(const actionlib::SimpleClientGoalState& state,
                                                  const UserCommandResultConstPtr& result)
{
  boost::mutex::scoped_lock l1( mutex_ );

  ROS_INFO("Action finished in state [%s]", state.toString().c_str());
  action_requested_ = false;
  status_ = "[" + state.toString() + "]";

  switch (user_command_goal_.request)
  {
    case UserCommandGoal::SEGMENT:
      seg_status_ = state.getText();
      break;
    case UserCommandGoal::RECOGNIZE:
      rec_status_ = state.getText();
      break;
    case UserCommandGoal::DETECT:
      det_status_ = state.getText();
      break;    
    default:
      break;
  }
}
void objectFinderDoneCb(const actionlib::SimpleClientGoalState& state,
        const object_finder_gamma::objectFinderResultConstPtr& result) {
    ROS_INFO(" objectFinderDoneCb: server responded with state [%s]", state.toString().c_str());
    g_found_object_code=result->found_object_code;
    ROS_INFO("got object code response = %d; ",g_found_object_code);
    if (g_found_object_code == object_finder_gamma::objectFinderResult::OBJECT_CODE_NOT_RECOGNIZED) {
        ROS_WARN("object code not recognized");
    }
    else if (g_found_object_code == object_finder_gamma::objectFinderResult::OBJECT_FOUND) {
        ROS_INFO("found object!");
         g_perceived_object_pose= result->object_pose;
         ROS_INFO("got pose x,y,z = %f, %f, %f",g_perceived_object_pose.pose.position.x,
                 g_perceived_object_pose.pose.position.y,
                 g_perceived_object_pose.pose.position.z);
    }
    else {
        ROS_WARN("object not found!");
    }
}
    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;

    }
예제 #23
0
void TaskActionServer::objectFinderDoneCb_(const actionlib::SimpleClientGoalState& state,
        const object_finder::objectFinderResultConstPtr& result) {
    ROS_INFO(" objectFinderDoneCb: server responded with state [%s]", state.toString().c_str());
    //found_object_code_ = result->found_object_code;
    ROS_INFO("got object code response = %d; ", result->found_object_code);
    if (result->found_object_code == object_finder::objectFinderResult::OBJECT_CODE_NOT_RECOGNIZED) {
        ROS_WARN("object code not recognized");
    } else if (result->found_object_code == object_finder::objectFinderResult::OBJECT_FOUND) {
        ROS_INFO("found object!");
        pickup_pose_ = result->object_pose;
        result_.object_pose = pickup_pose_;
        ROS_INFO("got pose x,y,z = %f, %f, %f", pickup_pose_.pose.position.x,
                pickup_pose_.pose.position.y,
                pickup_pose_.pose.position.z);
        pose_publisher_.publish(pickup_pose_);
    } else {
        ROS_WARN("object not found!");
    }
    //put this last--possible race condition with main!
    // pass this code back to the client
    result_.object_finder_return_code = found_object_code_;
    found_object_code_ = result->found_object_code;
}
예제 #24
0
void objectGrabberDoneCb(const actionlib::SimpleClientGoalState& state,
        const object_grabber::object_grabberResultConstPtr& result) {
    ROS_INFO(" objectGrabberDoneCb: server responded with state [%s]", state.toString().c_str());
    ROS_INFO("got result output = %d; ", result->return_code);
    g_object_grabber_return_code = result->return_code;
}
예제 #25
0
파일: main.cpp 프로젝트: yixincai/ROS
// Navigation stack goal completed
void doneCb(const actionlib::SimpleClientGoalState& astate, const move_base_msgs::MoveBaseResultConstPtr& result)
{
	// Nav stack goal finished
	ROS_INFO("Nav action client finished in state [%s]", astate.toString().c_str());
	state = RobotState::FINISHED;
}
예제 #26
0
void CPtuClient::doneCb(const actionlib::SimpleClientGoalState& state,const scitos_apps_msgs::ChargingResultConstPtr& result)
{
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  ROS_INFO("Answer: %s",result->Message.c_str());
}
예제 #27
0
void doneCb(const actionlib::SimpleClientGoalState& state,const move_base_msgs::MoveBaseActionResultConstPtr& result)
{
  printf("Finished in state [%s]\n", state.toString().c_str());
//  printf("Answer: %s\n",result->Message.c_str());
  ros::shutdown();
}
예제 #28
0
/**
 * Callback function
 */
void doneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) {
    ROS_INFO("Finished in state [%s]", state.toString().c_str());
}
// This function will be called once when the goal completes
// this is optional, but it is a convenient way to get access to the "result" message sent by the server
void doneCb(const actionlib::SimpleClientGoalState& state,
        const davinci_traj_streamer::trajResultConstPtr& result) {
    ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
    ROS_INFO("got return val = %d; traj_id = %d",result->return_val,result->traj_id);
}
void navigatorDoneCb(const actionlib::SimpleClientGoalState& state,
        const move_base_msgs::MoveBaseResultConstPtr& result) {
    ROS_INFO(" navigatorDoneCb: server responded with state [%s]", state.toString().c_str());
}