IRI ROS Specific Algorithm Class. More...

#include <trajectory_3Dscans_2_pointcloud_alg_node.h>

Inheritance diagram for Trajectory3DScans2PointcloudAlgNode:

Public Member Functions
	Trajectory3DScans2PointcloudAlgNode (void)
	Constructor.
	~Trajectory3DScans2PointcloudAlgNode (void)
	Destructor.
Protected Member Functions
void	add_to_PointCloud_msg (const sensor_msgs::PointCloud2 &newPointCloud)
	add to point cloud
void	addNodeDiagnostics (void)
	node add diagnostics
void	clear_PointCloud_msg ()
	laser scan to point cloud
geometry_msgs::Pose	interpole_slice_pose (const uint &id, const iri_poseslam::Trajectory &trajectory) const
	interpole slice pose
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	recompute_PointCloud_msg (const iri_poseslam::Trajectory &trajectory)
	recompute PointCloud msg
std::pair< uint, double >	search_interpolation (const sensor_msgs::PointCloud2 &slice, const iri_poseslam::Trajectory &trajectory) const
	search interpolation
sensor_msgs::PointCloud2	transform_point_cloud (const sensor_msgs::PointCloud2 &pcloud, const geometry_msgs::Pose &pose)
	transform point cloud
Matrix4f	transformation_matrix (const float x, const float y, const float z, const float alpha) const
	transformation matrix
Private Member Functions
void	slices3D_callback (const sensor_msgs::PointCloud2::ConstPtr &msg)
void	trajectory_callback (const iri_poseslam::Trajectory::ConstPtr &msg)
Private Attributes
bool	emptyPointCloud_
std::vector< std::pair< uint, double > >	interpolation_buffer_
sensor_msgs::PointCloud2	PointCloud_msg_
std::vector < sensor_msgs::PointCloud2 >	slice_buffer_
ros::Publisher	slices3D_pointcloud_publisher_
ros::Subscriber	slices3D_subscriber_
bool	started_
Matrix4f	T_laser_frame
std::vector < sensor_msgs::PointCloud2 >	trajectory_slice_buffer_
ros::Subscriber	trajectory_subscriber_

Detailed Description

IRI ROS Specific Algorithm Class.

Definition at line 56 of file trajectory_3Dscans_2_pointcloud_alg_node.h.

Constructor & Destructor Documentation

Trajectory3DScans2PointcloudAlgNode::Trajectory3DScans2PointcloudAlgNode ( void )

Constructor.

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

Definition at line 3 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

Trajectory3DScans2PointcloudAlgNode::~Trajectory3DScans2PointcloudAlgNode ( void )

Destructor.

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

Definition at line 28 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

Member Function Documentation

void Trajectory3DScans2PointcloudAlgNode::add_to_PointCloud_msg ( const sensor_msgs::PointCloud2 & newPointCloud ) [protected]

add to point cloud

Add all points of a point cloud in the point cloud message

Parameters:

newPointCloud The PointCloud2 to add at PointCloud2 message

Definition at line 153 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::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< Trajectory3DScans2PointcloudAlgorithm >.

Definition at line 90 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::clear_PointCloud_msg ( ) [protected]

laser scan to point cloud

Clear the point cloud message

Definition at line 180 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

geometry_msgs::Pose Trajectory3DScans2PointcloudAlgNode::interpole_slice_pose	(	const uint &	id,
		const iri_poseslam::Trajectory &	trajectory
	)		const `[protected]`

interpole slice pose

This function computes the pose of a slice interpolating poses of a trajectory message.

Parameters:

id	of the slice from which compute the pose
trajectory	the trajectory message for compute the interpolation

Returns:: pose of the slice

Definition at line 190 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::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< Trajectory3DScans2PointcloudAlgorithm >.

Definition at line 33 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::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< Trajectory3DScans2PointcloudAlgorithm >.

Definition at line 79 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

double Trajectory3DScans2PointcloudAlgNode::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 261 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::recompute_PointCloud_msg ( const iri_poseslam::Trajectory & trajectory ) [protected]

recompute PointCloud msg

Recompute the PointCloud message after a new trajectory msg received.

Parameters:

trajectory the new trajectory msg

Definition at line 100 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

std::pair< uint, double > Trajectory3DScans2PointcloudAlgNode::search_interpolation	(	const sensor_msgs::PointCloud2 &	slice,
		const iri_poseslam::Trajectory &	trajectory
	)		const `[protected]`

search interpolation

This function finds the index of the first of two poses of trajectory between the slice have been taken and the factor of interpolation

Parameters:

slice	from which compute the interpolation
trajectory	the trajectory message for compute the interpolation

Returns:: pair of uint index of the first pose and double the interpolation factor

Definition at line 213 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::slices3D_callback ( const sensor_msgs::PointCloud2::ConstPtr & msg ) [private]

Definition at line 45 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

void Trajectory3DScans2PointcloudAlgNode::trajectory_callback ( const iri_poseslam::Trajectory::ConstPtr & msg ) [private]

Definition at line 58 of file trajectory_3Dscans_2_pointcloud_alg_node.cpp.

sensor_msgs::PointCloud2 Trajectory3DScans2PointcloudAlgNode::transform_point_cloud	(	const sensor_msgs::PointCloud2 &	pcloud,
		const geometry_msgs::Pose &	pose
	)		`[protected]`