CobFrameTracker Class Reference

#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. More...
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: More...
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. More...
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. More...
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 639 of file cob_frame_tracker.cpp.

void CobFrameTracker::action_success

(

)

Definition at line 625 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 817 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 785 of file cob_frame_tracker.cpp.

int CobFrameTracker::checkServiceCallStatus ( )

private

Definition at line 702 of file cob_frame_tracker.cpp.

int CobFrameTracker::checkStatus ( )

private

ABORTION CRITERIA:

ToDo: better metric for when goal is reached

Definition at line 653 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 833 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 579 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 733 of file cob_frame_tracker.cpp.

void CobFrameTracker::preemptCB

(

)

Definition at line 610 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());

Definition at line 243 of file cob_frame_tracker.cpp.

void CobFrameTracker::publishZeroTwist

(

)

Definition at line 233 of file cob_frame_tracker.cpp.

void CobFrameTracker::reconfigureCallback ( cob_frame_tracker::FrameTrackerConfig &  config, uint32_t  level  )

private

Definition at line 773 of file cob_frame_tracker.cpp.

void CobFrameTracker::run

(

const ros::TimerEvent &

event

)

Definition at line 176 of file cob_frame_tracker.cpp.

bool CobFrameTracker::startLookatCallback	(	cob_srvs::SetString::Request &	request,
		cob_srvs::SetString::Response &	response
	)

Definition at line 440 of file cob_frame_tracker.cpp.

bool CobFrameTracker::startTrackingCallback	(	cob_srvs::SetString::Request &	request,
		cob_srvs::SetString::Response &	response
	)

Definition at line 390 of file cob_frame_tracker.cpp.

bool CobFrameTracker::stopCallback	(	std_srvs::Trigger::Request &	request,
		std_srvs::Trigger::Response &	response
	)

Definition at line 529 of file cob_frame_tracker.cpp.

Member Data Documentation

unsigned int CobFrameTracker::abortion_counter_

private

Definition at line 186 of file cob_frame_tracker.h.

cob_frame_tracker::FrameTrackingFeedback CobFrameTracker::action_feedback_

private

Definition at line 153 of file cob_frame_tracker.h.

std::string CobFrameTracker::action_name_

private

Action interface.

Definition at line 150 of file cob_frame_tracker.h.

cob_frame_tracker::FrameTrackingResult CobFrameTracker::action_result_

private

Definition at line 154 of file cob_frame_tracker.h.

boost::shared_ptr<SAS_FrameTrackingAction_t> CobFrameTracker::as_

private

Definition at line 151 of file cob_frame_tracker.h.

double CobFrameTracker::cart_distance_

private

Definition at line 183 of file cob_frame_tracker.h.

double CobFrameTracker::cart_min_dist_threshold_lin_

private

Definition at line 173 of file cob_frame_tracker.h.

double CobFrameTracker::cart_min_dist_threshold_rot_

private

Definition at line 174 of file cob_frame_tracker.h.

KDL::Chain CobFrameTracker::chain_

private

KDL Conversion.

Definition at line 133 of file cob_frame_tracker.h.

std::string CobFrameTracker::chain_base_link_

private

Definition at line 109 of file cob_frame_tracker.h.

std::string CobFrameTracker::chain_tip_link_

private

Definition at line 110 of file cob_frame_tracker.h.

KDL::Twist CobFrameTracker::current_twist_

private

Definition at line 180 of file cob_frame_tracker.h.

unsigned int CobFrameTracker::dof_

private

Definition at line 119 of file cob_frame_tracker.h.

bool CobFrameTracker::enable_abortion_checking_

private

Definition at line 172 of file cob_frame_tracker.h.

ros::Publisher CobFrameTracker::error_pub_

private

Definition at line 142 of file cob_frame_tracker.h.

HoldTf CobFrameTracker::ht_

private

Definition at line 101 of file cob_frame_tracker.h.

boost::shared_ptr<KDL::ChainFkSolverVel_recursive> CobFrameTracker::jntToCartSolver_vel_

private

Definition at line 136 of file cob_frame_tracker.h.

std::vector<std::string> CobFrameTracker::joints_

private

Definition at line 118 of file cob_frame_tracker.h.

ros::Subscriber CobFrameTracker::jointstate_sub_

private

Definition at line 140 of file cob_frame_tracker.h.

