helper.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_lib/helper.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 msgs::ErrorStatus determineStartFeetPose(msgs::Feet& start_feet, ros::ServiceClient& generate_feet_pose_client, const std_msgs::Header& header)
00006 {
00007   msgs::ErrorStatus status;
00008 
00009   // get start feet pose
00010   msgs::GenerateFeetPoseService feet_pose_service;
00011   feet_pose_service.request.request.header = header;
00012   feet_pose_service.request.request.flags = msgs::FeetPoseRequest::FLAG_CURRENT | msgs::FeetPoseRequest::FLAG_3D;
00013 
00014   if (!generate_feet_pose_client.call(feet_pose_service.request, feet_pose_service.response))
00015     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "determineStartFeetPose", "Can't call 'FeetPoseGenerator'!", true, 1.0);
00016 
00017   // check result
00018   if (hasError(feet_pose_service.response.status))
00019   {
00020     //status += feet_pose_service.response.status;
00021     status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN, "determineStartFeetPose", "Can't obtain start feet pose; defaulting to origin.", true, 1.0);
00022 
00023     feet_pose_service.request.request.pose = geometry_msgs::Pose();
00024     feet_pose_service.request.request.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
00025     feet_pose_service.request.request.flags = msgs::FeetPoseRequest::FLAG_3D;
00026 
00027     if (!generate_feet_pose_client.call(feet_pose_service.request, feet_pose_service.response))
00028       return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "determineStartFeetPose", "Can't call 'FeetPoseGenerator'!", true, 1.0);
00029 
00030     if (hasError(feet_pose_service.response.status))
00031     {
00032       status += feet_pose_service.response.status;
00033       return status;
00034     }
00035   }
00036   else if (hasWarning(feet_pose_service.response.status))
00037     status += feet_pose_service.response.status;
00038 
00039   start_feet = feet_pose_service.response.feet;
00040   return status;
00041 }
00042 
00043 msgs::ErrorStatus determineGoalFeetPose(msgs::Feet& goal_feet, ros::ServiceClient& generate_feet_pose_client, const geometry_msgs::PoseStamped& goal_pose)
00044 {
00045   msgs::GenerateFeetPoseService feet_pose_service;
00046   feet_pose_service.request.request.header = goal_pose.header;
00047   feet_pose_service.request.request.pose = goal_pose.pose;
00048   feet_pose_service.request.request.flags = msgs::FeetPoseRequest::FLAG_3D;
00049 
00050   if (!generate_feet_pose_client.call(feet_pose_service.request, feet_pose_service.response))
00051     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "determineStartFeetPose", "Can't call 'FeetPoseGenerator'!");
00052 
00053   goal_feet = feet_pose_service.response.feet;
00054 
00055   return feet_pose_service.response.status;
00056 }
00057 
00058 msgs::ErrorStatus transform(msgs::Foot& foot, ros::ServiceClient& transform_foot_pose_client, const std::string& target_frame)
00059 {
00060   msgs::TransformFootPoseService transform_service;
00061   transform_service.request.foot = foot;
00062   transform_service.request.target_frame.data = target_frame;
00063 
00064   if (!transform_foot_pose_client.call(transform_service.request, transform_service.response))
00065     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "transformToPlannerFrame", "Can't call 'FootPoseTransformer' for foot pose transform!");
00066 
00067   foot = transform_service.response.foot;
00068 
00069   return transform_service.response.status;
00070 }
00071 
00072 msgs::ErrorStatus transform(msgs::Feet& feet, ros::ServiceClient& transform_feet_poses_client, const std::string& target_frame)
00073 {
00074   msgs::TransformFeetPosesService transform_service;
00075   transform_service.request.feet = feet;
00076   transform_service.request.target_frame.data = target_frame;
00077 
00078   if (!transform_feet_poses_client.call(transform_service.request, transform_service.response))
00079     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "transformToPlannerFrame", "Can't call 'FootPoseTransformer' for feet poses transform!");
00080 
00081   feet = transform_service.response.feet;
00082 
00083   return transform_service.response.status;
00084 }
00085 
00086 msgs::ErrorStatus transform(msgs::StepPlan& step_plan, ros::ServiceClient& transform_step_plan_client, const std::string& target_frame)
00087 {
00088   msgs::TransformStepPlanService transform_service;
00089   transform_service.request.step_plan = step_plan;
00090   transform_service.request.target_frame.data = target_frame;
00091 
00092   if (!transform_step_plan_client.call(transform_service.request, transform_service.response))
00093     return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "transformToPlannerFrame", "Can't call 'FootPoseTransformer' for step plan transform!");
00094 
00095   step_plan = transform_service.response.step_plan;
00096 
00097   return transform_service.response.status;
00098 }
00099 
00100 
00101 
00102 bool getXYZ(ros::NodeHandle& nh, const std::string name, geometry_msgs::Vector3& val)
00103 {
00104   if (!nh.hasParam(name+"/x") || !nh.hasParam(name+"/y") || !nh.hasParam(name+"/z"))
00105   {
00106     ROS_ERROR("Couldn't retrieve parameter '%s' as Vector3", (nh.getNamespace()+"/"+name).c_str());
00107     return false;
00108   }
00109 
00110   nh.getParam(name+"/x", val.x);
00111   nh.getParam(name+"/y", val.y);
00112   nh.getParam(name+"/z", val.z);
00113   return true;
00114 }
00115 
00116 bool getRPY(ros::NodeHandle& nh, const std::string name, geometry_msgs::Vector3& val)
00117 {
00118   if (!nh.hasParam(name+"/roll") || !nh.hasParam(name+"/pitch") || !nh.hasParam(name+"/yaw"))
00119   {
00120     ROS_ERROR("Couldn't retrieve parameter '%s' as RPY orienation", (nh.getNamespace()+"/"+name).c_str());
00121     return false;
00122   }
00123 
00124   nh.getParam(name+"/roll", val.x);
00125   nh.getParam(name+"/pitch", val.y);
00126   nh.getParam(name+"/yaw", val.z);
00127   return true;
00128 }
00129 
00130 bool getFootSize(ros::NodeHandle& nh, geometry_msgs::Vector3& foot_size)
00131 {
00132   return getXYZ(nh, "foot/size", foot_size);
00133 }
00134 
00135 bool getUpperBodySize(ros::NodeHandle& nh, geometry_msgs::Vector3& upper_body_size)
00136 {
00137   return getXYZ(nh, "upper_body/size", upper_body_size);
00138 }
00139 
00140 bool getUpperBodyOriginShift(ros::NodeHandle& nh, geometry_msgs::Vector3& upper_body_origin_shift)
00141 {
00142   return getXYZ(nh, "upper_body/origin_shift", upper_body_origin_shift);
00143 }
00144 
00145 
00146 
00147 bool getGridMapCoords(const nav_msgs::OccupancyGrid& map, double x, double y, int& map_x, int& map_y)
00148 {
00149   map_x = round((x-map.info.origin.position.x)/map.info.resolution);
00150   map_y = round((y-map.info.origin.position.y)/map.info.resolution);
00151 
00152   if (map_x < 0 || (int) map.info.width  <= map_x ||
00153       map_y < 0 || (int) map.info.height <= map_y)
00154     return false;
00155 
00156   return true;
00157 }
00158 
00159 bool getGridMapIndex(const nav_msgs::OccupancyGrid& map, double x, double y, int& idx)
00160 {
00161   int map_x, map_y;
00162 
00163   if (!getGridMapCoords(map, x, y, map_x, map_y))
00164     return false;
00165   else
00166     idx = map_x + map_y * map.info.width;
00167 
00168   return true;
00169 }
00170 }


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56