#include <Node.h>

Public Member Functions
	Node ()

void	spin ()

virtual	~Node ()

Private Member Functions
bool	checkUpdateThresholds ()
	Check motion and time thresholds for AMCL update. More...

double	getYawFromTf (const tf::Transform &tf)
	Return yaw from a given TF. More...

void	initialPoseReceived (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)

void	odomCallback (const geometry_msgs::TransformStampedConstPtr &msg)

void	pointcloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)

void	publishGridSlice (const ros::TimerEvent &)

void	publishGridTf (const ros::TimerEvent &)

void	publishMapPointCloud (const ros::TimerEvent &)

void	publishParticles ()

void	rangeCallback (const rosinrange_msg::range_poseConstPtr &msg)

void	rvizMarkerPublish (const uint32_t anchor_id, const float r, const geometry_msgs::Point &uav, const geometry_msgs::Point &anchor)
	To show range sensors in Rviz. More...

void	setInitialPose (const tf::Transform &init_pose, const float x_dev, const float y_dev, const float z_dev, const float a_dev)
	Set the initial pose of the particle filter. More...

Private Attributes
bool	amcl_out_ { false }

tf::Transform	amcl_out_lastbase_2_odom_tf_

tf::Transform	base_2_odom_tf_

ros::Publisher	cloud_filter_pub_

Grid3d	grid3d_

nav_msgs::OccupancyGrid	grid_slice_msg_

ros::Publisher	grid_slice_pub_

ros::Timer	grid_slice_pub_timer_

tf::StampedTransform	grid_to_world_tf_

ros::Timer	grid_to_world_tf_timer_

ros::Subscriber	initialpose_sub_

tf::Transform	initodom_2_world_tf_

bool	is_odom_ { false }

tf::Transform	lastbase_2_world_tf_

Particle	lastmean_p_

tf::Transform	lastodom_2_world_tf_

tf::Transform	lastupdatebase_2_odom_tf_

sensor_msgs::PointCloud2	map_point_cloud_msg_

ros::Publisher	map_point_cloud_pub_

ros::Timer	map_point_cloud_pub_timer_

Particle	mean_p_

ros::Time	nextupdate_time_

ros::NodeHandle	nh_

ros::Publisher	odom_base_pub_

tf::Transform	odom_increment_tf

ros::Subscriber	odom_sub_

Parameters	parameters_

ros::Publisher	particles_pose_pub_

ParticleFilter	pf_

double	pitch_ { 0 }

ros::Subscriber	point_sub_

std::vector< Range >	range_data

ros::Publisher	range_markers_pub_

ros::Subscriber	range_sub_

double	roll_ { 0 }

Detailed Description

Definition at line 28 of file Node.h.

Constructor & Destructor Documentation

amcl3d::Node::Node ( )

explicit

Definition at line 27 of file Node.cpp.

amcl3d::Node::~Node ( )

virtual

Definition at line 32 of file Node.cpp.

Member Function Documentation

bool amcl3d::Node::checkUpdateThresholds ( )

private

Check motion and time thresholds for AMCL update.

Check translation threshold

Check yaw threshold

Definition at line 432 of file Node.cpp.

double amcl3d::Node::getYawFromTf ( const tf::Transform & tf )

private

Return yaw from a given TF.

Definition at line 488 of file Node.cpp.

void amcl3d::Node::initialPoseReceived ( const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg )

private

void amcl3d::Node::odomCallback ( const geometry_msgs::TransformStampedConstPtr & msg )

private

If the filter is not initialized then exit

Update roll and pitch from odometry

Check takeoff height

Check if AMCL went wrong (nan, inf)

Check jumps

Publish transform

Definition at line 265 of file Node.cpp.

void amcl3d::Node::pointcloudCallback ( const sensor_msgs::PointCloud2ConstPtr & msg )

private

Check if an update must be performed or not

Apply pass-though filter

Union

Perform particle prediction based on odometry

Compensate for the current roll and pitch of the base-link

Perform particle update based on current point-cloud

Clean the range buffer

Update time and transform information

Do the resampling if needed

Publish particles

Definition at line 130 of file Node.cpp.

void amcl3d::Node::publishGridSlice ( const ros::TimerEvent & )

private

Definition at line 93 of file Node.cpp.

void amcl3d::Node::publishGridTf ( const ros::TimerEvent & )

private

Definition at line 101 of file Node.cpp.

void amcl3d::Node::publishMapPointCloud ( const ros::TimerEvent & )

private

Definition at line 85 of file Node.cpp.

void amcl3d::Node::publishParticles ( )

private

If the filter is not initialized then exit

Build the msg based on the particles position and orientation

Publish particle cloud

Definition at line 111 of file Node.cpp.

