Go to the documentation of this file.00001 #include "tidyup_actions/actionExecutorDetectDoorState.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_detect_door_state,
00006 tidyup_actions::ActionExecutorDetectDoorState,
00007 continual_planning_executive::ActionExecutorInterface)
00008
00009 namespace tidyup_actions
00010 {
00011
00012 void ActionExecutorDetectDoorState::initialize(const std::deque<std::string> & arguments)
00013 {
00014 ActionExecutorService<tidyup_msgs::DetectDoorState>::initialize(arguments);
00015 }
00016
00017 bool ActionExecutorDetectDoorState::fillGoal(tidyup_msgs::DetectDoorState::Request & goal,
00018 const DurativeAction & a, const SymbolicState & current)
00019 {
00020 if(!PlanningSceneInterface::instance()->resetPlanningScene())
00021 ROS_ERROR("%s: PlanningScene reset failed.", __PRETTY_FUNCTION__);
00022
00023 return true;
00024 }
00025
00026 void ActionExecutorDetectDoorState::updateState(bool success, tidyup_msgs::DetectDoorState::Response & response,
00027 const DurativeAction & a, SymbolicState & current)
00028 {
00029 ROS_INFO("DetectDoorState returned result");
00030 if(success) {
00031 ROS_INFO("DetectDoorState succeeded, door_open is: %d.", response.door_open);
00032 ROS_ASSERT(a.parameters.size() == 2);
00033 string location = a.parameters[0];
00034 string door = a.parameters[1];
00035 current.setBooleanPredicate("door-state-known", door, true);
00036 current.setBooleanPredicate("door-open", door, response.door_open);
00037 }
00038 }
00039
00040 };
00041