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");
}
}
}
}
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);
}
// 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;
}
}
// 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;
}
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");
}
}
// 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();
}
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;
}
}
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;
}
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;
}
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;
}
// 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;
}
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());
}
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();
}
/**
* 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());
}
