git commit -m "first commit for v2"

Localizations/Libraries/Ros/amcl/include/amcl/amcl.h

@@ -0,0 +1,293 @@
+#ifndef _AMCL_LIBRARY_H_INCLUDED_
+#define _AMCL_LIBRARY_H_INCLUDED_
+
+#include <algorithm>
+#include <vector>
+#include <map>
+#include <cmath>
+#include <memory>
+
+#include <boost/bind.hpp>
+#include <boost/thread/mutex.hpp>
+
+
+
+#include "amcl/map/map.h"
+#include "amcl/pf/pf.h"
+#include "amcl/sensors/amcl_odom.h"
+#include "amcl/sensors/amcl_laser.h"
+#include "amcl/portable_utils.hpp"
+
+#include "ros/assert.h"
+
+// roscpp
+#include "ros/ros.h"
+
+// Messages that I need
+#include "sensor_msgs/LaserScan.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
+#include "geometry_msgs/PoseArray.h"
+#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
+#include "nav_msgs/GetMap.h"
+#include "nav_msgs/SetMap.h"
+#include "std_srvs/Empty.h"
+
+// For transform support
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/convert.h"
+#include "tf2/utils.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/message_filter.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "message_filters/subscriber.h"
+
+// Dynamic_reconfigure
+#include "dynamic_reconfigure/server.h"
+#include "amcl/AMCLConfig.h"
+
+// Allows AMCL to run from bag file
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <boost/foreach.hpp>
+
+// For monitoring the estimator
+#include <diagnostic_updater/diagnostic_updater.h>
+
+// using namespace amcl;
+
+#define NEW_UNIFORM_SAMPLING 1
+
+// Pose hypothesis
+typedef struct
+{
+  // Total weight (weights sum to 1)
+  double weight;
+
+  // Mean of pose esimate
+  pf_vector_t pf_pose_mean;
+
+  // Covariance of pose estimate
+  pf_matrix_t pf_pose_cov;
+
+} amcl_hyp_t;
+
+static double
+normalize(double z)
+{
+  return atan2(sin(z), cos(z));
+}
+static double
+angle_diff(double a, double b)
+{
+  double d1, d2;
+  a = normalize(a);
+  b = normalize(b);
+  d1 = a - b;
+  d2 = 2 * M_PI - fabs(d1);
+  if (d1 > 0)
+    d2 *= -1.0;
+  if (fabs(d1) < fabs(d2))
+    return(d1);
+  else
+    return(d2);
+}
+
+/* This function is only useful to have the whole code work
+ * with old rosbags that have trailing slashes for their frames
+ */
+inline
+std::string stripSlash(const std::string& in)
+{
+  std::string out = in;
+  if ((!in.empty()) && (in[0] == '/'))
+    out.erase(0, 1);
+  return out;
+}
+namespace amcl
+{
+  class Amcl
+  {
+  public:
+    Amcl();
+    Amcl(ros::NodeHandle nh);
+    virtual ~Amcl();
+
+    /**
+     * @brief Uses TF and LaserScan messages from bag file to drive AMCL instead
+     * @param in_bag_fn input bagfile
+     * @param trigger_global_localization whether to trigger global localization
+     * before starting to process the bagfile
+     */
+    void runFromBag(const std::string& in_bag_fn, bool trigger_global_localization = false);
+
+    int process();
+    void savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose);
+    bool getInitialized() {return initialized_;}
+
+  private:
+    bool initialized_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
+    std::shared_ptr<tf2_ros::TransformListener> tfl_;
+    std::shared_ptr<tf2_ros::Buffer> tf_;
+
+    bool sent_first_transform_;
+
+    tf2::Transform latest_tf_;
+    bool latest_tf_valid_;
+
+    // Pose-generating function used to uniformly distribute particles over
+    // the map
+    static pf_vector_t uniformPoseGenerator(void* arg);
+#if NEW_UNIFORM_SAMPLING
+    static std::vector<std::pair<int, int> > free_space_indices;
+#endif
+    // Callbacks
+    bool globalLocalizationCallback(std_srvs::Empty::Request& req,
+      std_srvs::Empty::Response& res);
+    bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
+      std_srvs::Empty::Response& res);
+    bool setMapCallback(nav_msgs::SetMap::Request& req,
+      nav_msgs::SetMap::Response& res);
+
+    void initialize(ros::NodeHandle nh);
+    void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
+    void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
+    void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
+    void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);
+
+    void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
+    void freeMapDependentMemory();
+    map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg);
+    void updatePoseFromServer();
+    void applyInitialPose();
+
+    //parameter for which odom to use
+    std::string odom_frame_id_;
+
+    //paramater to store latest odom pose
+    geometry_msgs::PoseStamped latest_odom_pose_;
+
+    //parameter for which base to use
+    std::string base_frame_id_;
+    std::string global_frame_id_;
+
+    bool use_map_topic_;
+    bool first_map_only_;
+
+    ros::Duration gui_publish_period;
+    ros::Time save_pose_last_time;
+    ros::Duration save_pose_period;
+
+    geometry_msgs::PoseWithCovarianceStamped last_published_pose;
+
+    map_t* map_;
+    char* mapdata;
+    int sx, sy;
+    double resolution;
+
+    message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
+    tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
+    ros::Subscriber initial_pose_sub_;
+    std::vector< amcl::AMCLLaser* > lasers_;
+    std::vector< bool > lasers_update_;
+    std::map< std::string, int > frame_to_laser_;
+
+    // Particle filter
+    pf_t* pf_;
+    double pf_err_, pf_z_;
+    bool pf_init_;
+    pf_vector_t pf_odom_pose_;
+    double d_thresh_, a_thresh_;
+    int resample_interval_;
+    int resample_count_;
+    double laser_min_range_;
+    double laser_max_range_;
+
+    //Nomotion update control
+    bool m_force_update;  // used to temporarily let amcl update samples even when no motion occurs...
+
+    amcl::AMCLOdom* odom_;
+    amcl::AMCLLaser* laser_;
+
+    ros::Duration cloud_pub_interval;
+    ros::Time last_cloud_pub_time;
+
+    // For slowing play-back when reading directly from a bag file
+    ros::WallDuration bag_scan_period_;
+
+    void requestMap();
+
+    // Helper to get odometric pose from transform system
+    bool getOdomPose(geometry_msgs::PoseStamped& pose,
+      double& x, double& y, double& yaw,
+      const ros::Time& t, const std::string& f);
+
+    //time for tolerance on the published transform,
+    //basically defines how long a map->odom transform is good for
+    ros::Duration transform_tolerance_;
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+    ros::Publisher pose_pub_;
+    ros::Publisher particlecloud_pub_;
+    ros::ServiceServer global_loc_srv_;
+    ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
+    ros::ServiceServer set_map_srv_;
+    ros::Subscriber initial_pose_sub_old_;
+    ros::Subscriber map_sub_;
+
+    diagnostic_updater::Updater diagnosic_updater_;
+    void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);
+    double std_warn_level_x_;
+    double std_warn_level_y_;
+    double std_warn_level_yaw_;
+
+    std::string scan_topic_;
+    std::string map_topic_;
+    std::string map_service_;
+
+    amcl_hyp_t* initial_pose_hyp_;
+    bool first_map_received_;
+    bool first_reconfigure_call_;
+
+    boost::recursive_mutex configuration_mutex_;
+    dynamic_reconfigure::Server<amcl::AMCLConfig>* dsrv_;
+    amcl::AMCLConfig default_config_;
+    ros::Timer check_laser_timer_;
+
+    int max_beams_, min_particles_, max_particles_;
+    double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+    double alpha_slow_, alpha_fast_;
+    double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
+    //beam skip related params
+    bool do_beamskip_;
+    double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
+    double laser_likelihood_max_dist_;
+    amcl::odom_model_t odom_model_type_;
+    double init_pose_[3];
+    double init_cov_[3];
+    amcl::laser_model_t laser_model_type_;
+    bool tf_broadcast_;
+    bool force_update_after_initialpose_;
+    bool force_update_after_set_map_;
+    bool selective_resampling_;
+    ros::Time last_tfb_time_;
+
+    void reconfigureCB(amcl::AMCLConfig& config, uint32_t level);
+
+    ros::Time last_laser_received_ts_;
+    ros::Duration laser_check_interval_;
+    void checkLaserReceived(const ros::TimerEvent& event);
+  };
+} // namespace amcl
+#define USAGE "USAGE: amcl"
+
+#if NEW_UNIFORM_SAMPLING
+std::vector<std::pair<int, int> > amcl::Amcl::free_space_indices;
+#endif
+
+#endif // _AMCL_LIBRARY_H_INCLUDED_

