Go to the documentation of this file.00001 #include "tidyup_actions/actionExecutorPickupSponge.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_pickup_sponge,
00006 tidyup_actions::ActionExecutorPickupSponge,
00007 continual_planning_executive::ActionExecutorInterface)
00008
00009 namespace tidyup_actions
00010 {
00011
00012 bool ActionExecutorPickupSponge::fillGoal(std_srvs::Empty::Request & goal,
00013 const DurativeAction & a, const SymbolicState & current)
00014 {
00015 if(!PlanningSceneInterface::instance()->resetPlanningScene())
00016 ROS_ERROR("%s: PlanningScene reset failed.", __PRETTY_FUNCTION__);
00017
00018 return true;
00019 }
00020
00021 void ActionExecutorPickupSponge::updateState(bool success,
00022 std_srvs::Empty::Response & response,
00023 const DurativeAction & a, SymbolicState & current)
00024 {
00025 ROS_INFO("PickupSponge returned result");
00026 ROS_ASSERT(a.parameters.size() == 1);
00027 string arm = a.parameters[0];
00028
00029 if(success) {
00030 ROS_INFO("picking up sponge with %s succeeded.", arm.c_str());
00031 current.setBooleanPredicate("grasped-sponge", arm, true);
00032
00033
00034 const char* ATTACH_SERVICE = "/tidyup/attach_sponge";
00035 if(!ros::service::exists(ATTACH_SERVICE, true)) {
00036 ROS_ERROR("%s: Service %s does not exist.", __func__, ATTACH_SERVICE);
00037 }
00038 std_srvs::Empty srv;
00039 if(!ros::service::call(ATTACH_SERVICE, srv)) {
00040 ROS_ERROR("%s: Service call to %s failed.", __func__, ATTACH_SERVICE);
00041 }
00042 } else {
00043 ROS_ERROR("picking up sponge with %s FAILED.", arm.c_str());
00044 }
00045
00046 current.setObjectFluent("arm-state", arm, "arm_unknown");
00047 }
00048
00049 };
00050