IRI ROS Specific Algorithm Class. More...

#include <kinton_arm_task_priority_control_alg_node.h>

Inheritance diagram for KintonArmTaskPriorityControlAlgNode:

Public Member Functions
	KintonArmTaskPriorityControlAlgNode (void)
	Constructor.
	~KintonArmTaskPriorityControlAlgNode (void)
	Destructor.
Protected Member Functions
void	addNodeDiagnostics (void)
	node add diagnostics
Matrix4d	get_HT_cam_in_tip (const string &cam_frame, const string &tip_frame)
	Get Homogenous Transform between cam and arm tip.
Matrix4d	getTransform (const string &frame1, const string &frame2)
	Get transform matrix.
void	init_arm ()
	Arm Initialization.
void	mainNodeThread (void)
	main node thread
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
bool	readJoints (urdf::Model &robot_model, const string &root_name, const string &tip_name)
	Get Joints information.
Private Member Functions
void	cam_vel_callback (const geometry_msgs::TwistWithCovariance::ConstPtr &msg)
void	input_tag_callback (const ar_pose::ARMarkers::ConstPtr &msg)
void	odom_callback (const nav_msgs::Odometry::ConstPtr &msg)
Private Attributes
bool	activate_
bool	arm_catec_
bool	arm_unina_
MatrixXd	cam_vel_
CMutex	cam_vel_mutex_
bool	cam_vel_ok_
ros::Subscriber	cam_vel_subscriber_
double	dt_
bool	enable_sec_task_
MatrixXd	gain_sec_task_
bool	init_
CMutex	input_tag_mutex_
ros::Subscriber	input_tag_subscriber_
vector< arm_joint >	joint_info_
ros::Publisher	joints_pub_publisher_
sensor_msgs::JointState	JointState_msg_
Chain	kdl_chain_
MatrixXd	lambda_robot_
int	num_joints_
CMutex	odom_mutex_
ros::Subscriber	odom_subscriber_
geometry_msgs::Pose	pose_
MatrixXd	q_
MatrixXd	q_rollpitch_
ros::Publisher	quad_vel_publisher_
KintonArmTaskPriorityControlAlgorithm	task_priority_ctrl_
Matrix4d	Tcam_in_tip_
ros::Time	time_
ros::Time	time_last_
Matrix4d	Ttag_in_cam_
geometry_msgs::Twist	Twist_msg_
MatrixXd	v_rollpitch_
MatrixXd	vel_

Detailed Description

IRI ROS Specific Algorithm Class.

Definition at line 52 of file kinton_arm_task_priority_control_alg_node.h.

Constructor & Destructor Documentation

KintonArmTaskPriorityControlAlgNode::KintonArmTaskPriorityControlAlgNode ( void )

Constructor.

This constructor initializes specific class attributes and all ROS communications variables to enable message exchange.

Definition at line 7 of file kinton_arm_task_priority_control_alg_node.cpp.

KintonArmTaskPriorityControlAlgNode::~KintonArmTaskPriorityControlAlgNode ( void )

Destructor.

This destructor frees all necessary dynamic memory allocated within this this class.

Definition at line 92 of file kinton_arm_task_priority_control_alg_node.cpp.

Member Function Documentation

void KintonArmTaskPriorityControlAlgNode::addNodeDiagnostics ( void ) [protected, virtual]

node add diagnostics

In this abstract function additional ROS diagnostics applied to the specific algorithms may be added.

Implements algorithm_base::IriBaseAlgorithm< KintonArmTaskPriorityControlAlgorithm >.

Definition at line 386 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::cam_vel_callback ( const geometry_msgs::TwistWithCovariance::ConstPtr & msg ) [private]

Definition at line 164 of file kinton_arm_task_priority_control_alg_node.cpp.

Matrix4d KintonArmTaskPriorityControlAlgNode::get_HT_cam_in_tip	(	const string &	cam_frame,
		const string &	tip_frame
	)		`[protected]`

Get Homogenous Transform between cam and arm tip.

Get fixed Homogenous Transform of the camera in tip frames

Definition at line 446 of file kinton_arm_task_priority_control_alg_node.cpp.

Matrix4d KintonArmTaskPriorityControlAlgNode::getTransform	(	const string &	frame1,
		const string &	frame2
	)		`[protected]`

Get transform matrix.

From tf to Eigen 4x4 homogenous transform

Definition at line 285 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::init_arm ( ) [protected]

Arm Initialization.

Arm Variables initializations.

Definition at line 319 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::input_tag_callback ( const ar_pose::ARMarkers::ConstPtr & msg ) [private]

Definition at line 232 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::mainNodeThread ( void ) [protected, virtual]

main node thread

This is the main thread node function. Code written here will be executed in every node loop while the algorithm is on running state. Loop frequency can be tuned by modifying loop_rate attribute.

Here data related to the process loop or to ROS topics (mainly data structs related to the MSG and SRV files) must be updated. ROS publisher objects must publish their data in this process. ROS client servers may also request data to the corresponding server topics.

Implements algorithm_base::IriBaseAlgorithm< KintonArmTaskPriorityControlAlgorithm >.

Definition at line 97 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::node_config_update	(	Config &	config,
		uint32_t	level
	)		`[protected, virtual]`

dynamic reconfigure server callback

This method is called whenever a new configuration is received through the dynamic reconfigure. The derivated generic algorithm class must implement it.

Parameters:

config	an object with new configuration from all algorithm parameters defined in the config file.
level	integer referring the level in which the configuration has been changed.

Implements algorithm_base::IriBaseAlgorithm< KintonArmTaskPriorityControlAlgorithm >.

Definition at line 361 of file kinton_arm_task_priority_control_alg_node.cpp.

void KintonArmTaskPriorityControlAlgNode::odom_callback ( const nav_msgs::Odometry::ConstPtr & msg ) [private]

Definition at line 192 of file kinton_arm_task_priority_control_alg_node.cpp.