00001 #include "planner_navigation_actions/stateCreatorROSNavigation.h"
00002 #include <pluginlib/class_list_macros.h>
00003
00004 PLUGINLIB_DECLARE_CLASS(planner_navigation_actions, state_creator_ros_navigation,
00005 planner_navigation_actions::StateCreatorROSNavigation, continual_planning_executive::StateCreator)
00006
00007 namespace planner_navigation_actions
00008 {
00009
00010 StateCreatorROSNavigation::StateCreatorROSNavigation()
00011 {
00012 ros::NodeHandle nhPriv("~");
00013 nhPriv.param("goal_tolerance", _goalTolerance, 0.5);
00014 ROS_INFO("Tolerance for accepting nav goals set to %f.", _goalTolerance);
00015 }
00016
00017 StateCreatorROSNavigation::~StateCreatorROSNavigation()
00018 {
00019 }
00020
00021 bool StateCreatorROSNavigation::fillState(SymbolicState & state)
00022 {
00023 state.setBooleanPredicate("static", "", true);
00024
00025 state.addObject("l0", "location");
00026
00027 tf::StampedTransform transform;
00028 try{
00029 _tf.lookupTransform("/map", "/base_link",
00030 ros::Time(0), transform);
00031 }
00032 catch (tf::TransformException ex){
00033 ROS_ERROR("%s",ex.what());
00034 return false;
00035 }
00036
00037 state.setBooleanPredicate("at", "l0", true);
00038 state.setNumericalFluent("x", "l0", transform.getOrigin().x());
00039 state.setNumericalFluent("y", "l0", transform.getOrigin().y());
00040 state.setNumericalFluent("z", "l0", transform.getOrigin().z());
00041 state.setNumericalFluent("qx", "l0", transform.getRotation().x());
00042 state.setNumericalFluent("qy", "l0", transform.getRotation().y());
00043 state.setNumericalFluent("qz", "l0", transform.getRotation().z());
00044 state.setNumericalFluent("qw", "l0", transform.getRotation().w());
00045
00046
00047 pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
00048 state.getTypedObjects().equal_range("target");
00049
00050 vector<string> paramList;
00051 paramList.push_back("dummy");
00052
00053 double minDist = HUGE_VAL;
00054 string nearestTarget = "";
00055
00056 for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
00057 string target = it->second;
00058 Predicate p;
00059 paramList[0] = target;
00060 p.parameters = paramList;
00061
00062 p.name = "x";
00063 double valueX;
00064 if(!state.hasNumericalFluent(p, &valueX)) {
00065 ROS_ERROR("%s: target object: %s - no x-location in goal.", __func__, target.c_str());
00066 continue;
00067 }
00068 double valueY;
00069 p.name = "y";
00070 if(!state.hasNumericalFluent(p, &valueY)) {
00071 ROS_ERROR("%s: target object: %s - no y-location in goal.", __func__, target.c_str());
00072 continue;
00073 }
00074
00075 double dx = transform.getOrigin().x() - valueX;
00076 double dy = transform.getOrigin().y() - valueY;
00077
00078 if(hypot(dx, dy) < _goalTolerance) {
00079 ROS_INFO("(at) target %s !", target.c_str());
00080 state.setBooleanPredicate("at", target, true);
00081 } else {
00082 state.setBooleanPredicate("at", target, false);
00083 }
00084 if(hypot(dx, dy) < minDist) {
00085 minDist = hypot(dx, dy);
00086 nearestTarget = target;
00087 }
00088 }
00089 ROS_INFO("Nearest target is %s (%f m).", nearestTarget.c_str(), minDist);
00090
00091 return true;
00092 }
00093
00094 };
00095