#include <cob_frame_tracker.h>

Public Member Functions
void	action_abort ()
void	action_success ()
	CobFrameTracker ()
bool	getTransform (const std::string &from, const std::string &to, tf::StampedTransform &stamped_tf)
void	goalCB ()
	Action interface.
bool	initialize ()
void	jointstateCallback (const sensor_msgs::JointState::ConstPtr &msg)
void	preemptCB ()
void	publishHoldTwist (const ros::Duration &period)
void	publishTwist (ros::Duration period, bool do_publish=true)
void	publishZeroTwist ()
void	run (const ros::TimerEvent &event)
bool	startLookatCallback (cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
bool	startTrackingCallback (cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
bool	stopCallback (std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
	~CobFrameTracker ()
Private Member Functions
bool	checkCartDistanceViolation (const double dist, const double rot)
bool	checkInfinitesimalTwist (const KDL::Twist current)
int	checkServiceCallStatus ()
int	checkStatus ()
	ABORTION CRITERIA:
bool	checkTwistViolation (const KDL::Twist current, const KDL::Twist target)
void	reconfigureCallback (cob_frame_tracker::FrameTrackerConfig &config, uint32_t level)
Private Attributes
unsigned int	abortion_counter_
cob_frame_tracker::FrameTrackingFeedback	action_feedback_
std::string	action_name_
	Action interface.
cob_frame_tracker::FrameTrackingResult	action_result_
boost::shared_ptr < SAS_FrameTrackingAction_t >	as_
double	cart_distance_
double	cart_min_dist_threshold_lin_
double	cart_min_dist_threshold_rot_
KDL::Chain	chain_
	KDL Conversion.
std::string	chain_base_link_
std::string	chain_tip_link_
KDL::Twist	current_twist_
unsigned int	dof_
bool	enable_abortion_checking_
ros::Publisher	error_pub_
HoldTf	ht_
boost::shared_ptr < KDL::ChainFkSolverVel_recursive >	jntToCartSolver_vel_
std::vector< std::string >	joints_
ros::Subscriber	jointstate_sub_
KDL::JntArray	last_q_
KDL::JntArray	last_q_dot_
bool	lookat_
std::string	lookat_focus_frame_
unsigned int	max_abortions_
double	max_vel_lin_
double	max_vel_rot_
bool	movable_rot_
bool	movable_trans_
control_toolbox::Pid	pid_controller_rot_x_
control_toolbox::Pid	pid_controller_rot_y_
control_toolbox::Pid	pid_controller_rot_z_
control_toolbox::Pid	pid_controller_trans_x_
control_toolbox::Pid	pid_controller_trans_y_
control_toolbox::Pid	pid_controller_trans_z_
boost::recursive_mutex	reconfig_mutex_
ros::ServiceClient	reconfigure_client_
boost::shared_ptr < dynamic_reconfigure::Server < cob_frame_tracker::FrameTrackerConfig > >	reconfigure_server_
double	rot_distance_
ros::ServiceServer	start_lookat_server_
ros::ServiceServer	start_tracking_server_
bool	stop_on_goal_
ros::ServiceServer	stop_server_
std::string	target_frame_
KDL::Twist	target_twist_
tf::TransformListener	tf_listener_
ros::Timer	timer_
bool	tracking_
double	tracking_duration_
std::string	tracking_frame_
bool	tracking_goal_
ros::Time	tracking_start_time_
double	twist_dead_threshold_lin_
double	twist_dead_threshold_rot_
double	twist_deviation_threshold_lin_
double	twist_deviation_threshold_rot_
ros::Publisher	twist_pub_
double	update_rate_

Detailed Description

Definition at line 64 of file cob_frame_tracker.h.

Constructor & Destructor Documentation

CobFrameTracker::CobFrameTracker ( ) [inline]

Definition at line 67 of file cob_frame_tracker.h.

CobFrameTracker::~CobFrameTracker ( ) [inline]

Definition at line 72 of file cob_frame_tracker.h.

Member Function Documentation

void CobFrameTracker::action_abort ( )

Definition at line 618 of file cob_frame_tracker.cpp.

void CobFrameTracker::action_success ( )

Definition at line 604 of file cob_frame_tracker.cpp.

bool CobFrameTracker::checkCartDistanceViolation	(	const double	dist,
		const double	rot
	)		`[private]`

checks whether the Cartesian distance between tip and target frame is ok

Cartesian distance is acceptable -> no violation

Definition at line 796 of file cob_frame_tracker.cpp.

bool CobFrameTracker::checkInfinitesimalTwist ( const KDL::Twist current ) [private]

checks whether the twist is infinitesimally small

all twist velocities are <= dead_threshold -> twist is infinitesimal

Definition at line 764 of file cob_frame_tracker.cpp.

int CobFrameTracker::checkServiceCallStatus ( ) [private]

Definition at line 681 of file cob_frame_tracker.cpp.

int CobFrameTracker::checkStatus ( ) [private]

ABORTION CRITERIA:

ToDo: better metric for when goal is reached

Definition at line 632 of file cob_frame_tracker.cpp.

bool CobFrameTracker::checkTwistViolation	(	const KDL::Twist	current,
		const KDL::Twist	target
	)		`[private]`

checks whether the current twist is following the target twist "close enough"

Cartesian Twist distance is acceptable -> no violation

Definition at line 812 of file cob_frame_tracker.cpp.

bool CobFrameTracker::getTransform	(	const std::string &	from,
		const std::string &	to,
		tf::StampedTransform &	stamped_tf
	)

Definition at line 215 of file cob_frame_tracker.cpp.

void CobFrameTracker::goalCB ( )

Action interface.

Definition at line 558 of file cob_frame_tracker.cpp.

bool CobFrameTracker::initialize ( )

get params

Definition at line 35 of file cob_frame_tracker.cpp.

void CobFrameTracker::jointstateCallback ( const sensor_msgs::JointState::ConstPtr & msg )

---------------------------------------------------------------------

Definition at line 712 of file cob_frame_tracker.cpp.

void CobFrameTracker::preemptCB ( )

Definition at line 589 of file cob_frame_tracker.cpp.

void CobFrameTracker::publishHoldTwist ( const ros::Duration & period )

Definition at line 338 of file cob_frame_tracker.cpp.

void CobFrameTracker::publishTwist	(	ros::Duration	period,
		bool	do_publish = `true`
	)

ToDo: Consider angular error as RPY or Quaternion? ToDo: What to do about sign conversion (pi->-pi) in angular rotation?

debug only

if (std::fabs(transform_tf.getOrigin().x()) >= max_vel_lin_) ROS_WARN("Twist.linear.x: %f exceeds limit %f", transform_tf.getOrigin().x(), max_vel_lin_); if (std::fabs(transform_tf.getOrigin().y()) >= max_vel_lin_) ROS_WARN("Twist.linear.y: %f exceeds limit %f", transform_tf.getOrigin().y(), max_vel_lin_); if (std::fabs(transform_tf.getOrigin().z()) >= max_vel_lin_) ROS_WARN("Twist.linear.z: %f exceeds limit %f", transform_tf.getOrigin().z(), max_vel_lin_); if (std::fabs(transform_tf.getOrigin().x()) >= max_vel_rot_) ROS_WARN("Twist.angular.x: %f exceeds limit %f", transform_tf.getOrigin().x(), max_vel_rot_); if (std::fabs(transform_tf.getOrigin().y()) >= max_vel_rot_) ROS_WARN("Twist.angular.y: %f exceeds limit %f", transform_tf.getOrigin().y(), max_vel_rot_); if (std::fabs(transform_tf.getOrigin().z()) >= max_vel_rot_) ROS_WARN("Twist.angular.z: %f exceeds limit %f", transform_tf.getOrigin().z(), max_vel_rot_);

twist_msg.twist.linear.x = copysign(std::min(max_vel_lin_, std::fabs(transform_tf.getOrigin().x())),transform_tf.getOrigin().x()); twist_msg.twist.linear.y = copysign(std::min(max_vel_lin_, std::fabs(transform_tf.getOrigin().y())),transform_tf.getOrigin().y()); twist_msg.twist.linear.z = copysign(std::min(max_vel_lin_, std::fabs(transform_tf.getOrigin().z())),transform_tf.getOrigin().z()); twist_msg.twist.angular.x = copysign(std::min(max_vel_rot_, std::fabs(transform_tf.getRotation().x())),transform_tf.getRotation().x()); twist_msg.twist.angular.y = copysign(std::min(max_vel_rot_, std::fabs(transform_tf.getRotation().y())),transform_tf.getRotation().y()); twist_msg.twist.angular.z = copysign(std::min(max_vel_rot_, std::fabs(transform_tf.getRotation().z())),transform_tf.getRotation().z());