tidyup_actions: stateCreatorRobotPose.cpp Source File

Go to the documentation of this file.
00001 #include "tidyup_actions/stateCreatorRobotPose.h"
00002 #include <pluginlib/class_list_macros.h>
00003 #include <visualization_msgs/MarkerArray.h>
00004 #include <angles/angles.h>
00005 
00006 PLUGINLIB_DECLARE_CLASS(tidyup_actions, state_creator_robot_pose,
00007         tidyup_actions::StateCreatorRobotPose, continual_planning_executive::StateCreator)
00008 
00009 namespace tidyup_actions
00010 {
00011 
00012     StateCreatorRobotPose::StateCreatorRobotPose()
00013     {
00014         ros::NodeHandle nhPriv("~");
00015         ros::NodeHandle nh;
00016         nhPriv.param("nav_target_tolerance_xy", _goalToleranceXY, 0.5);
00017         nhPriv.param("nav_target_tolerance_yaw", _goalToleranceYaw, 0.26);  //15deg
00018 
00019         bool relative;
00020         nhPriv.param("nav_target_tolerance_relative_to_move_base", relative, false);
00021         if(relative) {
00022             // relative mode: 1. get the namespace for base_local_planner
00023             std::string base_local_planner_ns;
00024             if(!nhPriv.getParam("nav_base_local_planner_ns", base_local_planner_ns)) {
00025                 ROS_WARN("nav_target_tolerance_relative_to_move_base set, but nav_base_local_planner_ns not set - trying to estimate");
00026                 std::string local_planner;
00027                 if(!nh.getParam("move_base_node/base_local_planner", local_planner)
00028                         && !nh.getParam("move_base/base_local_planner", local_planner)) {
00029                     ROS_ERROR("move_base(_node)/base_local_planner not set - falling back to absolute mode.");
00030                 } else {
00031                     // dwa_local_planner/DWAPlannerROS -> DWAPlannerROS
00032                     std::string::size_type x = local_planner.find_last_of("/");
00033                     if(x == std::string::npos)
00034                         base_local_planner_ns = local_planner;
00035                     else
00036                         base_local_planner_ns = local_planner.substr(x + 1);
00037                     ROS_INFO("Estimated base_local_planner_ns to %s.", base_local_planner_ns.c_str());
00038                 }
00039             }
00040             
00041             if(!base_local_planner_ns.empty()) { // success: 2. get the xy_goal_tolerance
00042                 double move_base_tol_xy;
00043                 if(!nh.getParam(base_local_planner_ns + "/xy_goal_tolerance", move_base_tol_xy)) {
00044                     ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
00045                             << (base_local_planner_ns + "/xy_goal_tolerance") << " was not set"
00046                             << " - falling back to absolute mode");
00047                 } else { // 2. add move_base's tolerance to our relative tolerance
00048                     _goalToleranceXY += move_base_tol_xy;
00049                 }
00050 
00051                 double move_base_tol_yaw;
00052                 if(!nh.getParam(base_local_planner_ns + "/yaw_goal_tolerance", move_base_tol_yaw)) {
00053                     ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
00054                             << (base_local_planner_ns + "/yaw_goal_tolerance") << " was not set"
00055                             << " - falling back to absolute mode");
00056                 } else { // 2. add move_base's tolerance to our relative tolerance
00057                     _goalToleranceYaw += move_base_tol_yaw;
00058                 }
00059             }
00060         }
00061 
00062         ROS_INFO("Tolerance for accepting nav goals set to %f m, %f deg.",
00063                 _goalToleranceXY, angles::to_degrees(_goalToleranceYaw));
00064 
00065         if(s_PublishLocationsAsMarkers) {
00066             _markerPub = nhPriv.advertise<visualization_msgs::MarkerArray>("robot_pose_markers", 5, true);
00067             ROS_INFO("marker topic: %s", _markerPub.getTopic().c_str());
00068         }
00069     }
00070 
00071     StateCreatorRobotPose::~StateCreatorRobotPose()
00072     {
00073     }
00074 
00075     void StateCreatorRobotPose::initialize(const std::deque<std::string> & arguments)
00076     {
00077         ROS_ASSERT(arguments.size() == 4);
00078 
00079         _robotPoseObject = arguments[0];
00080         _robotPoseType = arguments[1];
00081         _atPredicate = arguments[2];
00082         _locationType = arguments[3];
00083 
00084         if(_robotPoseObject == "-")
00085             _robotPoseObject = "";
00086         if(_robotPoseType == "-")
00087             _robotPoseType = "";
00088         if(_atPredicate == "-")
00089             _atPredicate = "";
00090         if(_locationType == "-")
00091             _locationType = "";
00092 
00093         if(!_robotPoseVis.initialize()) {
00094             ROS_ERROR("Failed to initialized RobotPoseVisualization.");
00095         }
00096     }
00097 
00098     bool StateCreatorRobotPose::fillState(SymbolicState & state)
00099     {
00100         tf::StampedTransform transform;
00101         try{
00102             _tf.lookupTransform("/map", "/base_link", ros::Time(0), transform);
00103         }
00104         catch (tf::TransformException& ex){
00105             ROS_ERROR("%s",ex.what());
00106             return false;
00107         }
00108 
00109         // 1. Real robot location
00110         if(!_robotPoseObject.empty()) {
00111             ROS_ASSERT(!_robotPoseType.empty());
00112             state.addObject(_robotPoseObject, _robotPoseType);
00113             state.setNumericalFluent("x", _robotPoseObject, transform.getOrigin().x());
00114             state.setNumericalFluent("y", _robotPoseObject, transform.getOrigin().y());
00115             state.setNumericalFluent("z", _robotPoseObject, transform.getOrigin().z());
00116             state.setNumericalFluent("qx", _robotPoseObject, transform.getRotation().x());
00117             state.setNumericalFluent("qy", _robotPoseObject, transform.getRotation().y());
00118             state.setNumericalFluent("qz", _robotPoseObject, transform.getRotation().z());
00119             state.setNumericalFluent("qw", _robotPoseObject, transform.getRotation().w());
00120             state.setNumericalFluent("timestamp", _robotPoseObject, ros::Time::now().toSec());
00121             state.addObject("/map", "frameid");
00122             state.setObjectFluent("frame-id", _robotPoseObject, "/map");
00123         }
00124 
00125         // 2.b check if we are at any _locations
00126         pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
00127             state.getTypedObjects().equal_range(_locationType);
00128 
00129         double minDist = HUGE_VAL;
00130         string nearestTarget = "";
00131 
00132         int atLocations = 0;
00133         for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
00134             string target = it->second;
00135             if(target == _robotPoseObject)  // skip current robot location
00136                 continue;
00137 
00138             geometry_msgs::PoseStamped targetPose;
00139             if(!extractPoseStamped(state, target, targetPose)) {
00140                 ROS_ERROR("%s: could not extract pose for target object: %s.", __func__, target.c_str());
00141                 continue; 
00142             }
00143             if(targetPose.header.frame_id != "/map") {
00144                 ROS_ERROR("Target pose %s had frame-id: %s - should be /map.",
00145                         target.c_str(), targetPose.header.frame_id.c_str());
00146                 continue;
00147             }
00148 
00149             // compute dXY, dYaw between current pose and target
00150             tf::Transform targetTransform;//(btQuaternion(qx, qy, qz, qw), btVector3(posX, posY, 0.0));
00151             tf::poseMsgToTF(targetPose.pose, targetTransform);
00152             tf::Transform deltaTransform = targetTransform.inverseTimes(transform);
00153 
00154             double dDist = hypot(deltaTransform.getOrigin().x(), deltaTransform.getOrigin().y());
00155             double dAng = tf::getYaw(deltaTransform.getRotation());
00156             ROS_INFO("Target %s dist: %f m ang: %f deg", target.c_str(), dDist, angles::to_degrees(dAng));
00157 
00158             if(!_atPredicate.empty()) {
00159                 // Found a target - update state!
00160                 if(dDist < _goalToleranceXY && fabs(dAng) < _goalToleranceYaw) {
00161                     ROS_INFO("(at) target %s !", target.c_str());
00162                     state.setBooleanPredicate(_atPredicate, target, true);
00163                     atLocations++;
00164                 } else {
00165                     state.setBooleanPredicate(_atPredicate, target, false);
00166                 }
00167                 if(dDist < minDist) {
00168                     minDist = dDist;
00169                     nearestTarget = target;
00170                 }
00171             }
00172         }
00173 
00174         ROS_INFO("Nearest target is %s (%f m).", nearestTarget.c_str(), minDist);
00175 
00176         // 2.a Set the robot pose, if we are not already at another pose
00177         if(!_atPredicate.empty() && !_robotPoseObject.empty()) {
00178             if(atLocations == 0) {
00179                 state.setBooleanPredicate(_atPredicate, _robotPoseObject, true);
00180             } else {
00181                 state.setBooleanPredicate(_atPredicate, _robotPoseObject, false);
00182                 if(atLocations > 1) {
00183                     ROS_WARN("We are at %d locations at the same time!.", atLocations);
00184                 }
00185             }
00186         }
00187 
00188         if(s_PublishLocationsAsMarkers)
00189             publishLocationsAsMarkers(state);
00190 
00191         return true;
00192     }
00193 
00194     bool StateCreatorRobotPose::extractPoseStamped(const SymbolicState & state, const string & object,
00195             geometry_msgs::PoseStamped & pose) const
00196     {
00197         bool ret = true;
00198 
00199         // first get xyz, qxyzw from state
00200         Predicate p;
00201         p.parameters.push_back(object);
00202 
00203         double posX = 0;
00204         p.name = "x";
00205         if(!state.hasNumericalFluent(p, &posX)) {
00206             ROS_ERROR("%s: object: %s - no x-location in state.", __func__, object.c_str());
00207             ret = false;;
00208         }
00209         double posY = 0;
00210         p.name = "y";
00211         if(!state.hasNumericalFluent(p, &posY)) {
00212             ROS_ERROR("%s: object: %s - no y-location in state.", __func__, object.c_str());
00213             ret = false;;
00214         }
00215         double posZ = 0;
00216         p.name = "z";
00217         if(!state.hasNumericalFluent(p, &posZ)) {
00218             ROS_ERROR("%s: object: %s - no z-location in state.", __func__, object.c_str());
00219             ret = false;;
00220         }
00221 
00222         double qx;
00223         p.name = "qx";
00224         if(!state.hasNumericalFluent(p, &qx)) {
00225             ROS_ERROR("%s: object: %s - no qx in state.", __func__, object.c_str());
00226             ret = false;;
00227         }
00228         double qy;
00229         p.name = "qy";
00230         if(!state.hasNumericalFluent(p, &qy)) {
00231             ROS_ERROR("%s: object: %s - no qy in state.", __func__, object.c_str());
00232             ret = false;;
00233         }
00234         double qz;
00235         p.name = "qz";
00236         if(!state.hasNumericalFluent(p, &qz)) {
00237             ROS_ERROR("%s: object: %s - no qz in state.", __func__, object.c_str());
00238             ret = false;;
00239         }
00240         double qw;
00241         p.name = "qw";
00242         if(!state.hasNumericalFluent(p, &qw)) {
00243             ROS_ERROR("%s: object: %s - no qw in state.", __func__, object.c_str());
00244             ret = false;;
00245         }
00246 
00247         double timestamp;
00248         p.name = "timestamp";
00249         if(!state.hasNumericalFluent(p, &timestamp)) {
00250             ROS_ERROR("%s: object: %s - no timestamp in state.", __func__, object.c_str());
00251             ret = false;;
00252         }
00253 
00254         string frameid;
00255         p.name = "frame-id";
00256         if(!state.hasObjectFluent(p, &frameid)) {
00257             ROS_ERROR("%s: object: %s - no frameid in state.", __func__, object.c_str());
00258             ret = false;;
00259         }
00260 
00261         pose.header.frame_id = frameid;
00262         pose.header.stamp = ros::Time(timestamp);
00263         pose.pose.position.x = posX;
00264         pose.pose.position.y = posY;
00265         pose.pose.position.z = posZ;
00266         pose.pose.orientation.x = qx;
00267         pose.pose.orientation.y = qy;
00268         pose.pose.orientation.z = qz;
00269         pose.pose.orientation.w = qw;
00270 
00271         return ret;
00272     }
00273 
00274     std_msgs::ColorRGBA StateCreatorRobotPose::getLocationColor(
00275             const SymbolicState & state, const string & location) const
00276     {
00277         std_msgs::ColorRGBA color;
00278 
00279         Predicate p;
00280         p.name = _atPredicate;
00281         p.parameters.push_back(location);
00282         bool at = false;
00283         if(!state.hasBooleanPredicate(p, &at)) {
00284             ROS_ERROR("%s: state has at-base not set for %s.", __func__, location.c_str());
00285             return color;
00286         }
00287 
00288         if(location == _robotPoseObject) {      //robot
00289             color.a = 0.5;
00290             if(at) {
00291                 color.b = 1.0;
00292             } else {
00293                 color.r = 1.0;
00294                 color.g = 1.0;
00295                 color.b = 1.0;
00296             }
00297         } else {            // targets
00298             color.a = 0.5;
00299             if(at) {
00300                 color.g = 1.0;
00301             } else {
00302                 color.r = 1.0;
00303                 color.g = 1.0;
00304             }
00305         }
00306 
00307         return color;
00308     }
00309 
00310     // from pr2_tasks/arm_tasks.py DEFAULT_SIDE_JOINT_TRAJECTORY
00311     sensor_msgs::JointState StateCreatorRobotPose::getArmSideJointState() const
00312     {
00313         sensor_msgs::JointState ret;
00314         ret.name.resize(14);
00315         ret.name[0] = "l_shoulder_pan_joint";
00316         ret.name[1] = "l_shoulder_lift_joint";
00317         ret.name[2] = "l_upper_arm_roll_joint";
00318         ret.name[3] = "l_elbow_flex_joint";
00319         ret.name[4] = "l_forearm_roll_joint";
00320         ret.name[5] = "l_wrist_flex_joint";
00321         ret.name[6] = "l_wrist_roll_joint";
00322 
00323         ret.name[7] = "r_shoulder_pan_joint";
00324         ret.name[8] = "r_shoulder_lift_joint";
00325         ret.name[9] = "r_upper_arm_roll_joint";
00326         ret.name[10] = "r_elbow_flex_joint";
00327         ret.name[11] = "r_forearm_roll_joint";
00328         ret.name[12] = "r_wrist_flex_joint";
00329         ret.name[13] = "r_wrist_roll_joint";
00330 
00331         ret.position.resize(14);
00332 
00333 
00334         bool arm_param_ok = true;
00335         ros::NodeHandle nh;
00336         XmlRpc::XmlRpcValue xmlRpcRight;
00337         if(!nh.getParam("/arm_configurations/side_tuck/position/right_arm", xmlRpcRight)) {
00338             ROS_WARN("Could not load /arm_configurations/side_tuck/position/right_arm.");
00339             arm_param_ok = false;
00340         } else {
00341             if(xmlRpcRight.getType() != XmlRpc::XmlRpcValue::TypeArray)
00342                 arm_param_ok = false;
00343             else if(xmlRpcRight.size() != 7)
00344                 arm_param_ok = false;
00345         }
00346         XmlRpc::XmlRpcValue xmlRpcLeft;
00347         if(!nh.getParam("/arm_configurations/side_tuck/position/left_arm", xmlRpcLeft)) {
00348             ROS_WARN("Could not load /arm_configurations/side_tuck/position/left_arm.");
00349             arm_param_ok = false;
00350         } else {
00351             if(xmlRpcLeft.getType() != XmlRpc::XmlRpcValue::TypeArray)
00352                 arm_param_ok = false;
00353             else if(xmlRpcLeft.size() != 7)
00354                 arm_param_ok = false;
00355         }
00356 
00357         if(arm_param_ok) {
00358             // left
00359             ret.position[0] = xmlRpcLeft[0];
00360             ret.position[1] = xmlRpcLeft[1];
00361             ret.position[2] = xmlRpcLeft[2];
00362             ret.position[3] = xmlRpcLeft[3];
00363             ret.position[4] = xmlRpcLeft[4];
00364             ret.position[5] = xmlRpcLeft[5];
00365             ret.position[6] = xmlRpcLeft[6];
00366 
00367             // right
00368             ret.position[7] = xmlRpcRight[0];
00369             ret.position[8] = xmlRpcRight[1];
00370             ret.position[9] = xmlRpcRight[2];
00371             ret.position[10] = xmlRpcRight[3];
00372             ret.position[11] = xmlRpcRight[4];
00373             ret.position[12] = xmlRpcRight[5];
00374             ret.position[13] = xmlRpcRight[6];
00375         } else {
00376             ROS_WARN("Using hardcoded arm configs");
00377 
00378             ret.position[0] = 2.1;
00379             ret.position[1] = 1.26;
00380             ret.position[2] = 1.8;
00381             ret.position[3] = -1.9;
00382             ret.position[4] = -3.5;
00383             ret.position[5] = -1.8;
00384             ret.position[6] = M_PI_2;
00385 
00386             ret.position[7] = -2.1;
00387             ret.position[8] = 1.26;
00388             ret.position[9] = -1.8;
00389             ret.position[10] = -1.9;
00390             ret.position[11] = 3.5;
00391             ret.position[12] = -1.8;
00392             ret.position[13] = M_PI_2;
00393         }
00394 
00395         return ret;
00396     }
00397 
00398     visualization_msgs::MarkerArray StateCreatorRobotPose::getLocationMarkers(const SymbolicState & state,
00399             const string & location, const string & ns, int id, bool useMeshes) const
00400     {
00401         visualization_msgs::MarkerArray ma;
00402 
00403         geometry_msgs::PoseStamped locationPose;
00404         if(!extractPoseStamped(state, location, locationPose)) {
00405             ROS_ERROR("%s: could not extract pose for location object: %s.", __func__, location.c_str());
00406             return ma; 
00407         }
00408         if(locationPose.header.frame_id != "/map") {
00409             ROS_ERROR("Location pose %s had frame-id: %s - should be /map.",
00410                     location.c_str(), locationPose.header.frame_id.c_str());
00411             return ma;
00412         }
00413 
00414         if(useMeshes) {
00415             _robotPoseVis.updateRobotStateJoints(getArmSideJointState());
00416             locationPose.pose.position.z = 0;
00417             // zero out z to account for different frame (/base_link vs. /base_footprint)
00418             _robotPoseVis.updateRobotStatePose(locationPose);
00419 
00420             std_msgs::ColorRGBA color = getLocationColor(state, location);
00421             std::stringstream ss;
00422             ss << "mesh_" << ns << "_" << location;
00423             ma = _robotPoseVis.getMarkers(color, ss.str());
00424 
00425             return ma;
00426         }
00427 
00428         visualization_msgs::Marker mark;
00429         mark.header.frame_id = "/map";
00430         mark.ns = ns; 
00431         mark.id = id;
00432         mark.type = visualization_msgs::Marker::ARROW;
00433         mark.action = visualization_msgs::Marker::ADD;
00434         mark.pose = locationPose.pose;
00435         mark.pose.position.z += 0.15;
00436         mark.scale.x = 1.0;     // radius / 10?
00437         mark.scale.y = 1.0;
00438         mark.scale.z = 0.12;     // arrow length
00439         mark.color = getLocationColor(state, location);
00440         mark.text = location;
00441 
00442         ma.markers.push_back(mark);
00443 
00444         mark.type = visualization_msgs::Marker::SPHERE;
00445         mark.scale.x = 0.2;
00446         mark.scale.y = 0.2;
00447         mark.scale.z = 0.2;
00448         mark.id++;
00449 
00450         ma.markers.push_back(mark);
00451         
00452         return ma;
00453     }
00454 
00460     void StateCreatorRobotPose::publishLocationsAsMarkers(const SymbolicState & state)
00461     {
00462         if(!_markerPub) {
00463             ROS_WARN("%s: _markerPub invalid.", __func__);
00464             return;
00465         }
00466 
00467         visualization_msgs::MarkerArray ma;
00468 
00469         if(!_locationType.empty()) {
00470             // Check if we are at any grasp_locations
00471             pair<SymbolicState::TypedObjectConstIterator, SymbolicState::TypedObjectConstIterator> targets =
00472                 state.getTypedObjects().equal_range(_locationType);
00473 
00474             unsigned int count = 0;
00475             for(SymbolicState::TypedObjectConstIterator it = targets.first; it != targets.second; it++) {
00476                 string target = it->second;
00477                 if(target == _robotPoseObject)  // skip current robot location
00478                     continue;
00479 
00480                 visualization_msgs::MarkerArray marks = getLocationMarkers(state, target,
00481                         "target_locations", count, false);
00482                 forEach(visualization_msgs::Marker & mark, marks.markers) {
00483                     if(mark.header.frame_id.empty())    // invalid mark
00484                         continue;
00485                     ma.markers.push_back(mark);
00486                 }
00487                 count += 2;
00488                 
00489                 if(s_PublishMeshMarkers) {
00490                     visualization_msgs::MarkerArray marks = getLocationMarkers(state, target,
00491                             "target_locations", count, true);
00492                     forEach(visualization_msgs::Marker & mark, marks.markers) {
00493                         if(mark.header.frame_id.empty())    // invalid mark
00494                             continue;
00495                         ma.markers.push_back(mark);
00496                     }
00497                 }
00498             }
00499 
00500             // all should be overwritten as #targets is const, but to be safe
00501             for(unsigned int i = count; i < 100; i++) {
00502                 visualization_msgs::Marker mark;
00503                 mark.header.frame_id = "/map";
00504                 mark.ns = "target_locations";
00505                 mark.id = i;
00506                 mark.action = visualization_msgs::Marker::DELETE;
00507                 ma.markers.push_back(mark);
00508             }
00509         }
00510 
00511         // finally robot location marker
00512         if(!_robotPoseObject.empty()) {
00513             visualization_msgs::MarkerArray marks = getLocationMarkers(state, _robotPoseObject,
00514                     "robot_location", 0, false);
00515             forEach(visualization_msgs::Marker & mark, marks.markers) {
00516                 if(mark.header.frame_id.empty())    // invalid mark
00517                     mark.action = visualization_msgs::Marker::DELETE;
00518                 ma.markers.push_back(mark);
00519             }
00520 
00521             if(s_PublishMeshMarkers) {
00522                 visualization_msgs::MarkerArray marks = getLocationMarkers(state, _robotPoseObject,
00523                         "robot_location", 0, true);
00524                 forEach(visualization_msgs::Marker & mark, marks.markers) {
00525                     if(mark.header.frame_id.empty())    // invalid mark
00526                         continue;
00527                     ma.markers.push_back(mark);
00528                 }
00529             }
00530         }
00531 
00532         _markerPub.publish(ma);
00533     }
00534 
00535 };
00536