Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef VIGIR_FOOTSTEP_PLAN_TRANSFORMER_H__
00030 #define VIGIR_FOOTSTEP_PLAN_TRANSFORMER_H__
00031
00032 #include <ros/ros.h>
00033 #include <tf/tf.h>
00034
00035 #include <vigir_footstep_planning_lib/helper.h>
00036
00037 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00038
00039
00040
00041 namespace vigir_footstep_planning
00042 {
00046 class FootPoseTransformer
00047 {
00048 public:
00049 FootPoseTransformer(ros::NodeHandle& nh);
00050 ~FootPoseTransformer();
00051
00052
00053 msgs::ErrorStatus transform(geometry_msgs::Pose& pose, const tf::Transform& transform) const;
00054 msgs::ErrorStatus transform(msgs::Foot& foot, const std::string& target_frame) const;
00055 msgs::ErrorStatus transform(msgs::Feet& feet, const std::string& target_frame) const;
00056 msgs::ErrorStatus transform(msgs::Step& step, const std::string& target_frame) const;
00057 msgs::ErrorStatus transform(msgs::StepPlan& step_plan, const std::string& target_frame) const;
00058
00059 template<template <typename...> class Container>
00060 msgs::ErrorStatus transform(Container<msgs::Step>& cont, const std::string& target_frame) const
00061 {
00062 msgs::ErrorStatus status;
00063 for (typename Container<msgs::Step>::iterator itr = cont.begin(); itr != cont.end(); itr++)
00064 status += transform(itr->foot, target_frame);
00065 return status;
00066 }
00067
00068
00069 template <typename T>
00070 msgs::ErrorStatus transformToPlannerFrame(T& p) const
00071 {
00072 return transform(p, "planner");
00073 }
00074
00075 template <typename T>
00076 msgs::ErrorStatus transformToRobotFrame(T& p) const
00077 {
00078 return transform(p, "robot");
00079 }
00080
00081
00082 typedef boost::shared_ptr<FootPoseTransformer> Ptr;
00083 typedef boost::shared_ptr<const FootPoseTransformer> ConstPtr;
00084
00085 protected:
00086
00087 tf::Transform left_foot_frame_transform;
00088 tf::Transform right_foot_frame_transform;
00089 };
00090 }
00091
00092 #endif