Public Types | Public Member Functions | Protected Member Functions | Protected Attributes
jsk_footstep_controller::Footcoords Class Reference

#include <footcoords.h>

List of all members.

Public Types

typedef
message_filters::sync_policies::ExactTime
< nav_msgs::Odometry,
sensor_msgs::Imu > 
OdomImuSyncPolicy
enum  OdomStatus { UNINITIALIZED, INITIALIZING, LLEG_SUPPORT, RLEG_SUPPORT }
enum  SupportLegStatus { LLEG_GROUND, RLEG_GROUND, AIR, BOTH_GROUND }
typedef
message_filters::sync_policies::ExactTime
< geometry_msgs::WrenchStamped,
geometry_msgs::WrenchStamped,
sensor_msgs::JointState,
geometry_msgs::PointStamped > 
SyncPolicy

Public Member Functions

 Footcoords ()
virtual ~Footcoords ()

Protected Member Functions

virtual bool allValueLargerThan (TimeStampedVector< ValueStamped::Ptr > &values, double threshold)
virtual bool allValueSmallerThan (TimeStampedVector< ValueStamped::Ptr > &values, double threshold)
virtual double applyLowPassFilter (double current_val, double prev_val) const
virtual bool computeMidCoords (const ros::Time &stamp)
virtual bool computeMidCoordsFromSingleLeg (const ros::Time &stamp, bool use_left_leg)
virtual void computeVelicity (double dt, std::map< std::string, double > &joint_angles, KDL::Chain &chain, geometry_msgs::Twist &output)
virtual void estimateOdometry ()
virtual void estimateOdometryMainSupportLeg ()
virtual void estimateOdometryNaive ()
virtual void estimateOdometryZMPSupportLeg ()
virtual void estimateVelocity (const ros::Time &stamp, std::map< std::string, double > &joint_angles)
virtual void filter (const jsk_footstep_controller::SynchronizedForces::ConstPtr &msg)
virtual void floorCoeffsCallback (const pcl_msgs::ModelCoefficients &coeffs)
virtual void getRollPitch (const Eigen::Affine3d &pose, float &roll, float &pitch)
virtual float getYaw (const Eigen::Affine3d &pose)
virtual void odomImuCallback (const nav_msgs::Odometry::ConstPtr &odom_msg, const sensor_msgs::Imu::ConstPtr &imu_msg)
virtual void odomInitTriggerCallback (const std_msgs::Empty &trigger)
virtual void periodicTimerCallback (const ros::TimerEvent &event)
virtual void publishContactState (const ros::Time &stamp)
virtual void publishState (const std::string &state)
virtual void publishTF (const ros::Time &stamp)
virtual bool resolveForceTf (const geometry_msgs::WrenchStamped &lfoot, const geometry_msgs::WrenchStamped &rfoot, tf::Vector3 &lfoot_force, tf::Vector3 &rfoot_force)
virtual void synchronizeForces (const geometry_msgs::WrenchStamped::ConstPtr &lfoot, const geometry_msgs::WrenchStamped::ConstPtr &rfoot, const sensor_msgs::JointState::ConstPtr &joint_states, const geometry_msgs::PointStamped::ConstPtr &zmp)
virtual void updateChain (std::map< std::string, double > &joint_angles, KDL::Chain &chain, tf::Pose &output_pose)
virtual bool updateGroundTF ()
virtual void updateLegDiagnostics (diagnostic_updater::DiagnosticStatusWrapper &stat)
virtual void updateRobotModel (std::map< std::string, double > &joint_angles)
virtual bool waitForEndEffectorTransformation (const ros::Time &stamp)
virtual bool waitForSensorFrameTransformation (const ros::Time &lstamp, const ros::Time &rstamp, const std::string &lsensor_frame, const std::string &rsensor_frame)

Protected Attributes

