IRI ROS Specific Algorithm Class. More...

#include <segway_rmp400_odom_6d_alg_node.h>

Inheritance diagram for SegwayRmp400OdomAlgNode:

Public Member Functions
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	SegwayRmp400OdomAlgNode (void)
	Constructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
	~SegwayRmp400OdomAlgNode (void)
	Destructor.
Protected Member Functions
void	addNodeDiagnostics (void)
	node add diagnostics
void	addNodeDiagnostics (void)
	node add diagnostics
void	addNodeDiagnostics (void)
	node add diagnostics
void	addNodeDiagnostics (void)
	node add diagnostics
void	addNodeDiagnostics (void)
	node add diagnostics
void	addNodeDiagnostics (void)
	node add diagnostics
void	mainNodeThread (void)
	main node thread
void	mainNodeThread (void)
	main node thread
void	mainNodeThread (void)
	main node thread
void	mainNodeThread (void)
	main node thread
void	mainNodeThread (void)
	main node thread
void	mainNodeThread (void)
	main node thread
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
void	node_config_update (Config &config, uint32_t level)
	dynamic reconfigure server callback
Protected Attributes
tf::TransformBroadcaster	odom_broadcaster_
Private Member Functions
void	imu_callback (const sensor_msgs::Imu::ConstPtr &msg)
void	imu_callback (const sensor_msgs::Imu::ConstPtr &msg)
void	imu_callback (const sensor_msgs::Imu::ConstPtr &msg)
double	pseudokalman3 (std::vector< double > v, std::vector< double > c)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
void	segway_status_callback (const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr &msg)
Private Attributes
geometry_msgs::Transform	accum_
std::string	base_link_id_
Eigen::Matrix3f	cov_
ros::Time	current_time_
Eigen::Vector4f	d
double	dth_
double	dx_local_accum_
double	epsilon
bool	first_imu_
bool	first_segway_status_
Eigen::Matrix4f	g
Eigen::Matrix4f	H
Eigen::Matrix3f	I
CMutex	imu_mutex_
ros::Subscriber	imu_subscriber_
Eigen::Matrix3f	Jd_
Eigen::Matrix3f	Jp_
ros::Time	last_time_
ros::Time	last_time_imu_
double	left_wheels_velocity_
std::string	odom_id_
ros::Publisher	odom_publisher_
ros::Publisher	odom_rel_publisher_
nav_msgs::Odometry	Odometry_msg_
nav_msgs::Odometry	Odometry_rel_msg_
double	offset_fwd_
double	offset_turn_
double	p_
double	pimu_
double	pitch_rate_
std::vector< double >	pk_
std::vector< double >	pkc_
geometry_msgs::PoseWithCovariance	pose_
bool	publish_odom_rel_
bool	publish_tf_
Eigen::Vector4f	q
Eigen::Matrix3f	Q_
Eigen::Matrix3f	R
double	r_
double	right_wheels_velocity_
double	rimu_
std::vector< double >	rk_
std::vector< double >	rkc_
double	roll_rate_
CMutex	segway_status_mutex_
ros::Subscriber	segway_status_subscriber_
double	sigma2_dth_
double	sigma_dx_
double	sigma_dx_fwd_
double	sigma_dx_turn_
double	sigma_thimu_
bool	six_d_
Eigen::Vector3f	t
Eigen::Vector3f	ta
Eigen::Vector3f	tb
std::string	tf_prefix_
double	th_
double	thimu_
geometry_msgs::Transform	transform_
geometry_msgs::TwistWithCovariance	twist_
Eigen::Vector3f	v
double	vpimu_
double	vrimu_
double	vT_
double	vthimu_
double	vyimu_
std::vector< double >	vyk_
std::vector< double >	vykc_
Eigen::Vector3f	w
Eigen::Vector3f	wt
Eigen::Matrix3f	wt_hat
double	wt_norm
Eigen::Vector3f	ww
double	y_
double	yaw_rate_
double	yimu_

Detailed Description

IRI ROS Specific Algorithm Class.

Definition at line 49 of file segway_rmp400_odom_6d_alg_node.h.

Constructor & Destructor Documentation

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

Definition at line 3 of file segway_rmp400_odom_6d_alg_node.cpp.

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

Definition at line 36 of file segway_rmp400_odom_6d_alg_node.cpp.

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode ( void )

Constructor.

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

SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode ( void )

Destructor.

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

Member Function Documentation

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

Definition at line 181 of file segway_rmp400_odom_6d_alg_node.cpp.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::imu_callback ( const sensor_msgs::Imu::ConstPtr & msg ) [private]

Definition at line 267 of file segway_rmp400_odom_6dex_fused_alg_node.cpp.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

Definition at line 41 of file segway_rmp400_odom_6d_alg_node.cpp.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

Definition at line 174 of file segway_rmp400_odom_6d_alg_node.cpp.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.

void SegwayRmp400OdomAlgNode::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< SegwayRmp400OdomAlgorithm >.