jsk_footstep_controller: footcoords.h Source File

Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2013, Ryohei Ueda and JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #ifndef JSK_FOOTSTEP_CONTROLLER_FOOTCOORDS_H_
00037 #define JSK_FOOTSTEP_CONTROLLER_FOOTCOORDS_H_
00038 #include <Eigen/Geometry>
00039 #include <sensor_msgs/Imu.h>
00040 #include <geometry_msgs/WrenchStamped.h>
00041 #include <tf/transform_listener.h>
00042 #include <diagnostic_updater/diagnostic_updater.h>
00043 #include <diagnostic_updater/publisher.h>
00044 #include <message_filters/subscriber.h>
00045 #include <message_filters/time_synchronizer.h>
00046 #include <message_filters/synchronizer.h>
00047 #include <tf/transform_listener.h>
00048 #include <tf/transform_broadcaster.h>
00049 #include <jsk_recognition_utils/geo/plane.h>
00050 #include <pcl_msgs/ModelCoefficients.h>
00051 #include <jsk_footstep_controller/GroundContactState.h>
00052 #include <jsk_footstep_controller/FootCoordsLowLevelInfo.h>
00053 #include <jsk_footstep_controller/SynchronizedForces.h>
00054 #include <std_msgs/Empty.h>
00055 #include <nav_msgs/Odometry.h>
00056 #include <urdf/model.h>
00057 #include <kdl/tree.hpp>
00058 #include <kdl/chainjnttojacsolver.hpp>
00059 #include <kdl_parser/kdl_parser.hpp>
00060 
00061 namespace jsk_footstep_controller
00062 {
00063   template <class T>
00064   class TimeStampedVector: public std::vector<T>
00065   {
00066   public:
00067     typedef typename std::vector<T>::iterator iterator;
00068     void removeBefore(const ros::Time& stamp)
00069     {
00070       for (iterator it = std::vector<T>::begin();
00071            it != std::vector<T>::end();) {
00072         if (((*it)->header.stamp - stamp) < ros::Duration(0.0)) {
00073           it = this->erase(it);
00074         }
00075         else {
00076           ++it;
00077         }
00078       }
00079     }
00080   protected:
00081   private:
00082   };
00083 
00084 
00085   class ValueStamped
00086   {
00087   public:
00088     typedef boost::shared_ptr<ValueStamped> Ptr;
00089     std_msgs::Header header;
00090     double value;
00091     ValueStamped(const std_msgs::Header& aheader, double avalue):
00092       header(aheader), value(avalue) {
00093     }
00094     ValueStamped() { }
00095   };
00096 
00097   class Footcoords
00098   {
00099   public:
00100     typedef message_filters::sync_policies::ExactTime<
00101     geometry_msgs::WrenchStamped,
00102     geometry_msgs::WrenchStamped,
00103     sensor_msgs::JointState,
00104     geometry_msgs::PointStamped> SyncPolicy;
00105     typedef message_filters::sync_policies::ExactTime<
00106       nav_msgs::Odometry,
00107       sensor_msgs::Imu> OdomImuSyncPolicy;
00108 
00109     Footcoords();
00110     virtual ~Footcoords();
00111 
00112     enum SupportLegStatus
00113     {
00114       LLEG_GROUND, RLEG_GROUND, AIR, BOTH_GROUND
00115     };
00116 
00117     enum OdomStatus
00118     {
00119       UNINITIALIZED, INITIALIZING, LLEG_SUPPORT, RLEG_SUPPORT
00120     };
00121 
00122   protected:
00123     
00124     // methods
00125 
00126     /* 
00127      * virtual void filter(const geometry_msgs::WrenchStamped::ConstPtr& lfoot,
00128      *                     const geometry_msgs::WrenchStamped::ConstPtr& rfoot);
00129      */
00130     virtual void filter(const jsk_footstep_controller::SynchronizedForces::ConstPtr& msg);
00131     virtual bool computeMidCoords(const ros::Time& stamp);
00132     virtual bool computeMidCoordsFromSingleLeg(const ros::Time& stamp,
00133                                                bool use_left_leg);
00134     virtual bool waitForEndEffectorTransformation(const ros::Time& stamp);
00135     virtual bool waitForSensorFrameTransformation(const ros::Time& lstamp,
00136                                                   const ros::Time& rstamp,
00137                                                   const std::string& lsensor_frame,
00138                                                   const std::string& rsensor_frame);
00139     virtual bool updateGroundTF();
00140     virtual void publishTF(const ros::Time& stamp);
00141     virtual void publishState(const std::string& state);
00142     virtual void updateLegDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
00143     virtual void publishContactState(const ros::Time& stamp);
00144     virtual double applyLowPassFilter(double current_val, double prev_val) const;
00145     virtual bool allValueLargerThan(TimeStampedVector<ValueStamped::Ptr>& values,
00146                                     double threshold);
00147     virtual bool allValueSmallerThan(TimeStampedVector<ValueStamped::Ptr>& values,
00148                                      double threshold);
00149     virtual bool resolveForceTf(const geometry_msgs::WrenchStamped& lfoot,
00150                                 const geometry_msgs::WrenchStamped& rfoot,
00151                                 tf::Vector3& lfoot_force, tf::Vector3& rfoot_force);
00152     virtual void periodicTimerCallback(const ros::TimerEvent& event);
00153     virtual void odomInitTriggerCallback(const std_msgs::Empty& trigger);
00154     virtual void floorCoeffsCallback(const pcl_msgs::ModelCoefficients& coeffs);
00155     virtual void odomImuCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
00156                                  const sensor_msgs::Imu::ConstPtr& imu_msg);
00157     // This callback is called when robot is put on the ground.
00158     // virtual void odomInitCallback(const std_msgs::Empty& odom_init_msg);
00159     virtual void synchronizeForces(const geometry_msgs::WrenchStamped::ConstPtr& lfoot,
00160                                    const geometry_msgs::WrenchStamped::ConstPtr& rfoot,
00161                                    const sensor_msgs::JointState::ConstPtr& joint_states,
00162                                    const geometry_msgs::PointStamped::ConstPtr& zmp);
00163     virtual void estimateOdometry();
00164     virtual void estimateOdometryNaive();
00165     virtual void estimateOdometryMainSupportLeg();
00166     virtual void estimateOdometryZMPSupportLeg();
00167     virtual void updateRobotModel(std::map<std::string, double>& joint_angles);
00168     virtual void updateChain(std::map<std::string, double>& joint_angles,
00169                              KDL::Chain& chain,
00170                              tf::Pose& output_pose);
00171     virtual void estimateVelocity(const ros::Time& stamp, std::map<std::string, double>& joint_angles);
00172     virtual void computeVelicity(double dt,
00173                                  std::map<std::string, double>& joint_angles,
00174                                  KDL::Chain& chain,
00175                                  geometry_msgs::Twist& output);
00176     virtual float getYaw(const Eigen::Affine3d& pose);
00177     virtual void getRollPitch(const Eigen::Affine3d& pose, float& roll, float& pitch);
00178     // ros variables
00179     boost::mutex mutex_;
00180     Eigen::Affine3d odom_pose_;
00181     Eigen::Affine3d odom_init_pose_;
00182     ros::Timer periodic_update_timer_;
00183     ros::Subscriber odom_init_trigger_sub_;
00184     //ros::Subscriber odom_sub_;
00185     message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
00186     message_filters::Subscriber<sensor_msgs::Imu> imu_sub_;
00187     boost::shared_ptr<message_filters::Synchronizer<OdomImuSyncPolicy> > odom_imu_sync_;
00188     ros::Subscriber synchronized_forces_sub_;
00189     message_filters::Subscriber<geometry_msgs::WrenchStamped> sub_lfoot_force_;
00190     message_filters::Subscriber<geometry_msgs::WrenchStamped> sub_rfoot_force_;
00191     message_filters::Subscriber<sensor_msgs::JointState> sub_joint_states_;
00192     message_filters::Subscriber<geometry_msgs::PointStamped> sub_zmp_;
00193     boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
00194     ros::Subscriber floor_coeffs_sub_;
00195     std::vector<float> floor_coeffs_;
00196     KDL::Chain lfoot_chain_;
00197     KDL::Chain rfoot_chain_;
00198     tf::Pose lfoot_pose_;
00199     tf::Pose rfoot_pose_;
00200     tf::Pose lfoot_to_origin_pose_;
00201     tf::Pose rfoot_to_origin_pose_;
00202     tf::Pose estimated_odom_pose_;
00203     ros::Publisher pub_state_;
00204     ros::Publisher pub_contact_state_;
00205     ros::Publisher pub_low_level_;
00206     ros::Publisher pub_synchronized_forces_;
00207     ros::Publisher pub_debug_lfoot_pos_;
00208     ros::Publisher pub_debug_rfoot_pos_;
00209     ros::Publisher pub_leg_odometory_;
00210     ros::Publisher pub_twist_;
00211     ros::Publisher pub_odom_init_transform_;
00212     ros::Publisher pub_odom_init_pose_stamped_;
00213     boost::shared_ptr<tf::TransformListener> tf_listener_;
00214     tf::TransformBroadcaster tf_broadcaster_;
00215     // parameters
00216     std::string zmp_frame_id_;
00217     std::string output_frame_id_;
00218     std::string parent_frame_id_;
00219     std::string midcoords_frame_id_;
00220     SupportLegStatus support_status_;
00221     OdomStatus odom_status_;
00222     Eigen::Vector3f zmp_;
00223     double force_thr_;
00224     bool before_on_the_air_;
00225     std::map<std::string, double> prev_joints_;
00226     bool use_imu_;
00227     bool use_imu_yaw_;
00228     ros::Time last_time_;
00229     std::string lfoot_frame_id_;
00230     std::string rfoot_frame_id_;
00231     std::string lfoot_sensor_frame_;
00232     std::string rfoot_sensor_frame_;
00233     std::string root_frame_id_;
00234     std::string body_on_odom_frame_;
00235     std::string odom_init_frame_id_;
00236     bool invert_odom_init_;
00237     tf::Transform ground_transform_;
00238     tf::Transform midcoords_;
00239     tf::Transform root_link_pose_;
00240     tf::Transform locked_midcoords_to_odom_on_ground_;
00241     boost::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
00242     double prev_lforce_;
00243     double prev_rforce_;
00244     double alpha_;
00245     double sampling_time_;
00246     bool publish_odom_tf_;
00247   private:
00248   };
00249 }
00250 #endif