Localizations/Libraries/Ros/amcl/include/amcl/map/map.h

@@ -0,0 +1,150 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Global map (grid-based)
+ * Author: Andrew Howard
+ * Date: 6 Feb 2003
+ * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
+ **************************************************************************/
+
+#ifndef MAP_H
+#define MAP_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _rtk_fig_t;
+
+  
+// Limits
+#define MAP_WIFI_MAX_LEVELS 8
+
+  
+// Description for a single map cell.
+typedef struct
+{
+  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
+  int occ_state;
+
+  // Distance to the nearest occupied cell
+  double occ_dist;
+
+  // Wifi levels
+  //int wifi_levels[MAP_WIFI_MAX_LEVELS];
+
+} map_cell_t;
+
+
+// Description for a map
+typedef struct
+{
+  // Map origin; the map is a viewport onto a conceptual larger map.
+  double origin_x, origin_y;
+  
+  // Map scale (m/cell)
+  double scale;
+
+  // Map dimensions (number of cells)
+  int size_x, size_y;
+  
+  // The map data, stored as a grid
+  map_cell_t *cells;
+
+  // Max distance at which we care about obstacles, for constructing
+  // likelihood field
+  double max_occ_dist;
+  
+} map_t;
+
+
+
+/**************************************************************************
+ * Basic map functions
+ **************************************************************************/
+
+// Create a new (empty) map
+map_t *map_alloc(void);
+
+// Destroy a map
+void map_free(map_t *map);
+
+// Get the cell at the given point
+map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);
+
+// Load an occupancy map
+int map_load_occ(map_t *map, const char *filename, double scale, int negate);
+
+// Load a wifi signal strength map
+//int map_load_wifi(map_t *map, const char *filename, int index);
+
+// Update the cspace distances
+void map_update_cspace(map_t *map, double max_occ_dist);
+
+
+/**************************************************************************
+ * Range functions
+ **************************************************************************/
+
+// Extract a single range reading from the map
+double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range);
+
+
+/**************************************************************************
+ * GUI/diagnostic functions
+ **************************************************************************/
+
+// Draw the occupancy grid
+void map_draw_occ(map_t *map, struct _rtk_fig_t *fig);
+
+// Draw the cspace map
+void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig);
+
+// Draw a wifi map
+void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index);
+
+
+/**************************************************************************
+ * Map manipulation macros
+ **************************************************************************/
+
+// Convert from map index to world coords
+#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
+#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
+
+// Convert from world coords to map coords
+#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
+#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
+
+// Test to see if the given map coords lie within the absolute map bounds.
+#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
+
+// Compute the cell index for the given map coords.
+#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h

