IRI ROS Specific Algorithm Class. More...

#include <sensors_2_link_alg_node.h>

Inheritance diagram for Sensors2LinkAlgNode:

Public Member Functions
	Sensors2LinkAlgNode (void)
	Constructor.
	~Sensors2LinkAlgNode (void)
	Destructor.
Protected Member Functions
void	addNodeDiagnostics (void)
	node add diagnostics
void	base_2_laser_frame (Vector3d &prior_d)
	from base footprint frame to laser frame
Matrix3d	covariance_2_matrix (const geometry_msgs::PoseWithCovariance &pose) const
	covariance to matrix
geometry_msgs::PoseWithCovariance	eigen_2_posewithcovariance (const Vector3d &p, const Matrix3d &cov) const
	matrix and vector to posewithcovariance
std::pair< Vector3d, Matrix3d >	interpolate_odom_rel (const int &odom_rel_idx, const ros::Time scan_stamp)
	interpolate odometry
void	laser_2_base_frame (Vector3d &odom_ICP, Matrix3d &odom_ICP_cov)
	from laser frame to base footprint frame
void	mainNodeThread (void)
	main node thread
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
geometry_msgs::PoseWithCovarianceStamped	odometry_fusion (const sensor_msgs::LaserScan &laser_scan, const int &odom_rel_idx)
	odometry fusion
iri_poseslam::GetLink::Response	offline_odometry ()
	offline odometry
iri_poseslam::GetLink::Response	online_odometry ()
	online odometry
Vector3d	pose_2_vector (const geometry_msgs::Pose &pose) const
	pose to vector
Matrix3d	rotation_matrix (const double &alpha) const
	rotation matrix
geometry_msgs::Pose	vector_2_pose (const Vector3d &p) const
	matrix and vector to posewithcovariance
Private Member Functions
void	cmd_vel_callback (const geometry_msgs::Twist::ConstPtr &msg)
bool	get_linkCallback (iri_poseslam::GetLink::Request &req, iri_poseslam::GetLink::Response &res)
void	odom_relative_callback (const nav_msgs::Odometry::ConstPtr &msg)
void	scan_callback (const sensor_msgs::LaserScan::ConstPtr &msg)
Private Attributes
ros::Subscriber	cmd_vel_subscriber_
bool	currently_stopped
Vector3d	d_base_2_laser
Vector3d	d_local
bool	fusion_ready_
ros::ServiceServer	get_link_server_
ros::ServiceClient	get_relative_pose_client_
iri_laser_icp::GetRelativePose	get_relative_pose_srv_
double	ICP_covariance_correction_factor
Matrix3d	Jd
Matrix3d	Jp
std::vector < sensor_msgs::LaserScan >	laser_scan_buffer_
sensor_msgs::LaserScan	last_laser_scan_
bool	new_laser_scan_
std::vector < geometry_msgs::PoseWithCovarianceStamped >	odom_buffer_
Vector3d	odom_rel_
std::vector< Vector3d >	odom_rel_buffer_
Matrix3d	odom_rel_cov_
std::vector< Matrix3d >	odom_rel_cov_buffer_
ros::Time	odom_rel_time_
std::vector< ros::Time >	odom_rel_time_buffer_
ros::Subscriber	odom_relative_subscriber_
bool	online_mode
uint	prev_seq
Matrix3d	Q
sensor_msgs::LaserScan	ready_laser_scan_
uint	ready_odom_id_
ros::Subscriber	scan_subscriber_
bool	stopped_since_last_odom

Detailed Description

IRI ROS Specific Algorithm Class.

Definition at line 52 of file sensors_2_link_alg_node.h.

Constructor & Destructor Documentation

Sensors2LinkAlgNode::Sensors2LinkAlgNode ( void )

Constructor.

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

Definition at line 3 of file sensors_2_link_alg_node.cpp.

Sensors2LinkAlgNode::~Sensors2LinkAlgNode ( void )

Destructor.

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

Definition at line 36 of file sensors_2_link_alg_node.cpp.

Member Function Documentation

void Sensors2LinkAlgNode::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< Sensors2LinkAlgorithm >.

Definition at line 274 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::base_2_laser_frame ( Vector3d & prior_d ) [protected]

from base footprint frame to laser frame

This function changes the prior_d for a ICP to the laser frame.

Parameters:

prior_d the vector containing the prior estimation for ICP in base footprint frame

Definition at line 540 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::cmd_vel_callback ( const geometry_msgs::Twist::ConstPtr & msg ) [private]

Definition at line 156 of file sensors_2_link_alg_node.cpp.

Matrix3d Sensors2LinkAlgNode::covariance_2_matrix ( const geometry_msgs::PoseWithCovariance & pose ) const [protected]

covariance to matrix

This function converts the covariance of a PoseWithCovariance ros message to a eigen::Matrix3d

Parameters:

pose	PoseWithCovariance ros message

Returns:: the covariance matrix

Definition at line 393 of file sensors_2_link_alg_node.cpp.

geometry_msgs::PoseWithCovariance Sensors2LinkAlgNode::eigen_2_posewithcovariance	(	const Vector3d &	p,
		const Matrix3d &	cov
	)		const `[protected]`

matrix and vector to posewithcovariance

This function converts a pose (eigen::Vector3d) and a covariance (eigen::Matrix3d) to a posewithcovariance ros message.

Parameters:

p	pose vector
cov	covariance matrix

Returns:: posewithcovariance message

Definition at line 421 of file sensors_2_link_alg_node.cpp.

bool Sensors2LinkAlgNode::get_linkCallback	(	iri_poseslam::GetLink::Request &	req,
		iri_poseslam::GetLink::Response &	res
	)		`[private]`

Definition at line 165 of file sensors_2_link_alg_node.cpp.

std::pair< Vector3d, Matrix3d > Sensors2LinkAlgNode::interpolate_odom_rel	(	const int &	odom_rel_idx,
		const ros::Time	scan_stamp
	)		`[protected]`

interpolate odometry

This function computes the interpolation of two relative odometries in a midle stamp.

Parameters:

odom_rel_idx	the index of the last odometry to interpolate in the buffer
scan_stamp	the stamp of the moment of interpolation

Returns:: odometry (pose and covariance) interpolated

Definition at line 498 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::laser_2_base_frame	(	Vector3d &	odom_ICP,
		Matrix3d &	odom_ICP_cov
	)		`[protected]`

from laser frame to base footprint frame

This function changes the ICP link to the base footprint frame.

Parameters:

odom_ICP	the vector containing the link
odom_ICP_cov	the covariance matrix of the link

Definition at line 549 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::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< Sensors2LinkAlgorithm >.

Definition at line 41 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::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< Sensors2LinkAlgorithm >.

Definition at line 257 of file sensors_2_link_alg_node.cpp.

void Sensors2LinkAlgNode::odom_relative_callback ( const nav_msgs::Odometry::ConstPtr & msg ) [private]

Definition at line 69 of file sensors_2_link_alg_node.cpp.

geometry_msgs::PoseWithCovarianceStamped Sensors2LinkAlgNode::odometry_fusion	(	const sensor_msgs::LaserScan &	laser_scan,
		const int &	odom_rel_idx
	)		`[protected]`