|
double | angle_cell_2_state (int angle, double angle_bin_size) |
|
int | angle_state_2_cell (double angle, double angle_bin_size) |
|
unsigned int | calc_hash_tag (int x, int y, int theta, int leg, unsigned int hash_pred, unsigned int hash_succ, int max_hash_size) |
|
double | cell_2_state (int value, double cell_size) |
|
double | cont_val (int length, double cell_size) |
|
msgs::ErrorStatus | determineGoalFeetPose (msgs::Feet &goal_feet, ros::ServiceClient &generate_feet_pose_client, const geometry_msgs::PoseStamped &goal_pose) |
|
msgs::ErrorStatus | determineStartFeetPose (msgs::Feet &start_feet, ros::ServiceClient &generate_feet_pose_client, const std_msgs::Header &header) |
|
int | disc_val (double length, double cell_size) |
|
double | euclidean_distance (int x1, int y1, int x2, int y2) |
|
double | euclidean_distance (double x1, double y1, double x2, double y2) |
|
double | euclidean_distance (int x1, int y1, int z1, int x2, int y2, int z2) |
|
double | euclidean_distance (double x1, double y1, double z1, double x2, double y2, double z2) |
|
double | euclidean_distance_sq (double x1, double y1, double z1, double x2, double y2, double z2) |
|
double | euclidean_distance_sq (int x1, int y1, int x2, int y2) |
|
double | euclidean_distance_sq (double x1, double y1, double x2, double y2) |
|
double | euclidean_distance_sq (int x1, int y1, int z1, int x2, int y2, int z2) |
|
void | getDeltaStep (const msgs::Foot ¤t, const msgs::Foot &next, geometry_msgs::Pose &dstep) |
|
void | getDeltaStep (const tf::Pose ¤t, const tf::Pose &next, tf::Pose &dstep) |
|
bool | getFootSize (ros::NodeHandle &nh, geometry_msgs::Vector3 &foot_size) |
|
bool | getGridMapCoords (const nav_msgs::OccupancyGrid &map, double x, double y, int &map_x, int &map_y) |
|
bool | getGridMapIndex (const nav_msgs::OccupancyGrid &map, double x, double y, int &idx) |
|
bool | getRPY (ros::NodeHandle &nh, const std::string name, geometry_msgs::Vector3 &val) |
|
bool | getUpperBodyOriginShift (ros::NodeHandle &nh, geometry_msgs::Vector3 &upper_body_origin_shift) |
|
bool | getUpperBodySize (ros::NodeHandle &nh, geometry_msgs::Vector3 &upper_body_size) |
|
bool | getXYZ (ros::NodeHandle &nh, const std::string name, geometry_msgs::Vector3 &val) |
|
double | grid_cost (int x1, int y1, int x2, int y2, float cell_size) |
|
unsigned int | int_hash (int key) |
|
void | normalToQuaternion (const geometry_msgs::Vector3 &n, double yaw, geometry_msgs::Quaternion &q) |
|
void | normalToRP (const geometry_msgs::Vector3 &n, double yaw, double &r, double &p) |
|
double | parabol (double x, double y, double a_inv, double b_inv) |
|
double | pceil (double x, double prec) |
|
double | pfloor (double x, double prec) |
|
bool | pointWithinPolygon (int x, int y, const std::vector< std::pair< int, int > > &edges) |
|
double | pround (double x, double prec) |
|
void | quaternionToNormal (const geometry_msgs::Quaternion &q, geometry_msgs::Vector3 &n) |
|
void | quaternionToNormalYaw (const geometry_msgs::Quaternion &q, geometry_msgs::Vector3 &n, double &yaw) |
|
void | RPYToNormal (double r, double p, double y, geometry_msgs::Vector3 &n) |
|
int | state_2_cell (float value, float cell_size) |
|
std::string & | strip (std::string &s, const char c) |
|
std::string | strip_const (const std::string &s, const char c) |
|
msgs::ErrorStatus | transform (msgs::Feet &feet, ros::ServiceClient &transform_feet_poses_client, const std::string &target_frame) |
|
msgs::ErrorStatus | transform (msgs::StepPlan &step_plan, ros::ServiceClient &transform_step_plan_client, const std::string &target_frame) |
|
msgs::ErrorStatus | transform (msgs::Foot &foot, ros::ServiceClient &transform_foot_pose_client, const std::string &target_frame) |
|
msgs::ErrorStatus | transformToPlannerFrame (T &p, ros::ServiceClient &foot_pose_transformer_client) |
|
msgs::ErrorStatus | transformToRobotFrame (T &p, ros::ServiceClient &foot_pose_transformer_client) |
|