#include <mrpt/utils/CTicTac.h>#include <mrpt/gui.h>#include <mrpt/base.h>#include <mrpt/opengl.h>#include <GL/gl.h>#include "ndt_mcl/CMyEllipsoid.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/impl/ndt_mcl.hpp"#include <ndt_map/ndt_map.h>#include "ndt_mcl/impl/mcl_visualization.hpp"
Go to the source code of this file.
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) |
| void | initialPoseReceived (const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) |
| int | main (int argc, char **argv) |
| void | sendMapToRviz (lslgeneric::NDTMap< pcl::PointXYZ > &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! ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////. | |
| ros::Publisher | ndtmap_pub |
| < here is a punch of visualization code based on the MRPT's GUI components | |
| NDTMCL< pcl::PointXYZ > * | 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" |
| Eigen::Affine3d | Todo |
| Eigen::Affine3d | Told |
| mrpt::utils::CTicTac | TT |
| 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
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 321 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 190 of file 2d_ndt_mcl_node.cpp.
| void initialPoseReceived | ( | const geometry_msgs::PoseWithCovarianceStampedConstPtr & | msg | ) |
Definition at line 167 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 480 of file 2d_ndt_mcl_node.cpp.
| void sendMapToRviz | ( | lslgeneric::NDTMap< pcl::PointXYZ > & | map | ) |
RVIZ NDT-MAP Visualization stuff
Definition at line 65 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 225 of file 2d_ndt_mcl_node.cpp.
Variable Documentation
bool has_sensor_offset_set = false [static] |
Definition at line 215 of file 2d_ndt_mcl_node.cpp.
| bool hasNewInitialPose = false |
Definition at line 160 of file 2d_ndt_mcl_node.cpp.
Definition at line 165 of file 2d_ndt_mcl_node.cpp.
| double ipos_x = 0 |
Definition at line 162 of file 2d_ndt_mcl_node.cpp.
| double ipos_y = 0 |
Definition at line 162 of file 2d_ndt_mcl_node.cpp.
| double ipos_yaw = 0 |
Definition at line 162 of file 2d_ndt_mcl_node.cpp.
bool isFirstLoad = true [static] |
Definition at line 216 of file 2d_ndt_mcl_node.cpp.
| double ivar_x = 0 |
Definition at line 163 of file 2d_ndt_mcl_node.cpp.
| double ivar_y = 0 |
Definition at line 163 of file 2d_ndt_mcl_node.cpp.
| double ivar_yaw = 0 |
Definition at line 163 of file 2d_ndt_mcl_node.cpp.
The output of MCL is published with this! ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////.
Definition at line 219 of file 2d_ndt_mcl_node.cpp.
< here is a punch of visualization code based on the MRPT's GUI components
Visualization stuff
Definition at line 60 of file 2d_ndt_mcl_node.cpp.
| float offa = 0 |
Definition at line 214 of file 2d_ndt_mcl_node.cpp.
| float offx = 0 |
Laser sensor offset.
Definition at line 212 of file 2d_ndt_mcl_node.cpp.
| float offy = 0 |
Definition at line 213 of file 2d_ndt_mcl_node.cpp.
| std::string tf_laser_link = "base_laser_link" |
Definition at line 316 of file 2d_ndt_mcl_node.cpp.
| std::string tf_odo_topic = "odom_base_link" |
Definition at line 314 of file 2d_ndt_mcl_node.cpp.
| std::string tf_state_topic = "base_link" |
Definition at line 315 of file 2d_ndt_mcl_node.cpp.
| Eigen::Affine3d Todo |
Definition at line 217 of file 2d_ndt_mcl_node.cpp.
| Eigen::Affine3d Told |
Definition at line 217 of file 2d_ndt_mcl_node.cpp.
| mrpt::utils::CTicTac TT |
Definition at line 312 of file 2d_ndt_mcl_node.cpp.
| bool userInitialPose = false |
Initial pose stuff (quick and dirty as always)
Definition at line 159 of file 2d_ndt_mcl_node.cpp.