Go to the documentation of this file.00001 #include "tidyup_actions/actionExecutorArmToCarry.h"
00002 #include <pluginlib/class_list_macros.h>
00003 #include "tidyup_utils/planning_scene_interface.h"
00004
00005 PLUGINLIB_DECLARE_CLASS(tidyup_actions, action_executor_arm_to_carry,
00006 tidyup_actions::ActionExecutorArmToCarry,
00007 continual_planning_executive::ActionExecutorInterface)
00008
00009 namespace tidyup_actions
00010 {
00011
00012 void ActionExecutorArmToCarry::initialize(const std::deque<std::string> & arguments)
00013 {
00014 ActionExecutorActionlib<tidyup_msgs::PostGraspPositionAction, tidyup_msgs::PostGraspPositionGoal,
00015 tidyup_msgs::PostGraspPositionResult>::initialize(arguments);
00016
00017 _armStatePredicateName = "arm-state";
00018 _armAtCarryConstantName = "arm_carrying";
00019
00020 if(arguments.size() >= 3) {
00021 _armStatePredicateName = arguments[2];
00022 }
00023 if(arguments.size() >= 4) {
00024 _armAtCarryConstantName = arguments[3];
00025 }
00026 }
00027
00028 bool ActionExecutorArmToCarry::fillGoal(tidyup_msgs::PostGraspPositionGoal & goal,
00029 const DurativeAction & a, const SymbolicState & current)
00030 {
00031 if(!PlanningSceneInterface::instance()->resetPlanningScene())
00032 ROS_ERROR("%s: PlanningScene reset failed.", __PRETTY_FUNCTION__);
00033
00034 goal.left_arm = false;
00035 goal.right_arm = false;
00036
00037 ROS_ASSERT(a.parameters.size() == 1);
00038 string arm = a.parameters[0];
00039 if(arm == "left_arm") {
00040 goal.left_arm = true;
00041 }
00042 if(arm == "right_arm") {
00043 goal.right_arm = true;
00044 }
00045
00046 return (goal.left_arm || goal.right_arm);
00047 }
00048
00049 void ActionExecutorArmToCarry::updateState(const actionlib::SimpleClientGoalState & actionReturnState,
00050 const tidyup_msgs::PostGraspPositionResult & result,
00051 const DurativeAction & a, SymbolicState & current)
00052 {
00053 ROS_INFO("PostGraspPosition returned result: %s", result.result.c_str());
00054 ROS_ASSERT(a.parameters.size() == 1);
00055 string arm = a.parameters[0];
00056 if(actionReturnState == actionlib::SimpleClientGoalState::SUCCEEDED) {
00057 ROS_INFO("Post Grasp Position succeeded.");
00058 current.setObjectFluent(_armStatePredicateName, arm, _armAtCarryConstantName);
00059 } else {
00060 current.setObjectFluent(_armStatePredicateName, arm, "arm_unknown");
00061 }
00062 }
00063
00064 };
00065