void amcl3d::Node::rangeCallback ( const rosinrange_msg::range_poseConstPtr & msg )

private

Definition at line 401 of file Node.cpp.

void amcl3d::Node::rvizMarkerPublish	(	const uint32_t	anchor_id,
		const float	r,
		const geometry_msgs::Point &	uav,
		const geometry_msgs::Point &	anchor
	)

private

To show range sensors in Rviz.

Indicate if AMCL was lost

Publish marker

Definition at line 496 of file Node.cpp.

void amcl3d::Node::setInitialPose	(	const tf::Transform &	init_pose,
		const float	x_dev,
		const float	y_dev,
		const float	z_dev,
		const float	a_dev
	)

private

Set the initial pose of the particle filter.

Extract TFs for future updates Reset lastupdatebase_2_odom_tf_

Publish particles

Definition at line 460 of file Node.cpp.

void amcl3d::Node::spin ( )

Definition at line 37 of file Node.cpp.

Member Data Documentation

bool amcl3d::Node::amcl_out_ { false }

private

Definition at line 79 of file Node.h.

tf::Transform amcl3d::Node::amcl_out_lastbase_2_odom_tf_

private

Definition at line 88 of file Node.h.

tf::Transform amcl3d::Node::base_2_odom_tf_

private

Definition at line 88 of file Node.h.

ros::Publisher amcl3d::Node::cloud_filter_pub_

private

Definition at line 93 of file Node.h.

Grid3d amcl3d::Node::grid3d_

private

Definition at line 62 of file Node.h.

nav_msgs::OccupancyGrid amcl3d::Node::grid_slice_msg_

private

Definition at line 71 of file Node.h.

ros::Publisher amcl3d::Node::grid_slice_pub_

private

Definition at line 72 of file Node.h.

ros::Timer amcl3d::Node::grid_slice_pub_timer_

private

Definition at line 73 of file Node.h.

tf::StampedTransform amcl3d::Node::grid_to_world_tf_

private

Definition at line 75 of file Node.h.

ros::Timer amcl3d::Node::grid_to_world_tf_timer_

private

Definition at line 76 of file Node.h.

ros::Subscriber amcl3d::Node::initialpose_sub_

private

Definition at line 85 of file Node.h.

tf::Transform amcl3d::Node::initodom_2_world_tf_

private

Definition at line 88 of file Node.h.

bool amcl3d::Node::is_odom_ { false }

private

Definition at line 78 of file Node.h.

tf::Transform amcl3d::Node::lastbase_2_world_tf_

private

Definition at line 88 of file Node.h.

Particle amcl3d::Node::lastmean_p_

private

Definition at line 83 of file Node.h.

tf::Transform amcl3d::Node::lastodom_2_world_tf_

private

Definition at line 88 of file Node.h.

tf::Transform amcl3d::Node::lastupdatebase_2_odom_tf_

private

Definition at line 88 of file Node.h.

sensor_msgs::PointCloud2 amcl3d::Node::map_point_cloud_msg_

private

Definition at line 67 of file Node.h.

ros::Publisher amcl3d::Node::map_point_cloud_pub_

private

Definition at line 68 of file Node.h.

ros::Timer amcl3d::Node::map_point_cloud_pub_timer_

private

Definition at line 69 of file Node.h.

Particle amcl3d::Node::mean_p_

private

Definition at line 83 of file Node.h.

ros::Time amcl3d::Node::nextupdate_time_

private

Definition at line 91 of file Node.h.

ros::NodeHandle amcl3d::Node::nh_

private

Definition at line 65 of file Node.h.

ros::Publisher amcl3d::Node::odom_base_pub_

private

Definition at line 86 of file Node.h.

tf::Transform amcl3d::Node::odom_increment_tf

private

Definition at line 88 of file Node.h.

ros::Subscriber amcl3d::Node::odom_sub_

private

Definition at line 85 of file Node.h.

Parameters amcl3d::Node::parameters_

private

Definition at line 61 of file Node.h.

ros::Publisher amcl3d::Node::particles_pose_pub_

private

Definition at line 86 of file Node.h.

ParticleFilter amcl3d::Node::pf_

private

Definition at line 63 of file Node.h.

double amcl3d::Node::pitch_ { 0 }

private

Definition at line 80 of file Node.h.

ros::Subscriber amcl3d::Node::point_sub_

private

Definition at line 85 of file Node.h.

std::vector<Range> amcl3d::Node::range_data

private

Definition at line 82 of file Node.h.

ros::Publisher amcl3d::Node::range_markers_pub_

private

Definition at line 86 of file Node.h.

ros::Subscriber amcl3d::Node::range_sub_

private

Definition at line 85 of file Node.h.

double amcl3d::Node::roll_ { 0 }

private

Definition at line 80 of file Node.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation