#include <footcoords.h>

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	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	waitForEndEffectorTrasnformation (const ros::Time &stamp)
virtual bool	waitForSensorFrameTransformation (const ros::Time &stamp, 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_
double	force_thr_
tf::Transform	ground_transform_
message_filters::Subscriber < sensor_msgs::Imu >	imu_sub_
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_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 95 of file footcoords.h.

Member Typedef Documentation

typedef message_filters::sync_policies::ExactTime< nav_msgs::Odometry, sensor_msgs::Imu> jsk_footstep_controller::Footcoords::OdomImuSyncPolicy

Definition at line 105 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 102 of file footcoords.h.

Member Enumeration Documentation

enum jsk_footstep_controller::Footcoords::OdomStatus

Enumerator:

UNINITIALIZED
INITIALIZING
LLEG_SUPPORT
RLEG_SUPPORT

Definition at line 115 of file footcoords.h.

enum jsk_footstep_controller::Footcoords::SupportLegStatus

Enumerator:

LLEG_GROUND
RLEG_GROUND
AIR
BOTH_GROUND

Definition at line 110 of file footcoords.h.

Constructor & Destructor Documentation

jsk_footstep_controller::Footcoords::Footcoords ( )

Definition at line 52 of file footcoords.cpp.

jsk_footstep_controller::Footcoords::~Footcoords ( ) [virtual]

Definition at line 143 of file footcoords.cpp.

Member Function Documentation

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

Definition at line 173 of file footcoords.cpp.

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

Definition at line 184 of file footcoords.cpp.

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

Definition at line 168 of file footcoords.cpp.

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

Definition at line 798 of file footcoords.cpp.

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

Definition at line 753 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 355 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometry ( ) [protected, virtual]

Definition at line 466 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryMainSupportLeg ( ) [protected, virtual]

Definition at line 534 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryNaive ( ) [protected, virtual]

Definition at line 472 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryZMPSupportLeg ( ) [protected, virtual]

Definition at line 613 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 380 of file footcoords.cpp.

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

Definition at line 397 of file footcoords.cpp.

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

Definition at line 928 of file footcoords.cpp.

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

Definition at line 936 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 1008 of file footcoords.cpp.

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

Definition at line 1001 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::periodicTimerCallback ( const ros::TimerEvent & event ) [protected, virtual]

Definition at line 241 of file footcoords.cpp.

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

Definition at line 701 of file footcoords.cpp.

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

Definition at line 694 of file footcoords.cpp.

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

Definition at line 945 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 195 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 290 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 320 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::updateGroundTF ( ) [protected, virtual]

Definition at line 897 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::updateLegDiagnostics ( diagnostic_updater::DiagnosticStatusWrapper & stat ) [protected, virtual]

Definition at line 148 of file footcoords.cpp.

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

Definition at line 343 of file footcoords.cpp.

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

Definition at line 868 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::waitForSensorFrameTransformation	(	const ros::Time &	stamp,
		const std::string &	lsensor_frame,
		const std::string &	rsensor_frame
	)		`[protected, virtual]`