@@ -0,0 +1,11 @@
+
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+   Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef _eig_h
+
+/* Symmetric matrix A => eigenvectors in columns of V, corresponding
+   eigenvalues in d. */
+void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h

@@ -0,0 +1,210 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Simple particle filter for localization.
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_H
+#define PF_H
+
+#include "pf_vector.h"
+#include "pf_kdtree.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _pf_t;
+struct _rtk_fig_t;
+struct _pf_sample_set_t;
+
+// Function prototype for the initialization model; generates a sample pose from
+// an appropriate distribution.
+typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
+
+// Function prototype for the action model; generates a sample pose from
+// an appropriate distribution
+typedef void (*pf_action_model_fn_t) (void *action_data, 
+                                      struct _pf_sample_set_t* set);
+
+// Function prototype for the sensor model; determines the probability
+// for the given set of sample poses.
+typedef double (*pf_sensor_model_fn_t) (void *sensor_data, 
+                                        struct _pf_sample_set_t* set);
+
+
+// Information for a single sample
+typedef struct
+{
+  // Pose represented by this sample
+  pf_vector_t pose;
+
+  // Weight for this pose
+  double weight;
+  
+} pf_sample_t;
+
+
+// Information for a cluster of samples
+typedef struct
+{
+  // Number of samples
+  int count;
+
+  // Total weight of samples in this cluster
+  double weight;
+
+  // Cluster statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+
+  // Workspace
+  double m[4], c[2][2];
+  
+} pf_cluster_t;
+
+
+// Information for a set of samples
+typedef struct _pf_sample_set_t
+{
+  // The samples
+  int sample_count;
+  pf_sample_t *samples;
+
+  // A kdtree encoding the histogram
+  pf_kdtree_t *kdtree;
+
+  // Clusters
+  int cluster_count, cluster_max_count;
+  pf_cluster_t *clusters;
+
+  // Filter statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+  int converged; 
+  double n_effective;
+} pf_sample_set_t;
+
+
+// Information for an entire filter
+typedef struct _pf_t
+{
+  // This min and max number of samples
+  int min_samples, max_samples;
+
+  // Population size parameters
+  double pop_err, pop_z;
+
+  // Resample limit cache
+  int *limit_cache;
+  
+  // The sample sets.  We keep two sets and use [current_set]
+  // to identify the active set.
+  int current_set;
+  pf_sample_set_t sets[2];
+
+  // Running averages, slow and fast, of likelihood
+  double w_slow, w_fast;
+
+  // Decay rates for running averages
+  double alpha_slow, alpha_fast;
+
+  // Function used to draw random pose samples
+  pf_init_model_fn_t random_pose_fn;
+  void *random_pose_data;
+
+  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
+  int converged; 
+
+  // boolean parameter to enamble/diable selective resampling
+  int selective_resampling;
+} pf_t;
+
+
+// Create a new filter
+pf_t *pf_alloc(int min_samples, int max_samples,
+               double alpha_slow, double alpha_fast,
+               pf_init_model_fn_t random_pose_fn, void *random_pose_data);
+
+// Free an existing filter
+void pf_free(pf_t *pf);
+
+// Initialize the filter using a guassian
+void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
+
+// Initialize the filter using some model
+void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
+
+// Update the filter with some new action
+void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
+
+// Update the filter with some new sensor observation
+void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
+
+// Resample the distribution
+void pf_update_resample(pf_t *pf);
+
+// set selective resampling parameter
+void pf_set_selective_resampling(pf_t *pf, int selective_resampling);
+
+// Compute the CEP statistics (mean and variance).
+void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
+
+// Compute the statistics for a particular cluster.  Returns 0 if
+// there is no such cluster.
+int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
+                         pf_vector_t *mean, pf_matrix_t *cov);
+
+// Re-compute the cluster statistics for a sample set
+void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
+
+
+// Display the sample set
+void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
+
+// Draw the histogram (kdtree)
+void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
+
+// Draw the CEP statistics
+void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
+
+// Draw the cluster statistics
+void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
+
+//calculate if the particle filter has converged - 
+//and sets the converged flag in the current set and the pf 
+int pf_update_converged(pf_t *pf);
+
+//sets the current set and pf converged values to zero
+void pf_init_converged(pf_t *pf);
+
+void pf_copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h

@@ -0,0 +1,109 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: KD tree functions
+ * Author: Andrew Howard
+ * Date: 18 Dec 2002
+ * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
+ *************************************************************************/
+
+#ifndef PF_KDTREE_H
+#define PF_KDTREE_H
+
+#ifdef INCLUDE_RTKGUI
+#include "rtk.h"
+#endif
+
+
+// Info for a node in the tree
+typedef struct pf_kdtree_node
+{
+  // Depth in the tree
+  int leaf, depth;
+
+  // Pivot dimension and value
+  int pivot_dim;
+  double pivot_value;
+
+  // The key for this node
+  int key[3];
+
+  // The value for this node
+  double value;
+
+  // The cluster label (leaf nodes)
+  int cluster;
+
+  // Child nodes
+  struct pf_kdtree_node *children[2];
+
+} pf_kdtree_node_t;
+
+
+// A kd tree
+typedef struct
+{
+  // Cell size
+  double size[3];
+
+  // The root node of the tree
+  pf_kdtree_node_t *root;
+
+  // The number of nodes in the tree
+  int node_count, node_max_count;
+  pf_kdtree_node_t *nodes;
+
+  // The number of leaf nodes in the tree
+  int leaf_count;
+
+} pf_kdtree_t;
+
+
+// Create a tree
+extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
+
+// Destroy a tree
+extern void pf_kdtree_free(pf_kdtree_t *self);
+
+// Clear all entries from the tree
+extern void pf_kdtree_clear(pf_kdtree_t *self);
+
+// Insert a pose into the tree
+extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
+
+// Cluster the leaves in the tree
+extern void pf_kdtree_cluster(pf_kdtree_t *self);
+
+// Determine the probability estimate for the given pose
+extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
+
+// Determine the cluster label for the given pose
+extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
+
+
+#ifdef INCLUDE_RTKGUI
+
+// Draw the tree
+extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
+
+#endif
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h

@@ -0,0 +1,85 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Useful pdf functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_PDF_H
+#define PF_PDF_H
+
+#include "pf_vector.h"
+
+//#include <gsl/gsl_rng.h>
+//#include <gsl/gsl_randist.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**************************************************************************
+ * Gaussian
+ *************************************************************************/
+
+// Gaussian PDF info
+typedef struct
+{
+  // Mean, covariance and inverse covariance
+  pf_vector_t x;
+  pf_matrix_t cx;
+  //pf_matrix_t cxi;
+  double cxdet;
+
+  // Decomposed covariance matrix (rotation * diagonal)
+  pf_matrix_t cr;
+  pf_vector_t cd;
+
+  // A random number generator
+  //gsl_rng *rng;
+
+} pf_pdf_gaussian_t;
+
+
+// Create a gaussian pdf
+pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
+
+// Destroy the pdf
+void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);
+
+// Compute the value of the pdf at some point [z].
+//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
+
+// Draw randomly from a zero-mean Gaussian distribution, with standard
+// deviation sigma.
+// We use the polar form of the Box-Muller transformation, explained here:
+//   http://www.taygeta.com/random/gaussian.html
+double pf_ran_gaussian(double sigma);
+
+// Generate a sample from the pdf.
+pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h

@@ -0,0 +1,94 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+/**************************************************************************
+ * Desc: Vector functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef PF_VECTOR_H
+#define PF_VECTOR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+  
+// The basic vector
+typedef struct
+{
+  double v[3];
+} pf_vector_t;
+
+
+// The basic matrix
+typedef struct
+{
+  double m[3][3];
+} pf_matrix_t;
+
+
+// Return a zero vector
+pf_vector_t pf_vector_zero();
+
+// Check for NAN or INF in any component
+int pf_vector_finite(pf_vector_t a);
+
+// Print a vector
+void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
+
+// Simple vector addition
+pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
+
+// Simple vector subtraction
+pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
+
+// Transform from local to global coords (a + b)
+pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
+
+// Transform from global to local coords (a - b)
+pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
+
+
+// Return a zero matrix
+pf_matrix_t pf_matrix_zero();
+
+// Check for NAN or INF in any component
+int pf_matrix_finite(pf_matrix_t a);
+
+// Print a matrix
+void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);
+
+// Compute the matrix inverse.  Will also return the determinant,
+// which should be checked for underflow (indicated singular matrix).
+//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
+
+// Decompose a covariance matrix [a] into a rotation matrix [r] and a
+// diagonal matrix [d] such that a = r * d * r^T.
+void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/portable_utils.hpp

@@ -0,0 +1,35 @@
+#ifndef PORTABLE_UTILS_H
+#define PORTABLE_UTILS_H
+
+#include <stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef HAVE_DRAND48
+// Some system (e.g., Windows) doesn't come with drand48(), srand48().
+// Use rand, and srand for such system.
+
+// Remove extern declarations to avoid conflicts
+// extern double drand48(void); // Change this to static if it should be static
+
+extern double drand48(void)
+{
+    return ((double)rand())/RAND_MAX;
+}
+
+// Remove extern declarations to avoid conflicts
+// extern double srand48(long int seedval); // Change this to static if it should be static
+
+extern void srand48(long int seedval)
+{
+    srand(seedval);
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.h

@@ -0,0 +1,156 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: LASER sensor model for AMCL
+// Author: Andrew Howard
+// Date: 17 Aug 2003
+// CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_LASER_H
+#define AMCL_LASER_H
+
+#include "amcl_sensor.h"
+#include "../map/map.h"
+
+namespace amcl
+{
+
+typedef enum
+{
+  LASER_MODEL_BEAM,
+  LASER_MODEL_LIKELIHOOD_FIELD,
+  LASER_MODEL_LIKELIHOOD_FIELD_PROB
+} laser_model_t;
+
+// Laser sensor data
+class AMCLLaserData : public AMCLSensorData
+{
+  public:
+    AMCLLaserData () {ranges=NULL;};
+    virtual ~AMCLLaserData() {delete [] ranges;};
+  // Laser range data (range, bearing tuples)
+  public: int range_count;
+  public: double range_max;
+  public: double (*ranges)[2];
+};
+
+
+// Laseretric sensor model
+class AMCLLaser : public AMCLSensor
+{
+  // Default constructor
+  public: AMCLLaser(size_t max_beams, map_t* map);
+
+  public: virtual ~AMCLLaser(); 
+
+  public: void SetModelBeam(double z_hit,
+                            double z_short,
+                            double z_max,
+                            double z_rand,
+                            double sigma_hit,
+                            double lambda_short,
+                            double chi_outlier);
+
+  public: void SetModelLikelihoodField(double z_hit,
+                                       double z_rand,
+                                       double sigma_hit,
+                                       double max_occ_dist);
+
+  //a more probabilistically correct model - also with the option to do beam skipping
+  public: void SetModelLikelihoodFieldProb(double z_hit,
+					   double z_rand,
+					   double sigma_hit,
+					   double max_occ_dist, 
+					   bool do_beamskip, 
+					   double beam_skip_distance, 
+					   double beam_skip_threshold, 
+					   double beam_skip_error_threshold);
+
+  // Update the filter based on the sensor model.  Returns true if the
+  // filter has been updated.
+  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Set the laser's pose after construction
+  public: void SetLaserPose(pf_vector_t& laser_pose) 
+          {this->laser_pose = laser_pose;}
+
+  // Determine the probability for the given pose
+  private: static double BeamModel(AMCLLaserData *data, 
+                                   pf_sample_set_t* set);
+  // Determine the probability for the given pose
+  private: static double LikelihoodFieldModel(AMCLLaserData *data, 
+                                              pf_sample_set_t* set);
+
+  // Determine the probability for the given pose - more probablistic model 
+  private: static double LikelihoodFieldModelProb(AMCLLaserData *data, 
+					     pf_sample_set_t* set);
+
+  private: void reallocTempData(int max_samples, int max_obs);
+
+  private: laser_model_t model_type;
+
+  // Current data timestamp
+  private: double time;
+
+  // The laser map
+  private: map_t *map;
+
+  // Laser offset relative to robot
+  private: pf_vector_t laser_pose;
+  
+  // Max beams to consider
+  private: int max_beams;
+
+  // Beam skipping parameters (used by LikelihoodFieldModelProb model)
+  private: bool do_beamskip; 
+  private: double beam_skip_distance; 
+  private: double beam_skip_threshold; 
+  //threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods 
+  //this would be an error condition 
+  private: double beam_skip_error_threshold;
+
+  //temp data that is kept before observations are integrated to each particle (requried for beam skipping)
+  private: int max_samples;
+  private: int max_obs;
+  private: double **temp_obs;
+
+  // Laser model params
+  //
+  // Mixture params for the components of the model; must sum to 1
+  private: double z_hit;
+  private: double z_short;
+  private: double z_max;
+  private: double z_rand;
+  //
+  // Stddev of Gaussian model for laser hits.
+  private: double sigma_hit;
+  // Decay rate of exponential model for short readings.
+  private: double lambda_short;
+  // Threshold for outlier rejection (unused)
+  private: double chi_outlier;
+};
+
+
+}
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.puml

@@ -0,0 +1,54 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLLaserData {
+  + AMCLLaserData()
+  + ~AMCLLaserData()
+  # range_count: int
+  # range_max: double
+  # ranges: double[][]
+
+}
+
+class AMCLLaser {
+  - model_type: laser_model_t
+  - time: double
+  - map: map_t
+  - laser_pose: pf_vector_t
+  - max_beams: int
+  - do_beamskip: bool
+  - beam_skip_distance: double
+  - beam_skip_threshold: double
+  - beam_skip_error_threshold: double
+  - max_samples: int
+  - max_obs: int
+  - temp_obs: double[][]
+  - z_hit: double
+  - z_short: double
+  - z_max: double
+  - z_rand: double
+  - sigma_hit: double
+  - lambda_short: double
+  - chi_outlier: double
+
+  + AMCLLaser(max_beams: size_t, map: map_t)
+  + ~AMCLLaser()
+  + SetModelBeam(z_hit: double, z_short: double, z_max: double, z_rand: double, sigma_hit: double, lambda_short: double, chi_outlier: double)
+  + SetModelLikelihoodField(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double)
+  + SetModelLikelihoodFieldProb(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double, do_beamskip: bool, beam_skip_distance: double, beam_skip_threshold: double, beam_skip_error_threshold: double)
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+  + SetLaserPose(laser_pose: pf_vector_t)
+  + BeamModel(data: AMCLLaserData, set: pf_sample_set_t): double
+  + LikelihoodFieldModel(data: AMCLLaserData, set: pf_sample_set_t): double
+  + LikelihoodFieldModelProb(data: AMCLLaserData, set: pf_sample_set_t): double
+  + reallocTempData(max_samples: int, max_obs: int)
+}
+
+Player --|> AMCLSensor
+
+AMCLLaserData --|> AMCLSensorData
+
+@enduml

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.h

@@ -0,0 +1,98 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Odometry sensor model for AMCL
+// Author: Andrew Howard
+// Date: 17 Aug 2003
+// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_ODOM_H
+#define AMCL_ODOM_H
+
+#include "amcl_sensor.h"
+#include "../pf/pf_pdf.h"
+
+namespace amcl
+{
+
+typedef enum
+{
+  ODOM_MODEL_DIFF,
+  ODOM_MODEL_OMNI,
+  ODOM_MODEL_DIFF_CORRECTED,
+  ODOM_MODEL_OMNI_CORRECTED
+} odom_model_t;
+
+// Odometric sensor data
+class AMCLOdomData : public AMCLSensorData
+{
+  // Odometric pose
+  public: pf_vector_t pose;
+
+  // Change in odometric pose
+  public: pf_vector_t delta;
+};
+
+
+// Odometric sensor model
+class AMCLOdom : public AMCLSensor
+{
+  // Default constructor
+  public: AMCLOdom();
+
+  public: void SetModelDiff(double alpha1, 
+                            double alpha2, 
+                            double alpha3, 
+                            double alpha4);
+
+  public: void SetModelOmni(double alpha1, 
+                            double alpha2, 
+                            double alpha3, 
+                            double alpha4,
+                            double alpha5);
+
+  public: void SetModel( odom_model_t type,
+                         double alpha1,
+                         double alpha2,
+                         double alpha3,
+                         double alpha4,
+                         double alpha5 = 0 );
+
+  // Update the filter based on the action model.  Returns true if the filter
+  // has been updated.
+  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
+
+  // Current data timestamp
+  private: double time;
+  
+  // Model type
+  private: odom_model_t model_type;
+
+  // Drift parameters
+  private: double alpha1, alpha2, alpha3, alpha4, alpha5;
+};
+
+
+}
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.puml

@@ -0,0 +1,54 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLSensor {
+  - is_action: bool
+  - pose: pf_vector_t
+
+  + AMCLSensor()
+  + ~AMCLSensor()
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+  + InitSensor(pf: pf_t, data: AMCLSensorData): bool
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+}
+
+class AMCLSensorData {
+  - sensor: AMCLSensor
+
+  + AMCLSensorData()
+  + ~AMCLSensorData()
+}
+
+class AMCLOdomData {
+  - pose: pf_vector_t
+  - delta: pf_vector_t
+
+  + AMCLOdomData()
+}
+
+class AMCLOdom {
+  - time: double
+  - model_type: odom_model_t
+  - alpha1: double
+  - alpha2: double
+  - alpha3: double
+  - alpha4: double
+  - alpha5: double
+
+  + AMCLOdom()
+  + SetModelDiff(alpha1: double, alpha2: double, alpha3: double, alpha4: double)
+  + SetModelOmni(alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double)
+  + SetModel(type: odom_model_t, alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double = 0)
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+}
+
+Player --|> AMCLSensor
+
+AMCLSensorData --|> AMCLSensor
+AMCLOdomData --|> AMCLSensorData
+AMCLOdom --|> AMCLSensor
+
+@enduml

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.h

@@ -0,0 +1,97 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey et al.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+///////////////////////////////////////////////////////////////////////////
+//
+// Desc: Adaptive Monte-Carlo localization
+// Author: Andrew Howard
+// Date: 6 Feb 2003
+// CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $
+//
+///////////////////////////////////////////////////////////////////////////
+
+#ifndef AMCL_SENSOR_H
+#define AMCL_SENSOR_H
+
+#include "../pf/pf.h"
+
+namespace amcl
+{
+
+// Forward declarations
+class AMCLSensorData;
+
+
+// Base class for all AMCL sensors
+class AMCLSensor
+{
+  // Default constructor
+  public: AMCLSensor();
+         
+  // Default destructor
+  public: virtual ~AMCLSensor();
+
+  // Update the filter based on the action model.  Returns true if the filter
+  // has been updated.
+  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
+
+  // Initialize the filter based on the sensor model.  Returns true if the
+  // filter has been initialized.
+  public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Update the filter based on the sensor model.  Returns true if the
+  // filter has been updated.
+  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
+
+  // Flag is true if this is the action sensor
+  public: bool is_action;
+
+  // Action pose (action sensors only)
+  public: pf_vector_t pose;
+
+  // AMCL Base
+  //protected: AdaptiveMCL & AMCL;
+
+#ifdef INCLUDE_RTKGUI
+  // Setup the GUI
+  public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
+
+  // Finalize the GUI
+  public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
+
+  // Draw sensor data
+  public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
+#endif
+};
+
+
+
+// Base class for all AMCL sensor measurements
+class AMCLSensorData
+{
+  // Pointer to sensor that generated the data
+  public: AMCLSensor *sensor;
+          virtual ~AMCLSensorData() {}
+
+  // Data timestamp
+  public: double time;
+};
+
+}
+
+#endif

Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.puml

@@ -0,0 +1,29 @@
+@startuml
+
+class Player {
+  + Player()
+}
+
+class AMCLSensor {
+  - is_action: bool
+  - pose: pf_vector_t
+
+  + AMCLSensor()
+  + ~AMCLSensor()
+  + UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+  + InitSensor(pf: pf_t, data: AMCLSensorData): bool
+  + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+}
+
+class AMCLSensorData {
+  - sensor: AMCLSensor
+
+  + AMCLSensorData()
+  + ~AMCLSensorData()
+}
+
+Player --|> AMCLSensor
+
+AMCLSensorData --|> AMCLSensor
+
+@enduml