feet_pose_generator.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Simulation, Systems Optimization and Robotics
13 // group, TU Darmstadt nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef VIGIR_FEET_POSE_GENERATOR_H__
30 #define VIGIR_FEET_POSE_GENERATOR_H__
31 
32 #include <ros/ros.h>
33 #include <tf/tf.h>
34 #include <tf/transform_listener.h>
35 
36 #include <geometry_msgs/PoseStamped.h>
37 #include <geometry_msgs/PoseWithCovarianceStamped.h>
38 
39 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
40 #include <vigir_footstep_planning_lib/helper.h>
41 
42 #include <vigir_terrain_classifier/terrain_model.h>
43 
44 
45 
47 {
49 {
50 public:
52  virtual ~FeetPoseGenerator();
53 
54  void setRobotPose(const geometry_msgs::PoseStampedConstPtr &robot_pose);
55  void setRobotPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStampedConstPtr &robot_pose);
56 
57  void setTerrainModel(const vigir_terrain_classifier::TerrainModelMsg::ConstPtr& terrain_model);
58 
59  msgs::ErrorStatus generateFeetPose(const msgs::FeetPoseRequest& request, msgs::Feet& feet);
60 
61  // typedefs
64 
65 protected:
66  msgs::ErrorStatus updateFeetPose(msgs::Feet& feet) const;
67 
68  bool getCurrentFeetPose(msgs::Feet& feet, const std::string& request_frame);
69 
73  msgs::Feet generateFeetPose(const geometry_msgs::PoseStamped& pose);
74 
75  void pelvisToGroundTransform(msgs::Feet& feet) const;
76 
77  // service clients
79 
80  vigir_terrain_classifier::TerrainModel::Ptr terrain_model;
81 
82  std::string left_foot_frame_id;
83  std::string right_foot_frame_id;
84 
88 
89  geometry_msgs::Vector3 pelvis_to_feet_center;
90 
92  geometry_msgs::PoseStamped robot_pose;
93 
95 };
96 }
97 
98 #endif
msgs::ErrorStatus updateFeetPose(msgs::Feet &feet) const
boost::shared_ptr< FeetPoseGenerator > Ptr
void setTerrainModel(const vigir_terrain_classifier::TerrainModelMsg::ConstPtr &terrain_model)
void setRobotPose(const geometry_msgs::PoseStampedConstPtr &robot_pose)
msgs::ErrorStatus generateFeetPose(const msgs::FeetPoseRequest &request, msgs::Feet &feet)
bool getCurrentFeetPose(msgs::Feet &feet, const std::string &request_frame)
void pelvisToGroundTransform(msgs::Feet &feet) const
void setRobotPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStampedConstPtr &robot_pose)
vigir_terrain_classifier::TerrainModel::Ptr terrain_model
boost::shared_ptr< const FeetPoseGenerator > ConstPtr


vigir_feet_pose_generator
Author(s): Alexander Stumpf
autogenerated on Sun Nov 17 2019 03:29:56