IRI ROS Specific Algorithm Class. More...

#include <poseslam_alg_node.h>

Inheritance diagram for PoseslamAlgNode:

Public Member Functions
	PoseslamAlgNode (void)
	Constructor.
	~PoseslamAlgNode (void)
	Destructor.
Protected Member Functions
void	addNodeDiagnostics (void)
	node add diagnostics
double	angle_between_2D_vectors (const Vector2d &v, const Vector2d &w) const
	angle between 2D vectors
void	augment_trajectories (const std_msgs::Header &header)
	augment trajectories
geometry_msgs::PoseWithCovariance	create_PoseWithCovariance (const VectorXd &last_pose, const std::vector< double > &last_cov)
	create posewithcovariace
geometry_msgs::PoseWithCovarianceStamped	create_PoseWithCovarianceStamped (const std_msgs::Header &header, const VectorXd &last_pose, const std::vector< double > &last_cov)
	create posewithcovariacestamped
void	mainNodeThread (void)
	main node thread
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
double	pi_2_pi (const double &angle) const
	pi to pi
void	print_results () const
	print results
std::vector< VectorXd >	recompute_segment (const VectorXd &new_initial_pose, const VectorXd &new_final_pose, const int &initial_step, const int &final_step)
	recompute segment
void	recompute_trajectory ()
	recompute trajectory
MatrixXd	rotation_matrix (const double &alpha) const
	rotation matrix
void	update_trajectories (const bool &LoopClosed)
	update trajectories
Private Attributes
double	dz_footprint_2_base_
uint	ended
ros::ServiceClient	get_link_client_
iri_poseslam::GetLink	get_link_srv_
std::vector< uint >	loops_
ros::Publisher	no_loops_trajectory_publisher_
uint	step_
iri_poseslam::Trajectory	Trajectory_msg_
iri_poseslam::Trajectory	Trajectory_msg_no_loops_
std::vector< VectorXd >	Trajectory_poses_
ros::Publisher	trajectory_publisher_

Detailed Description

IRI ROS Specific Algorithm Class.

Definition at line 45 of file poseslam_alg_node.h.

Constructor & Destructor Documentation

PoseslamAlgNode::PoseslamAlgNode ( void )

Constructor.

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

Definition at line 3 of file poseslam_alg_node.cpp.

PoseslamAlgNode::~PoseslamAlgNode ( void )

Destructor.

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

Definition at line 30 of file poseslam_alg_node.cpp.

Member Function Documentation

void PoseslamAlgNode::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< PoseslamAlgorithm >.

Definition at line 199 of file poseslam_alg_node.cpp.

double PoseslamAlgNode::angle_between_2D_vectors	(	const Vector2d &	v,
		const Vector2d &	w
	)		const `[protected]`

angle between 2D vectors

This function returns a double cointaining the angle (with sign) between 2 vectors in xy plane

Parameters:

v	first vector
w	second vector

Returns:: the angle betwen them

Definition at line 452 of file poseslam_alg_node.cpp.

void PoseslamAlgNode::augment_trajectories ( const std_msgs::Header & header ) [protected]

augment trajectories

Update the trajectories messages adding the new pose.

Parameters:

header of the new pose

Definition at line 209 of file poseslam_alg_node.cpp.

geometry_msgs::PoseWithCovariance PoseslamAlgNode::create_PoseWithCovariance	(	const VectorXd &	last_pose,
		const std::vector< double > &	last_cov
	)		`[protected]`

create posewithcovariace

create a posewithcovariance from:

Parameters:

vector	of the pose
std::vector<double>	of the covariance

Definition at line 425 of file poseslam_alg_node.cpp.

geometry_msgs::PoseWithCovarianceStamped PoseslamAlgNode::create_PoseWithCovarianceStamped	(	const std_msgs::Header &	header,
		const VectorXd &	last_pose,
		const std::vector< double > &	last_cov
	)		`[protected]`

create posewithcovariacestamped

create a posewithcovariancestamped from:

Parameters:

header
vector	of the pose
std::vector<double>	of the covariance

Definition at line 414 of file poseslam_alg_node.cpp.

void PoseslamAlgNode::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< PoseslamAlgorithm >.

Definition at line 35 of file poseslam_alg_node.cpp.

void PoseslamAlgNode::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< PoseslamAlgorithm >.

Definition at line 191 of file poseslam_alg_node.cpp.

double PoseslamAlgNode::pi_2_pi ( const double & angle ) const [protected]

pi to pi

This function returns the angle given in the (-pi, pi] interval

Parameters:

angle

Returns:: the (-pi, pi] angle

Definition at line 472 of file poseslam_alg_node.cpp.

void PoseslamAlgNode::print_results ( ) const [protected]

print results

Prints the time and covariances results in a matlab file.

Definition at line 260 of file poseslam_alg_node.cpp.

std::vector< VectorXd > PoseslamAlgNode::recompute_segment	(	const VectorXd &	new_initial_pose,
		const VectorXd &	new_final_pose,
		const int &	initial_step,
		const int &	final_step
	)		`[protected]`