/*!
* \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();
}
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_);
}
}
/*!
* \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
}
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");
}
}
}
}
//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;
}
/**
* @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;
}
}
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];
}
}
//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.");
}
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_);
}
}
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
}
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);
}
}
//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_);
}
}
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;
}
}