KDL::JntArray CobFrameTracker::last_q_

private

Definition at line 134 of file cob_frame_tracker.h.

KDL::JntArray CobFrameTracker::last_q_dot_

private

Definition at line 135 of file cob_frame_tracker.h.

bool CobFrameTracker::lookat_

private

Definition at line 108 of file cob_frame_tracker.h.

std::string CobFrameTracker::lookat_focus_frame_

private

Definition at line 111 of file cob_frame_tracker.h.

unsigned int CobFrameTracker::max_abortions_

private

Definition at line 187 of file cob_frame_tracker.h.

double CobFrameTracker::max_vel_lin_

private

Definition at line 115 of file cob_frame_tracker.h.

double CobFrameTracker::max_vel_rot_

private

Definition at line 116 of file cob_frame_tracker.h.

bool CobFrameTracker::movable_rot_

private

Definition at line 122 of file cob_frame_tracker.h.

bool CobFrameTracker::movable_trans_

private

Definition at line 121 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_rot_x_

private

Definition at line 128 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_rot_y_

private

< Internal PID controller.

Definition at line 129 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_rot_z_

private

Definition at line 130 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_trans_x_

private

Definition at line 124 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_trans_y_

private

< Internal PID controller.

Definition at line 125 of file cob_frame_tracker.h.

control_toolbox::Pid CobFrameTracker::pid_controller_trans_z_

private

Definition at line 126 of file cob_frame_tracker.h.

boost::recursive_mutex CobFrameTracker::reconfig_mutex_

private

Definition at line 156 of file cob_frame_tracker.h.

ros::ServiceClient CobFrameTracker::reconfigure_client_

private

Definition at line 147 of file cob_frame_tracker.h.

boost::shared_ptr< dynamic_reconfigure::Server<cob_frame_tracker::FrameTrackerConfig> > CobFrameTracker::reconfigure_server_

private

Definition at line 157 of file cob_frame_tracker.h.

double CobFrameTracker::rot_distance_

private

Definition at line 184 of file cob_frame_tracker.h.

ros::ServiceServer CobFrameTracker::start_lookat_server_

private

Definition at line 145 of file cob_frame_tracker.h.

ros::ServiceServer CobFrameTracker::start_tracking_server_

private

Definition at line 144 of file cob_frame_tracker.h.

bool CobFrameTracker::stop_on_goal_

private

Definition at line 168 of file cob_frame_tracker.h.

ros::ServiceServer CobFrameTracker::stop_server_

private

Definition at line 146 of file cob_frame_tracker.h.

std::string CobFrameTracker::target_frame_

private

Definition at line 113 of file cob_frame_tracker.h.

KDL::Twist CobFrameTracker::target_twist_

private

Definition at line 181 of file cob_frame_tracker.h.

tf::TransformListener CobFrameTracker::tf_listener_

private

Definition at line 138 of file cob_frame_tracker.h.

ros::Timer CobFrameTracker::timer_

private

Definition at line 104 of file cob_frame_tracker.h.

bool CobFrameTracker::tracking_

private

Definition at line 106 of file cob_frame_tracker.h.

double CobFrameTracker::tracking_duration_

private

Definition at line 169 of file cob_frame_tracker.h.

std::string CobFrameTracker::tracking_frame_

private

Definition at line 112 of file cob_frame_tracker.h.

bool CobFrameTracker::tracking_goal_

private

Definition at line 107 of file cob_frame_tracker.h.

ros::Time CobFrameTracker::tracking_start_time_

private

Definition at line 170 of file cob_frame_tracker.h.

double CobFrameTracker::twist_dead_threshold_lin_

private

Definition at line 175 of file cob_frame_tracker.h.

double CobFrameTracker::twist_dead_threshold_rot_

private

Definition at line 176 of file cob_frame_tracker.h.

double CobFrameTracker::twist_deviation_threshold_lin_

private

Definition at line 177 of file cob_frame_tracker.h.

double CobFrameTracker::twist_deviation_threshold_rot_

private

Definition at line 178 of file cob_frame_tracker.h.

ros::Publisher CobFrameTracker::twist_pub_

private

Definition at line 141 of file cob_frame_tracker.h.

double CobFrameTracker::update_rate_

private

Definition at line 103 of file cob_frame_tracker.h.

---

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

• cob_frame_tracker.h
• cob_frame_tracker.cpp