#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	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

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

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

enum jsk_footstep_controller::Footcoords::OdomStatus

Enumerator
UNINITIALIZED
INITIALIZING
LLEG_SUPPORT
RLEG_SUPPORT

Definition at line 117 of file footcoords.h.

enum jsk_footstep_controller::Footcoords::SupportLegStatus

Enumerator
LLEG_GROUND
RLEG_GROUND
AIR
BOTH_GROUND

Definition at line 112 of file footcoords.h.

Constructor & Destructor Documentation

jsk_footstep_controller::Footcoords::Footcoords ( )

Definition at line 51 of file footcoords.cpp.

jsk_footstep_controller::Footcoords::~Footcoords ( )

virtual

Definition at line 151 of file footcoords.cpp.

Member Function Documentation

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

protectedvirtual

Definition at line 181 of file footcoords.cpp.

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

protectedvirtual

Definition at line 192 of file footcoords.cpp.

double jsk_footstep_controller::Footcoords::applyLowPassFilter	(	double	current_val,
		double	prev_val
	)		const

protectedvirtual

Definition at line 176 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::computeMidCoords ( const ros::Time & stamp )

protectedvirtual

Definition at line 806 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::computeMidCoordsFromSingleLeg	(	const ros::Time &	stamp,
		bool	use_left_leg
	)

protectedvirtual

Definition at line 761 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
	)

protectedvirtual

Definition at line 363 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometry ( )

protectedvirtual

Definition at line 474 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryMainSupportLeg ( )

protectedvirtual

Definition at line 542 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryNaive ( )

protectedvirtual

Definition at line 480 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::estimateOdometryZMPSupportLeg ( )

protectedvirtual

Definition at line 621 of file footcoords.cpp.

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

protectedvirtual

Definition at line 388 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::filter ( const jsk_footstep_controller::SynchronizedForces::ConstPtr & msg )

protectedvirtual

Definition at line 405 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::floorCoeffsCallback ( const pcl_msgs::ModelCoefficients & coeffs )

protectedvirtual

Definition at line 1051 of file footcoords.cpp.

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

protectedvirtual

Definition at line 937 of file footcoords.cpp.

float jsk_footstep_controller::Footcoords::getYaw ( const Eigen::Affine3d & pose )

protectedvirtual

Definition at line 945 of file footcoords.cpp.

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

protectedvirtual

Definition at line 1083 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::odomInitTriggerCallback ( const std_msgs::Empty & trigger )

protectedvirtual

Definition at line 1021 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::periodicTimerCallback ( const ros::TimerEvent & event )

protectedvirtual

Definition at line 249 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishContactState ( const ros::Time & stamp )

protectedvirtual

Definition at line 709 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishState ( const std::string & state )

protectedvirtual

Definition at line 702 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::publishTF ( const ros::Time & stamp )

protectedvirtual

Definition at line 954 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
	)

protectedvirtual

Definition at line 203 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
	)

protectedvirtual

Definition at line 298 of file footcoords.cpp.

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

protectedvirtual

Definition at line 328 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::updateGroundTF ( )

protectedvirtual

Definition at line 906 of file footcoords.cpp.

void jsk_footstep_controller::Footcoords::updateLegDiagnostics ( diagnostic_updater::DiagnosticStatusWrapper & stat )

protectedvirtual

Definition at line 156 of file footcoords.cpp.

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

protectedvirtual

Definition at line 351 of file footcoords.cpp.

bool jsk_footstep_controller::Footcoords::waitForEndEffectorTransformation ( const ros::Time & stamp )

protectedvirtual

Definition at line 877 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
	)

protectedvirtual

Definition at line 854 of file footcoords.cpp.

Member Data Documentation

double jsk_footstep_controller::Footcoords::alpha_

protected

Definition at line 244 of file footcoords.h.

bool jsk_footstep_controller::Footcoords::before_on_the_air_

protected

Definition at line 224 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::body_on_odom_frame_

protected

Definition at line 234 of file footcoords.h.

boost::shared_ptr<diagnostic_updater::Updater> jsk_footstep_controller::Footcoords::diagnostic_updater_

protected

Definition at line 241 of file footcoords.h.

tf::Pose jsk_footstep_controller::Footcoords::estimated_odom_pose_

protected

Definition at line 202 of file footcoords.h.

std::vector<float> jsk_footstep_controller::Footcoords::floor_coeffs_

protected

Definition at line 195 of file footcoords.h.

ros::Subscriber jsk_footstep_controller::Footcoords::floor_coeffs_sub_

protected

Definition at line 194 of file footcoords.h.

double jsk_footstep_controller::Footcoords::force_thr_

protected

Definition at line 223 of file footcoords.h.

tf::Transform jsk_footstep_controller::Footcoords::ground_transform_

protected

Definition at line 237 of file footcoords.h.

message_filters::Subscriber<sensor_msgs::Imu> jsk_footstep_controller::Footcoords::imu_sub_

protected

Definition at line 186 of file footcoords.h.

bool jsk_footstep_controller::Footcoords::invert_odom_init_

protected

Definition at line 236 of file footcoords.h.

ros::Time jsk_footstep_controller::Footcoords::last_time_

protected

Definition at line 228 of file footcoords.h.

KDL::Chain jsk_footstep_controller::Footcoords::lfoot_chain_

protected

Definition at line 196 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::lfoot_frame_id_

protected

Definition at line 229 of file footcoords.h.

tf::Pose jsk_footstep_controller::Footcoords::lfoot_pose_

protected

Definition at line 198 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::lfoot_sensor_frame_

protected

Definition at line 231 of file footcoords.h.

tf::Pose jsk_footstep_controller::Footcoords::lfoot_to_origin_pose_

