bool moveArmToAbove ( bool rightArm )
{
bool result = true;
if (rightArm == true)
{
arm_navigation_msgs::MoveArmGoal rightArmGoal;
initRightArmGoalAbove(&rightArmGoal);
//move right arm
result = moveRightArm (&rightArmGoal);
}
else
{
arm_navigation_msgs::MoveArmGoal leftArmGoal;
initLeftArmGoalAbove(&leftArmGoal);
//move left arm
result = moveLeftArm (&leftArmGoal);
}
return result;
}
void UserController::choice_movement(std::string room)
{
int task_right = 0;
int task_left = 0;
int task_head = 0;
int task_body = 0;
if (room == "lobby")
{
task_left = 0;
task_right = 1;
task_head = 2;
task_body = 0;
}
else if (room == "kitchen")
{
task_left = 2;
task_right = 0;
task_head = 3;
task_body = 0;
}
else if (room == "living room")
{
task_left = 1;
task_right = 0;
task_head = 4;
task_body = 0;
}
else if (room == "bed room")
{
task_left = 0;
task_right = 2;
task_head = 1;
task_body = 0;
}
else if (room == "finish")
{
task_left = 0;
task_right = 0;
task_head = 0;
task_body = 1;
}
else
{
task_left = 0;
task_right = 0;
task_head = 0;
task_body = 0;
}
moveLeftArm(task_left);
moveRightArm(task_right);
moveHead(task_head);
moveBody(task_body);
}
//TODO rename to moveArmSingle ( bool rightArm )
bool moveRightArm ( void )
{
bool result = true;
arm_navigation_msgs::MoveArmGoal rightArmGoal;
initRightArmGoalSingle(&rightArmGoal);
//move right arm
result &= moveRightArm (&rightArmGoal);
return result;
}
bool moveBothArms ( void )
{
bool result = true;
arm_navigation_msgs::MoveArmGoal leftArmGoal;
arm_navigation_msgs::MoveArmGoal rightArmGoal;
initBothArmGoals(&leftArmGoal, &rightArmGoal);
//move left arm first
result = moveLeftArm (&leftArmGoal);
//move right arm
result &= moveRightArm (&rightArmGoal);
return result;
}
void executeCB(const race_move_arms::CarryarmGoalConstPtr &goal)
{
// make sure that the action hasn't been canceled
if (!as_.isActive())
return;
bool success = true;
bool rightArm = true; // right arm
ROS_INFO("Executing, creating carryarm action");
// start executing the action
if (goal->carrypose == 1) // both arms are not in carrypose!
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
success = moveRightArm();
break;
case 1:
ROS_INFO("Using left arm");
success = moveLeftArm();
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
success = moveArmToAbove(rightArm);
success &= moveBothArms();
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 2) // the other arm is already in carrypose!
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
success = moveBothArms(); // trivial case
break;
case 1:
ROS_INFO("Using left arm");
//move right arm to side first
rightArm = true;
success = moveArmToAbove(rightArm);
success &= moveBothArms(); // trivial case
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 3) // move arm to above
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
rightArm = true;
success = moveArmToAbove(rightArm);
break;
case 1:
ROS_INFO("Using left arm");
rightArm = false;
success = moveArmToAbove(rightArm);
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
success = moveArmToAbove(rightArm);
rightArm = false;
success &= moveArmToAbove(rightArm);
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
else if (goal->carrypose == 4) // move arm to side
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
}
else
{
ROS_INFO("Executing arm trajectory..");
switch (goal->carryarm)
{
case 0:
ROS_INFO("Using right arm");
rightArm = true;
//success = moveArmToAbove(rightArm);
success = moveArmToSide(rightArm);
break;
case 1:
ROS_INFO("Using left arm");
rightArm = true;
success = moveArmToAbove(rightArm);
rightArm = false;
//success = moveArmToAbove(rightArm);
success &= moveArmToSide(rightArm);
success &= moveRightArm();
break;
case 2:
ROS_INFO("Using both arms");
rightArm = true;
//success = moveArmToAbove(rightArm);
success = moveArmToSide(rightArm);
rightArm = false;
//success &= moveArmToAbove(rightArm);
success &= moveArmToSide(rightArm);
break;
default:
ROS_INFO("Unknown carryarm command, ignoring");
break;
}
}
}
if(success)
{
result_.result = 1;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
else
{
result_.result = 0;
ROS_INFO("%s: Failed", action_name_.c_str());
// set the action state to failed
//as_.setSucceeded(result_);
//as_.setAborted(result_);
as_.setAborted(result_);
}
}
