36 #ifndef JSK_FOOTSTEP_CONTROLLER_FOOTCOORDS_H_ 37 #define JSK_FOOTSTEP_CONTROLLER_FOOTCOORDS_H_ 38 #include <Eigen/Geometry> 39 #include <sensor_msgs/Imu.h> 40 #include <geometry_msgs/WrenchStamped.h> 50 #include <pcl_msgs/ModelCoefficients.h> 51 #include <jsk_footstep_controller/GroundContactState.h> 52 #include <jsk_footstep_controller/FootCoordsLowLevelInfo.h> 53 #include <jsk_footstep_controller/SynchronizedForces.h> 54 #include <std_msgs/Empty.h> 55 #include <nav_msgs/Odometry.h> 57 #include <kdl/tree.hpp> 58 #include <kdl/chainjnttojacsolver.hpp> 67 typedef typename std::vector<T>::iterator
iterator;
70 for (iterator it = std::vector<T>::begin();
71 it != std::vector<T>::end();) {
92 header(aheader), value(avalue) {
101 geometry_msgs::WrenchStamped,
102 geometry_msgs::WrenchStamped,
103 sensor_msgs::JointState,
130 virtual void filter(
const jsk_footstep_controller::SynchronizedForces::ConstPtr& msg);
131 virtual bool computeMidCoords(
const ros::Time& stamp);
132 virtual bool computeMidCoordsFromSingleLeg(
const ros::Time& stamp,
134 virtual bool waitForEndEffectorTransformation(
const ros::Time& stamp);
135 virtual bool waitForSensorFrameTransformation(
const ros::Time& lstamp,
137 const std::string& lsensor_frame,
138 const std::string& rsensor_frame);
139 virtual bool updateGroundTF();
140 virtual void publishTF(
const ros::Time& stamp);
141 virtual void publishState(
const std::string& state);
143 virtual void publishContactState(
const ros::Time& stamp);
144 virtual double applyLowPassFilter(
double current_val,
double prev_val)
const;
149 virtual bool resolveForceTf(
const geometry_msgs::WrenchStamped& lfoot,
150 const geometry_msgs::WrenchStamped& rfoot,
153 virtual void odomInitTriggerCallback(
const std_msgs::Empty& trigger);
154 virtual void floorCoeffsCallback(
const pcl_msgs::ModelCoefficients& coeffs);
155 virtual void odomImuCallback(
const nav_msgs::Odometry::ConstPtr& odom_msg,
156 const sensor_msgs::Imu::ConstPtr& imu_msg);
159 virtual void synchronizeForces(
const geometry_msgs::WrenchStamped::ConstPtr& lfoot,
160 const geometry_msgs::WrenchStamped::ConstPtr& rfoot,
161 const sensor_msgs::JointState::ConstPtr& joint_states,
162 const geometry_msgs::PointStamped::ConstPtr& zmp);
163 virtual void estimateOdometry();
164 virtual void estimateOdometryNaive();
165 virtual void estimateOdometryMainSupportLeg();
166 virtual void estimateOdometryZMPSupportLeg();
167 virtual void updateRobotModel(std::map<std::string, double>& joint_angles);
168 virtual void updateChain(std::map<std::string, double>& joint_angles,
171 virtual void estimateVelocity(
const ros::Time& stamp, std::map<std::string, double>& joint_angles);
172 virtual void computeVelicity(
double dt,
173 std::map<std::string, double>& joint_angles,
175 geometry_msgs::Twist& output);
176 virtual float getYaw(
const Eigen::Affine3d& pose);
177 virtual void getRollPitch(
const Eigen::Affine3d& pose,
float& roll,
float& pitch);
double getYaw(const tf2::Quaternion &q)