#include <ndt_visualisation/ndt_viz.h>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose.h"
#include "ndt_mcl/ndt_mcl.h"
#include <ndt_map/ndt_map.h>

Include dependency graph for 2d_ndt_mcl_node.cpp:

Defines
#define	USE_VISUALIZATION_DEBUG
	Enable / Disable visualization.
Functions
void	callback (const sensor_msgs::LaserScan::ConstPtr &scan)
Eigen::Affine3d	getAsAffine (float x, float y, float yaw)
double	getDoubleTime ()
void	initialPoseReceived (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
int	main (int argc, char **argv)
void	sendMapToRviz (lslgeneric::NDTMap &map)
bool	sendROSOdoMessage (Eigen::Vector3d mean, Eigen::Matrix3d cov, ros::Time ts)
Variables
static bool	has_sensor_offset_set = false
bool	hasNewInitialPose = false
ros::Subscriber	initial_pose_sub_
double	ipos_x = 0
double	ipos_y = 0
double	ipos_yaw = 0
static bool	isFirstLoad = true
double	ivar_x = 0
double	ivar_y = 0
double	ivar_yaw = 0
ros::Publisher	mcl_pub
	The output of MCL is published with this! ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////.
NDTViz	ndt_viz
ros::Publisher	ndtmap_pub
NDTMCL *	ndtmcl
float	offa = 0
float	offx = 0
	Laser sensor offset.
float	offy = 0
std::string	tf_laser_link = "base_laser_link"
std::string	tf_odo_topic = "odom_base_link"
std::string	tf_state_topic = "base_link"
double	time_end
Eigen::Affine3d	Todo
Eigen::Affine3d	Told
bool	userInitialPose = false

Define Documentation

#define USE_VISUALIZATION_DEBUG

Enable / Disable visualization.

2D NDT-MCL Node. This application runs the ndt-mcl localization based on a map and laser scanner and odometry.

The initialization is based now on ground truth pose information (should be replaced with manual input).

The visualization depends on mrpt-gui

More details about the algorithm: Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL) IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan

Author:: Jari Saarinen (jari.p.saarinen@gmail.com)

Initialization from GUI Global initialization possibility Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work

Definition at line 20 of file 2d_ndt_mcl_node.cpp.

Function Documentation

void callback ( const sensor_msgs::LaserScan::ConstPtr & scan )

Callback for laser scan messages

Get state information

Ground truth --- Not generally available so should be changed to the manual initialization

Odometry

Number of scans

Pose conversions

Initialize the particle filter

Calculate the differential motion from the last frame

< just integrates odometry for the visualization

Calculate the laser pose with respect to the base

Laser pose in base frame

Laser scan to PointCloud expressed in the base frame

Now we have the sensor origin and pointcloud -- Lets do MCL

<predicts, updates and resamples if necessary (ndt_mcl.hpp)

Maximum aposteriori pose

Pose covariance ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Spit out the pose estimate

This is all for visualization

Definition at line 319 of file 2d_ndt_mcl_node.cpp.

Eigen::Affine3d getAsAffine	(	float	x,
		float	y,
		float	yaw
	)

Convert x,y,yaw to Eigen::Affine3d

Definition at line 183 of file 2d_ndt_mcl_node.cpp.

double getDoubleTime ( )

Definition at line 310 of file 2d_ndt_mcl_node.cpp.

void initialPoseReceived ( const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg )

Definition at line 160 of file 2d_ndt_mcl_node.cpp.

int main	(	int	argc,
		char **	argv
	)

Parameters for the mapper

< flag to indicate that we want to load a map

<name and the path to the map

< indicates if we want to save the map in a regular intervals

Set the values from a config file

Prepare the map

Set up our output

Set the subscribers and setup callbacks and message filters

Definition at line 467 of file 2d_ndt_mcl_node.cpp.

void sendMapToRviz ( lslgeneric::NDTMap & map )

RVIZ NDT-MAP Visualization stuff

Definition at line 58 of file 2d_ndt_mcl_node.cpp.

bool sendROSOdoMessage	(	Eigen::Vector3d	mean,
		Eigen::Matrix3d	cov,
		ros::Time	ts
	)

O.pose.covariance = { cov(0,0), cov(0,1), 0 , 0 , 0 , 0, cov(1,0), cov(1,1), 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0 , cov(2,2)};

Definition at line 218 of file 2d_ndt_mcl_node.cpp.

Variable Documentation

bool has_sensor_offset_set = false [static]

Definition at line 208 of file 2d_ndt_mcl_node.cpp.

bool hasNewInitialPose = false

Definition at line 153 of file 2d_ndt_mcl_node.cpp.

ros::Subscriber initial_pose_sub_

Definition at line 158 of file 2d_ndt_mcl_node.cpp.

double ipos_x = 0

Definition at line 155 of file 2d_ndt_mcl_node.cpp.

double ipos_y = 0

Definition at line 155 of file 2d_ndt_mcl_node.cpp.

double ipos_yaw = 0

Definition at line 155 of file 2d_ndt_mcl_node.cpp.

bool isFirstLoad = true [static]

Definition at line 209 of file 2d_ndt_mcl_node.cpp.

double ivar_x = 0

Definition at line 156 of file 2d_ndt_mcl_node.cpp.

double ivar_y = 0

Definition at line 156 of file 2d_ndt_mcl_node.cpp.

double ivar_yaw = 0

Definition at line 156 of file 2d_ndt_mcl_node.cpp.

ros::Publisher mcl_pub

The output of MCL is published with this! ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////.

Definition at line 212 of file 2d_ndt_mcl_node.cpp.

NDTViz ndt_viz

Visualization stuff

Definition at line 51 of file 2d_ndt_mcl_node.cpp.

ros::Publisher ndtmap_pub

Definition at line 53 of file 2d_ndt_mcl_node.cpp.

NDTMCL* ndtmcl

Update measurement

Globals

Definition at line 203 of file 2d_ndt_mcl_node.cpp.

float offa = 0

Definition at line 207 of file 2d_ndt_mcl_node.cpp.

float offx = 0

Laser sensor offset.

Definition at line 205 of file 2d_ndt_mcl_node.cpp.

float offy = 0

Definition at line 206 of file 2d_ndt_mcl_node.cpp.

std::string tf_laser_link = "base_laser_link"

Definition at line 308 of file 2d_ndt_mcl_node.cpp.

std::string tf_odo_topic = "odom_base_link"

Definition at line 306 of file 2d_ndt_mcl_node.cpp.

std::string tf_state_topic = "base_link"

Definition at line 307 of file 2d_ndt_mcl_node.cpp.

double time_end

Definition at line 305 of file 2d_ndt_mcl_node.cpp.

Eigen::Affine3d Todo

Definition at line 210 of file 2d_ndt_mcl_node.cpp.

Eigen::Affine3d Told

Definition at line 210 of file 2d_ndt_mcl_node.cpp.

bool userInitialPose = false

Initial pose stuff (quick and dirty as always)

Definition at line 152 of file 2d_ndt_mcl_node.cpp.

Defines

Functions

Variables

Define Documentation

Function Documentation

Variable Documentation