double alpha_
bool before_on_the_air_
std::string body_on_odom_frame_
boost::shared_ptr
< diagnostic_updater::Updater
diagnostic_updater_
tf::Pose estimated_odom_pose_
std::vector< float > floor_coeffs_
ros::Subscriber floor_coeffs_sub_
double force_thr_
tf::Transform ground_transform_
message_filters::Subscriber
< sensor_msgs::Imu > 
imu_sub_
bool invert_odom_init_
ros::Time last_time_
KDL::Chain lfoot_chain_
std::string lfoot_frame_id_
tf::Pose lfoot_pose_
std::string lfoot_sensor_frame_
tf::Pose lfoot_to_origin_pose_
tf::Transform locked_midcoords_to_odom_on_ground_
tf::Transform midcoords_
std::string midcoords_frame_id_
boost::mutex mutex_
boost::shared_ptr
< message_filters::Synchronizer
< OdomImuSyncPolicy > > 
odom_imu_sync_
std::string odom_init_frame_id_
Eigen::Affine3d odom_init_pose_
ros::Subscriber odom_init_trigger_sub_
Eigen::Affine3d odom_pose_
OdomStatus odom_status_
message_filters::Subscriber
< nav_msgs::Odometry > 
odom_sub_
std::string output_frame_id_
std::string parent_frame_id_
ros::Timer periodic_update_timer_
std::map< std::string, double > prev_joints_
double prev_lforce_
double prev_rforce_
ros::Publisher pub_contact_state_
ros::Publisher pub_debug_lfoot_pos_
ros::Publisher pub_debug_rfoot_pos_
ros::Publisher pub_leg_odometory_
ros::Publisher pub_low_level_
ros::Publisher pub_odom_init_pose_stamped_
ros::Publisher pub_odom_init_transform_
ros::Publisher pub_state_
ros::Publisher pub_synchronized_forces_
ros::Publisher pub_twist_
bool publish_odom_tf_
KDL::Chain rfoot_chain_
std::string rfoot_frame_id_
tf::Pose rfoot_pose_
std::string rfoot_sensor_frame_
tf::Pose rfoot_to_origin_pose_
std::string root_frame_id_
tf::Transform root_link_pose_
double sampling_time_
message_filters::Subscriber
< sensor_msgs::JointState > 
sub_joint_states_
message_filters::Subscriber
< geometry_msgs::WrenchStamped > 
sub_lfoot_force_
message_filters::Subscriber
< geometry_msgs::WrenchStamped > 
sub_rfoot_force_
message_filters::Subscriber
< geometry_msgs::PointStamped > 
sub_zmp_
SupportLegStatus support_status_
boost::shared_ptr
< message_filters::Synchronizer
< SyncPolicy > > 
sync_
ros::Subscriber synchronized_forces_sub_
tf::TransformBroadcaster tf_broadcaster_
boost::shared_ptr
< tf::TransformListener
tf_listener_
bool use_imu_
bool use_imu_yaw_
Eigen::Vector3f zmp_
std::string zmp_frame_id_

Detailed Description

Definition at line 97 of file footcoords.h.


Member Typedef Documentation

Definition at line 107 of file footcoords.h.

typedef message_filters::sync_policies::ExactTime< geometry_msgs::WrenchStamped, geometry_msgs::WrenchStamped, sensor_msgs::JointState, geometry_msgs::PointStamped> jsk_footstep_controller::Footcoords::SyncPolicy

Definition at line 104 of file footcoords.h.


Member Enumeration Documentation

Enumerator:
UNINITIALIZED 
INITIALIZING 
LLEG_SUPPORT 
RLEG_SUPPORT 

Definition at line 117 of file footcoords.h.

Enumerator:
LLEG_GROUND 
RLEG_GROUND 
AIR 
BOTH_GROUND 

Definition at line 112 of file footcoords.h.


Constructor & Destructor Documentation

Definition at line 51 of file footcoords.cpp.

Definition at line 148 of file footcoords.cpp.


Member Function Documentation

bool jsk_footstep_controller::Footcoords::allValueLargerThan ( TimeStampedVector< ValueStamped::Ptr > &  values,
double  threshold 
) [protected, virtual]

Definition at line 178 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::allValueSmallerThan ( TimeStampedVector< ValueStamped::Ptr > &  values,
double  threshold 
) [protected, virtual]

Definition at line 189 of file footcoords.cpp.

double jsk_footstep_controller::Footcoords::applyLowPassFilter ( double  current_val,
double  prev_val 
) const [protected, virtual]

Definition at line 173 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::computeMidCoords ( const ros::Time stamp) [protected, virtual]

Definition at line 803 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::computeMidCoordsFromSingleLeg ( const ros::Time stamp,
bool  use_left_leg 
) [protected, virtual]

Definition at line 758 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::computeVelicity ( double  dt,
std::map< std::string, double > &  joint_angles,
KDL::Chain chain,
geometry_msgs::Twist &  output 
) [protected, virtual]

Definition at line 360 of file footcoords.cpp.

Definition at line 471 of file footcoords.cpp.

Definition at line 539 of file footcoords.cpp.

Definition at line 477 of file footcoords.cpp.

Definition at line 618 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateVelocity ( const ros::Time stamp,
std::map< std::string, double > &  joint_angles 
) [protected, virtual]

Definition at line 385 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::filter ( const jsk_footstep_controller::SynchronizedForces::ConstPtr &  msg) [protected, virtual]

Definition at line 402 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::floorCoeffsCallback ( const pcl_msgs::ModelCoefficients &  coeffs) [protected, virtual]

Definition at line 1048 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::getRollPitch ( const Eigen::Affine3d &  pose,
float &  roll,
float &  pitch 
) [protected, virtual]

Definition at line 934 of file footcoords.cpp.

float jsk_footstep_controller::Footcoords::getYaw ( const Eigen::Affine3d &  pose) [protected, virtual]

Definition at line 942 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::odomImuCallback ( const nav_msgs::Odometry::ConstPtr &  odom_msg,
const sensor_msgs::Imu::ConstPtr &  imu_msg 
) [protected, virtual]

Definition at line 1080 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::odomInitTriggerCallback ( const std_msgs::Empty &  trigger) [protected, virtual]

Definition at line 1018 of file footcoords.cpp.

Definition at line 246 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishContactState ( const ros::Time stamp) [protected, virtual]

Definition at line 706 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishState ( const std::string state) [protected, virtual]

Definition at line 699 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishTF ( const ros::Time stamp) [protected, virtual]

Definition at line 951 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::resolveForceTf ( const geometry_msgs::WrenchStamped &  lfoot,
const geometry_msgs::WrenchStamped &  rfoot,
tf::Vector3 lfoot_force,
tf::Vector3 rfoot_force 
) [protected, virtual]

Definition at line 200 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::synchronizeForces ( const geometry_msgs::WrenchStamped::ConstPtr &  lfoot,
const geometry_msgs::WrenchStamped::ConstPtr &  rfoot,
const sensor_msgs::JointState::ConstPtr &  joint_states,
const geometry_msgs::PointStamped::ConstPtr &  zmp 
) [protected, virtual]

Definition at line 295 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::updateChain ( std::map< std::string, double > &  joint_angles,
KDL::Chain chain,
tf::Pose output_pose 
) [protected, virtual]

Definition at line 325 of file footcoords.cpp.

Definition at line 903 of file footcoords.cpp.

Definition at line 153 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::updateRobotModel ( std::map< std::string, double > &  joint_angles) [protected, virtual]

Definition at line 348 of file footcoords.cpp.

Definition at line 874 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::waitForSensorFrameTransformation ( const ros::Time lstamp,
const ros::Time rstamp,
const std::string lsensor_frame,
const std::string rsensor_frame 
) [protected, virtual]

Definition at line 851 of file footcoords.cpp.


Member Data Documentation

Definition at line 244 of file footcoords.h.

Definition at line 224 of file footcoords.h.

Definition at line 234 of file footcoords.h.

Definition at line 241 of file footcoords.h.

Definition at line 202 of file footcoords.h.

Definition at line 195 of file footcoords.h.

Definition at line 194 of file footcoords.h.

Definition at line 223 of file footcoords.h.

Definition at line 237 of file footcoords.h.

Definition at line 186 of file footcoords.h.

Definition at line 236 of file footcoords.h.

Definition at line 228 of file footcoords.h.

Definition at line 196 of file footcoords.h.

Definition at line 229 of file footcoords.h.

Definition at line 198 of file footcoords.h.

Definition at line 231 of file footcoords.h.

Definition at line 200 of file footcoords.h.

Definition at line 240 of file footcoords.h.

Definition at line 238 of file footcoords.h.

Definition at line 219 of file footcoords.h.

Definition at line 179 of file footcoords.h.

Definition at line 187 of file footcoords.h.

Definition at line 235 of file footcoords.h.

Definition at line 181 of file footcoords.h.

Definition at line 183 of file footcoords.h.

Definition at line 180 of file footcoords.h.

Definition at line 221 of file footcoords.h.

Definition at line 185 of file footcoords.h.

Definition at line 217 of file footcoords.h.

Definition at line 218 of file footcoords.h.

Definition at line 182 of file footcoords.h.

Definition at line 225 of file footcoords.h.

Definition at line 242 of file footcoords.h.

Definition at line 243 of file footcoords.h.

Definition at line 204 of file footcoords.h.

Definition at line 207 of file footcoords.h.

Definition at line 208 of file footcoords.h.

Definition at line 209 of file footcoords.h.

Definition at line 205 of file footcoords.h.

Definition at line 212 of file footcoords.h.

Definition at line 211 of file footcoords.h.

Definition at line 203 of file footcoords.h.

Definition at line 206 of file footcoords.h.

Definition at line 210 of file footcoords.h.

Definition at line 246 of file footcoords.h.

Definition at line 197 of file footcoords.h.

Definition at line 230 of file footcoords.h.

Definition at line 199 of file footcoords.h.

Definition at line 232 of file footcoords.h.

Definition at line 201 of file footcoords.h.

Definition at line 233 of file footcoords.h.

Definition at line 239 of file footcoords.h.

Definition at line 245 of file footcoords.h.

Definition at line 191 of file footcoords.h.

Definition at line 189 of file footcoords.h.

Definition at line 190 of file footcoords.h.

Definition at line 192 of file footcoords.h.

Definition at line 220 of file footcoords.h.

Definition at line 193 of file footcoords.h.

Definition at line 188 of file footcoords.h.

Definition at line 214 of file footcoords.h.

Definition at line 213 of file footcoords.h.

Definition at line 226 of file footcoords.h.

Definition at line 227 of file footcoords.h.

Eigen::Vector3f jsk_footstep_controller::Footcoords::zmp_ [protected]

Definition at line 222 of file footcoords.h.

Definition at line 216 of file footcoords.h.


The documentation for this class was generated from the following files:


jsk_footstep_controller
Author(s):
autogenerated on Fri Apr 19 2019 03:45:37