protected

Definition at line 200 of file footcoords.h.

tf::Transform jsk_footstep_controller::Footcoords::locked_midcoords_to_odom_on_ground_

protected

Definition at line 240 of file footcoords.h.

tf::Transform jsk_footstep_controller::Footcoords::midcoords_

protected

Definition at line 238 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::midcoords_frame_id_

protected

Definition at line 219 of file footcoords.h.

boost::mutex jsk_footstep_controller::Footcoords::mutex_

protected

Definition at line 179 of file footcoords.h.

boost::shared_ptr<message_filters::Synchronizer<OdomImuSyncPolicy> > jsk_footstep_controller::Footcoords::odom_imu_sync_

protected

Definition at line 187 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::odom_init_frame_id_

protected

Definition at line 235 of file footcoords.h.

Eigen::Affine3d jsk_footstep_controller::Footcoords::odom_init_pose_

protected

Definition at line 181 of file footcoords.h.

ros::Subscriber jsk_footstep_controller::Footcoords::odom_init_trigger_sub_

protected

Definition at line 183 of file footcoords.h.

Eigen::Affine3d jsk_footstep_controller::Footcoords::odom_pose_

protected

Definition at line 180 of file footcoords.h.

OdomStatus jsk_footstep_controller::Footcoords::odom_status_

protected

Definition at line 221 of file footcoords.h.

message_filters::Subscriber<nav_msgs::Odometry> jsk_footstep_controller::Footcoords::odom_sub_

protected

Definition at line 185 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::output_frame_id_

protected

Definition at line 217 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::parent_frame_id_

protected

Definition at line 218 of file footcoords.h.

ros::Timer jsk_footstep_controller::Footcoords::periodic_update_timer_

protected

Definition at line 182 of file footcoords.h.

std::map<std::string, double> jsk_footstep_controller::Footcoords::prev_joints_

protected

Definition at line 225 of file footcoords.h.

double jsk_footstep_controller::Footcoords::prev_lforce_

protected

Definition at line 242 of file footcoords.h.

double jsk_footstep_controller::Footcoords::prev_rforce_

protected

Definition at line 243 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_contact_state_

protected

Definition at line 204 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_debug_lfoot_pos_

protected

Definition at line 207 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_debug_rfoot_pos_

protected

Definition at line 208 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_leg_odometory_

protected

Definition at line 209 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_low_level_

protected

Definition at line 205 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_odom_init_pose_stamped_

protected

Definition at line 212 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_odom_init_transform_

protected

Definition at line 211 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_state_

protected

Definition at line 203 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_synchronized_forces_

protected

Definition at line 206 of file footcoords.h.

ros::Publisher jsk_footstep_controller::Footcoords::pub_twist_

protected

Definition at line 210 of file footcoords.h.

bool jsk_footstep_controller::Footcoords::publish_odom_tf_

protected

Definition at line 246 of file footcoords.h.

KDL::Chain jsk_footstep_controller::Footcoords::rfoot_chain_

protected

Definition at line 197 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::rfoot_frame_id_

protected

Definition at line 230 of file footcoords.h.

tf::Pose jsk_footstep_controller::Footcoords::rfoot_pose_

protected

Definition at line 199 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::rfoot_sensor_frame_

protected

Definition at line 232 of file footcoords.h.

tf::Pose jsk_footstep_controller::Footcoords::rfoot_to_origin_pose_

protected

Definition at line 201 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::root_frame_id_

protected

Definition at line 233 of file footcoords.h.

tf::Transform jsk_footstep_controller::Footcoords::root_link_pose_

protected

Definition at line 239 of file footcoords.h.

double jsk_footstep_controller::Footcoords::sampling_time_

protected

Definition at line 245 of file footcoords.h.

message_filters::Subscriber<sensor_msgs::JointState> jsk_footstep_controller::Footcoords::sub_joint_states_

protected

Definition at line 191 of file footcoords.h.

message_filters::Subscriber<geometry_msgs::WrenchStamped> jsk_footstep_controller::Footcoords::sub_lfoot_force_

protected

Definition at line 189 of file footcoords.h.

message_filters::Subscriber<geometry_msgs::WrenchStamped> jsk_footstep_controller::Footcoords::sub_rfoot_force_

protected

Definition at line 190 of file footcoords.h.

message_filters::Subscriber<geometry_msgs::PointStamped> jsk_footstep_controller::Footcoords::sub_zmp_

protected

Definition at line 192 of file footcoords.h.

SupportLegStatus jsk_footstep_controller::Footcoords::support_status_

protected

Definition at line 220 of file footcoords.h.

boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > jsk_footstep_controller::Footcoords::sync_

protected

Definition at line 193 of file footcoords.h.

ros::Subscriber jsk_footstep_controller::Footcoords::synchronized_forces_sub_

protected

Definition at line 188 of file footcoords.h.

tf::TransformBroadcaster jsk_footstep_controller::Footcoords::tf_broadcaster_

protected

Definition at line 214 of file footcoords.h.

boost::shared_ptr<tf::TransformListener> jsk_footstep_controller::Footcoords::tf_listener_

protected

Definition at line 213 of file footcoords.h.

bool jsk_footstep_controller::Footcoords::use_imu_

protected

Definition at line 226 of file footcoords.h.

bool jsk_footstep_controller::Footcoords::use_imu_yaw_

protected

Definition at line 227 of file footcoords.h.

Eigen::Vector3f jsk_footstep_controller::Footcoords::zmp_

protected

Definition at line 222 of file footcoords.h.

std::string jsk_footstep_controller::Footcoords::zmp_frame_id_

protected

Definition at line 216 of file footcoords.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation