00001 //================================================================================================= 00002 // Copyright (c) 2018, Alexander Stumpf, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #ifndef VIGIR_FEET_POSE_GENERATOR_H__ 00030 #define VIGIR_FEET_POSE_GENERATOR_H__ 00031 00032 #include <ros/ros.h> 00033 #include <tf/tf.h> 00034 #include <tf/transform_listener.h> 00035 00036 #include <geometry_msgs/PoseStamped.h> 00037 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00038 00039 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h> 00040 #include <vigir_footstep_planning_lib/helper.h> 00041 00042 #include <vigir_terrain_classifier/terrain_model.h> 00043 00044 00045 00046 namespace vigir_footstep_planning 00047 { 00048 class FeetPoseGenerator 00049 { 00050 public: 00051 FeetPoseGenerator(ros::NodeHandle& nh); 00052 virtual ~FeetPoseGenerator(); 00053 00054 void setRobotPose(const geometry_msgs::PoseStampedConstPtr &robot_pose); 00055 void setRobotPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStampedConstPtr &robot_pose); 00056 00057 void setTerrainModel(const vigir_terrain_classifier::TerrainModelMsg::ConstPtr& terrain_model); 00058 00059 msgs::ErrorStatus generateFeetPose(const msgs::FeetPoseRequest& request, msgs::Feet& feet); 00060 00061 // typedefs 00062 typedef boost::shared_ptr<FeetPoseGenerator> Ptr; 00063 typedef boost::shared_ptr<const FeetPoseGenerator> ConstPtr; 00064 00065 protected: 00066 msgs::ErrorStatus updateFeetPose(msgs::Feet& feet) const; 00067 00068 bool getCurrentFeetPose(msgs::Feet& feet, const std::string& request_frame); 00069 00073 msgs::Feet generateFeetPose(const geometry_msgs::PoseStamped& pose); 00074 00075 void pelvisToGroundTransform(msgs::Feet& feet) const; 00076 00077 // service clients 00078 ros::ServiceClient transform_feet_poses_client; 00079 00080 vigir_terrain_classifier::TerrainModel::Ptr terrain_model; 00081 00082 std::string left_foot_frame_id; 00083 std::string right_foot_frame_id; 00084 00085 double foot_separation; 00086 double left_foot_shift_z; 00087 double right_foot_shift_z; 00088 00089 geometry_msgs::Vector3 pelvis_to_feet_center; 00090 00091 bool has_robot_pose; 00092 geometry_msgs::PoseStamped robot_pose; 00093 00094 tf::TransformListener tf_listener; 00095 }; 00096 } 00097 00098 #endif