Go to the documentation of this file.00001 #include "planner_navigation_actions/actionExecutorROSNavigation.h"
00002 #include <pluginlib/class_list_macros.h>
00003 #include "tidyup_utils/planning_scene_interface.h"
00004
00005 PLUGINLIB_DECLARE_CLASS(planner_navigation_actions, action_executor_ros_navigation,
00006 planner_navigation_actions::ActionExecutorROSNavigation,
00007 continual_planning_executive::ActionExecutorInterface)
00008
00009 namespace planner_navigation_actions
00010 {
00011
00012 void ActionExecutorROSNavigation::initialize(const std::deque<std::string> & arguments)
00013 {
00014 ActionExecutorActionlib<move_base_msgs::MoveBaseAction, move_base_msgs::MoveBaseGoal,
00015 move_base_msgs::MoveBaseResult>::initialize(arguments);
00016
00017 if(arguments.size() < 3)
00018 return;
00019
00020 bool parseStart = true;
00021 if(arguments[2] == "goal") {
00022 parseStart = false;
00023 } else {
00024 ROS_ASSERT(arguments[2] == "start");
00025 }
00026
00027 unsigned int curArg = 3;
00028 while(curArg < arguments.size()) {
00029 if(arguments[curArg] == "goal") {
00030 parseStart = false;
00031 curArg++;
00032 continue;
00033 }
00034 ROS_ASSERT(arguments.size() >= curArg + 2);
00035 string pred = arguments[curArg];
00036 string setS = arguments[curArg + 1];
00037 bool set = false;
00038 if(setS == "true")
00039 set = true;
00040 else if(setS == "false")
00041 set = false;
00042 else
00043 ROS_ASSERT(false);
00044 if(parseStart)
00045 _startPredicates.push_back(std::make_pair(pred, set));
00046 else
00047 _goalPredicates.push_back(std::make_pair(pred, set));
00048 curArg += 2;
00049 }
00050 }
00051
00052 bool ActionExecutorROSNavigation::fillGoal(move_base_msgs::MoveBaseGoal & goal,
00053 const DurativeAction & a, const SymbolicState & current)
00054 {
00055 if(!PlanningSceneInterface::instance()->resetPlanningScene())
00056 ROS_ERROR("ActionExecutorROSNavigation::fillGoal PlanningScene reset failed.");
00057
00058
00059
00060 goal.target_pose.header.stamp = ros::Time::now();
00061
00062 ROS_ASSERT(a.parameters.size() == 2);
00063 string targetName = a.parameters[1];
00064
00065
00066 Predicate p;
00067 p.parameters.push_back(targetName);
00068 p.name = "frame-id";
00069 if(!current.hasObjectFluent(p, &goal.target_pose.header.frame_id))
00070 return false;
00071 p.name = "x";
00072 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.position.x))
00073 return false;
00074 p.name = "y";
00075 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.position.y))
00076 return false;
00077 p.name = "z";
00078 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.position.z))
00079 return false;
00080 p.name = "qx";
00081 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.orientation.x))
00082 return false;
00083 p.name = "qy";
00084 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.orientation.y))
00085 return false;
00086 p.name = "qz";
00087 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.orientation.z))
00088 return false;
00089 p.name = "qw";
00090 if(!current.hasNumericalFluent(p, &goal.target_pose.pose.orientation.w))
00091 return false;
00092
00093 ROS_INFO_STREAM("Created goal for ActionExecutorROSNavigation as: " << goal);
00094
00095 return true;
00096 }
00097
00098 void ActionExecutorROSNavigation::updateState(const actionlib::SimpleClientGoalState & actionReturnState,
00099 const move_base_msgs::MoveBaseResult & result,
00100 const DurativeAction & a, SymbolicState & current)
00101 {
00102 ROS_ASSERT(a.parameters.size() == 2);
00103 string startName = a.parameters[0];
00104 string targetName = a.parameters[1];
00105
00106
00107 for(std::vector<std::pair<std::string, bool> >::iterator it = _startPredicates.begin();
00108 it != _startPredicates.end(); it++) {
00109 current.setBooleanPredicate(it->first, startName, it->second);
00110 }
00111
00112 if(actionReturnState == actionlib::SimpleClientGoalState::SUCCEEDED) {
00113 for(std::vector<std::pair<std::string, bool> >::iterator it = _goalPredicates.begin();
00114 it != _goalPredicates.end(); it++) {
00115 current.setBooleanPredicate(it->first, targetName, it->second);
00116 }
00117 }
00118 }
00119
00120 };
00121