add humble-navigation2

navigation2/nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -0,0 +1,398 @@
+/*
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__AMCL_NODE_HPP_
+#define NAV2_AMCL__AMCL_NODE_HPP_
+
+#include <atomic>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "message_filters/subscriber.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/sensors/laser/laser.hpp"
+#include "nav2_msgs/msg/particle.hpp"
+#include "nav2_msgs/msg/particle_cloud.hpp"
+#include "nav2_msgs/srv/set_initial_pose.hpp"
+#include "nav_msgs/srv/set_map.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "pluginlib/class_loader.hpp"
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wreorder"
+#include "tf2_ros/message_filter.h"
+#pragma GCC diagnostic pop
+
+#define NEW_UNIFORM_SAMPLING 1
+
+namespace nav2_amcl
+{
+/*
+ * @class AmclNode
+ * @brief ROS wrapper for AMCL
+ */
+class AmclNode : public nav2_util::LifecycleNode
+{
+public:
+  /*
+   * @brief AMCL constructor
+   * @param options Additional options to control creation of the node.
+   */
+  explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /*
+   * @brief AMCL destructor
+   */
+  ~AmclNode();
+
+protected:
+  /*
+   * @brief Lifecycle configure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle activate
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle deactivate
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle cleanup
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle shutdown
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
+  // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
+  // respond until we're in the active state
+  std::atomic<bool> active_{false};
+
+  // Dedicated callback group and executor for services and subscriptions in AmclNode,
+  // in order to isolate TF timer used in message filter.
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
+  // Pose hypothesis
+  typedef struct
+  {
+    double weight;             // Total weight (weights sum to 1)
+    pf_vector_t pf_pose_mean;  // Mean of pose esimate
+    pf_matrix_t pf_pose_cov;   // Covariance of pose estimate
+  } amcl_hyp_t;
+
+  // Map-related
+  /*
+   * @brief Get new map from ROS topic to localize in
+   * @param msg Map message
+   */
+  void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+  /*
+   * @brief Handle a new map message
+   * @param msg Map message
+   */
+  void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg);
+  /*
+   * @brief Creates lookup table of free cells in map
+   */
+  void createFreeSpaceVector();
+  /*
+   * @brief Frees allocated map related memory
+   */
+  void freeMapDependentMemory();
+  map_t * map_{nullptr};
+  /*
+   * @brief Convert an occupancy grid map to an AMCL map
+   * @param map_msg Map message
+   * @return pointer to map for AMCL to use
+   */
+  map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg);
+  bool first_map_only_{true};
+  std::atomic<bool> first_map_received_{false};
+  amcl_hyp_t * initial_pose_hyp_;
+  std::recursive_mutex mutex_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
+#if NEW_UNIFORM_SAMPLING
+  static std::vector<std::pair<int, int>> free_space_indices;
+#endif
+
+  // Transforms
+  /*
+   * @brief Initialize required ROS transformations
+   */
+  void initTransforms();
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  bool sent_first_transform_{false};
+  bool latest_tf_valid_{false};
+  tf2::Transform latest_tf_;
+
+  // Message filters
+  /*
+   * @brief Initialize incoming data message subscribers and filters
+   */
+  void initMessageFilters();
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
+  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
+  message_filters::Connection laser_scan_connection_;
+
+  // Publishers and subscribers
+  /*
+   * @brief Initialize pub subs of AMCL
+   */
+  void initPubSub();
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
+    initial_pose_sub_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    pose_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
+    particle_cloud_pub_;
+  /*
+   * @brief Handle with an initial pose estimate is received
+   */
+  void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  /*
+   * @brief Handle when a laser scan is received
+   */
+  void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
+
+  // Services and service callbacks
+  /*
+   * @brief Initialize state services
+   */
+  void initServices();
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
+  /*
+   * @brief Service callback for a global relocalization request
+   */
+  void globalLocalizationCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+  // service server for providing an initial pose guess
+  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  /*
+   * @brief Service callback for an initial pose guess request
+   */
+  void initialPoseReceivedSrv(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
+    std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
+
+  // Let amcl update samples without requiring motion
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
+  /*
+   * @brief Request an AMCL update even though the robot hasn't moved
+   */
+  void nomotionUpdateCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+  // Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs
+  std::atomic<bool> force_update_{false};
+
+  // Odometry
+  /*
+   * @brief Initialize odometry
+   */
+  void initOdometry();
+  std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
+  geometry_msgs::msg::PoseStamped latest_odom_pose_;
+  geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
+  double init_pose_[3];  // Initial robot pose
+  double init_cov_[3];
+  pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl",
+    "nav2_amcl::MotionModel"};
+  /*
+   * @brief Get robot pose in odom frame using TF
+   */
+  bool getOdomPose(
+    // Helper to get odometric pose from transform system
+    geometry_msgs::msg::PoseStamped & pose,
+    double & x, double & y, double & yaw,
+    const rclcpp::Time & sensor_timestamp, const std::string & frame_id);
+  std::atomic<bool> first_pose_sent_;
+
+  // Particle filter
+  /*
+   * @brief Initialize particle filter
+   */
+  void initParticleFilter();
+  /*
+   * @brief Pose-generating function used to uniformly distribute particles over the map
+   */
+  static pf_vector_t uniformPoseGenerator(void * arg);
+  pf_t * pf_{nullptr};
+  bool pf_init_;
+  pf_vector_t pf_odom_pose_;
+  int resample_count_{0};
+
+  // Laser scan related
+  /*
+   * @brief Initialize laser scan
+   */
+  void initLaserScan();
+  /*
+   * @brief Create a laser object
+   */
+  nav2_amcl::Laser * createLaserObject();
+  int scan_error_count_{0};
+  std::vector<nav2_amcl::Laser *> lasers_;
+  std::vector<bool> lasers_update_;
+  std::map<std::string, int> frame_to_laser_;
+  rclcpp::Time last_laser_received_ts_;
+
+  /*
+   * @brief Check if sufficient time has elapsed to get an update
+   */
+  bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time);
+  rclcpp::Time last_time_printed_msg_;
+  /*
+   * @brief Add a new laser scanner if a new one is received in the laser scallbacks
+   */
+  bool addNewScanner(
+    int & laser_index,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::string & laser_scan_frame_id,
+    geometry_msgs::msg::PoseStamped & laser_pose);
+  /*
+   * @brief Whether the pf needs to be updated
+   */
+  bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta);
+  /*
+   * @brief Update the PF
+   */
+  bool updateFilter(
+    const int & laser_index,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const pf_vector_t & pose);
+  /*
+   * @brief Publish particle cloud
+   */
+  void publishParticleCloud(const pf_sample_set_t * set);
+  /*
+   * @brief Get the current state estimat hypothesis from the particle cloud
+   */
+  bool getMaxWeightHyp(
+    std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
+    int & max_weight_hyp);
+  /*
+   * @brief Publish robot pose in map frame from AMCL
+   */
+  void publishAmclPose(
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp);
+  /*
+   * @brief Determine TF transformation from map to odom
+   */
+  void calculateMaptoOdomTransform(
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::vector<amcl_hyp_t> & hyps,
+    const int & max_weight_hyp);
+  /*
+   * @brief Publish TF transformation from map to odom
+   */
+  void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
+  /*
+   * @brief Handle a new pose estimate callback
+   */
+  void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg);
+  bool init_pose_received_on_inactive{false};
+  bool initial_pose_is_known_{false};
+  bool set_initial_pose_{false};
+  bool always_reset_initial_pose_;
+  double initial_pose_x_;
+  double initial_pose_y_;
+  double initial_pose_z_;
+  double initial_pose_yaw_;
+
+  /*
+   * @brief Get ROS parameters for node
+   */
+  void initParameters();
+  double alpha1_;
+  double alpha2_;
+  double alpha3_;
+  double alpha4_;
+  double alpha5_;
+  std::string base_frame_id_;
+  double beam_skip_distance_;
+  double beam_skip_error_threshold_;
+  double beam_skip_threshold_;
+  bool do_beamskip_;
+  std::string global_frame_id_;
+  double lambda_short_;
+  double laser_likelihood_max_dist_;
+  double laser_max_range_;
+  double laser_min_range_;
+  std::string sensor_model_type_;
+  int max_beams_;
+  int max_particles_;
+  int min_particles_;
+  std::string odom_frame_id_;
+  double pf_err_;
+  double pf_z_;
+  double alpha_fast_;
+  double alpha_slow_;
+  int resample_interval_;
+  std::string robot_model_type_;
+  tf2::Duration save_pose_period_;
+  double sigma_hit_;
+  bool tf_broadcast_;
+  tf2::Duration transform_tolerance_;
+  double a_thresh_;
+  double d_thresh_;
+  double z_hit_;
+  double z_max_;
+  double z_short_;
+  double z_rand_;
+  std::string scan_topic_{"scan"};
+  std::string map_topic_{"map"};
+};
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__AMCL_NODE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/angleutils.hpp

@@ -0,0 +1,78 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__ANGLEUTILS_HPP_
+#define NAV2_AMCL__ANGLEUTILS_HPP_
+
+#include <math.h>
+
+namespace nav2_amcl
+{
+
+/*
+ * @class angleutils
+ * @brief Some utilities for working with angles
+ */
+class angleutils
+{
+public:
+  /*
+   * @brief Normalize angles
+   * @brief z Angle to normalize
+   * @return normalized angle
+   */
+  static double normalize(double z);
+
+  /*
+   * @brief Find minimum distance between 2 angles
+   * @brief a Angle 1
+   * @brief b Angle 2
+   * @return normalized angle difference
+   */
+  static double angle_diff(double a, double b);
+};
+
+inline double
+angleutils::normalize(double z)
+{
+  return atan2(sin(z), cos(z));
+}
+
+inline double
+angleutils::angle_diff(double a, double b)
+{
+  a = normalize(a);
+  b = normalize(b);
+  double d1 = a - b;
+  double d2 = 2 * M_PI - fabs(d1);
+  if (d1 > 0) {
+    d2 *= -1.0;
+  }
+  if (fabs(d1) < fabs(d2)) {
+    return d1;
+  } else {
+    return d2;
+  }
+}
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__ANGLEUTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/map/map.hpp

@@ -0,0 +1,138 @@
+/*
+ *  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 NAV2_AMCL__MAP__MAP_HPP_
+#define NAV2_AMCL__MAP__MAP_HPP_
+
+#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);
+
+// 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  // NAV2_AMCL__MAP__MAP_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class DifferentialMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5);
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif  // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

@@ -0,0 +1,62 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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
+
+#ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
+
+#include <string>
+#include <memory>
+
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_pdf.hpp"
+
+namespace nav2_amcl
+{
+
+/**
+ * @class nav2_amcl::MotionModel
+ * @brief An abstract motion model class
+ */
+class MotionModel
+{
+public:
+  virtual ~MotionModel() = default;
+
+  /**
+   * @brief An factory to create motion models
+   * @param type Type of motion model to create in factory
+   * @param alpha1 error parameters, see documentation
+   * @param alpha2 error parameters, see documentation
+   * @param alpha3 error parameters, see documentation
+   * @param alpha4 error parameters, see documentation
+   * @param alpha5 error parameters, see documentation
+   * @return MotionModel A pointer to the motion model it created
+   */
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5) = 0;
+
+  /**
+   * @brief Update on new odometry data
+   * @param pf The particle filter to update
+   * @param pose pose of robot in odometry update
+   * @param delta change in pose in odometry update
+   */
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
+};
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class OmniMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5);
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif  // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/eig3.hpp

@@ -0,0 +1,31 @@
+/*
+ *  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
+ *
+ */
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+   Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef NAV2_AMCL__PF__EIG3_HPP_
+#define NAV2_AMCL__PF__EIG3_HPP_
+
+/* 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  // NAV2_AMCL__PF__EIG3_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf.hpp

@@ -0,0 +1,200 @@
+/*
+ *  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 NAV2_AMCL__PF__PF_HPP_
+#define NAV2_AMCL__PF__PF_HPP_
+
+#include "nav2_amcl/pf/pf_vector.hpp"
+#include "nav2_amcl/pf/pf_kdtree.hpp"
+
+#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;
+} 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;
+
+  // 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;
+
+  double dist_threshold;  // distance threshold in each axis over which the pf is considered to not
+                          // be converged
+  int converged;
+} 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);
+
+// 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, void * random_pose_data);
+
+// 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);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  // NAV2_AMCL__PF__PF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp

@@ -0,0 +1,107 @@
+/*
+ *  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 NAV2_AMCL__PF__PF_KDTREE_HPP_
+#define NAV2_AMCL__PF__PF_KDTREE_HPP_
+
+#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  // NAV2_AMCL__PF__PF_KDTREE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp

@@ -0,0 +1,84 @@
+/*
+ *  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 NAV2_AMCL__PF__PF_PDF_HPP_
+#define NAV2_AMCL__PF__PF_PDF_HPP_
+
+#include "nav2_amcl/pf/pf_vector.hpp"
+
+// #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  // NAV2_AMCL__PF__PF_PDF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp

@@ -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 NAV2_AMCL__PF__PF_VECTOR_HPP_
+#define NAV2_AMCL__PF__PF_VECTOR_HPP_
+
+#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(void);
+
+// 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(void);
+
+// 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  // NAV2_AMCL__PF__PF_VECTOR_HPP_

navigation2/nav2_amcl/include/nav2_amcl/portable_utils.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef NAV2_AMCL__PORTABLE_UTILS_HPP_
+#define NAV2_AMCL__PORTABLE_UTILS_HPP_
+
+#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.
+static double drand48(void)
+{
+  return ((double)rand()) / RAND_MAX;// NOLINT
+}
+
+static void srand48(long int seedval)// NOLINT
+{
+  srand(seedval);
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__PORTABLE_UTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

@@ -0,0 +1,210 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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
+
+
+#ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
+#define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
+
+#include <string>
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_pdf.hpp"
+#include "nav2_amcl/map/map.hpp"
+
+namespace nav2_amcl
+{
+
+// Forward declarations
+class LaserData;
+
+/*
+ * @class Laser
+ * @brief Base class for laser sensor models
+ */
+class Laser
+{
+public:
+  /**
+   * @brief A Laser constructor
+   * @param max_beams number of beams to use
+   * @param map Map pointer to use
+   */
+  Laser(size_t max_beams, map_t * map);
+
+  /*
+   * @brief Laser destructor
+   */
+  virtual ~Laser();
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0;
+
+  /*
+   * @brief Set the laser pose from an update
+   * @param laser_pose Pose of the laser
+   */
+  void SetLaserPose(pf_vector_t & laser_pose);
+
+protected:
+  double z_hit_;
+  double z_rand_;
+  double sigma_hit_;
+
+  /*
+   * @brief Reallocate weights
+   * @param max_samples Max number of samples
+   * @param max_obs number of observations
+   */
+  void reallocTempData(int max_samples, int max_obs);
+  map_t * map_;
+  pf_vector_t laser_pose_;
+  int max_beams_;
+  int max_samples_;
+  int max_obs_;
+  double ** temp_obs_;
+};
+
+/*
+ * @class LaserData
+ * @brief Class of laser data to process
+ */
+class LaserData
+{
+public:
+  Laser * laser;
+
+  /*
+   * @brief LaserData constructor
+   */
+  LaserData() {ranges = NULL;}
+  /*
+   * @brief LaserData destructor
+   */
+  virtual ~LaserData() {delete[] ranges;}
+
+public:
+  int range_count;
+  double range_max;
+  double(*ranges)[2];
+};
+
+/*
+ * @class BeamModel
+ * @brief Beam model laser sensor
+ */
+class BeamModel : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  BeamModel(
+    double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
+    double lambda_short, double chi_outlier, size_t max_beams, map_t * map);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+  double z_short_;
+  double z_max_;
+  double lambda_short_;
+  double chi_outlier_;
+};
+
+/*
+ * @class LikelihoodFieldModel
+ * @brief likelihood field model laser sensor
+ */
+class LikelihoodFieldModel : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  LikelihoodFieldModel(
+    double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
+    size_t max_beams, map_t * map);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  /*
+   * @brief Perform the update function
+   * @param data Laser data to use
+   * @param pf Particle filter to use
+   * @return if it was succesful
+   */
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+};
+
+/*
+ * @class LikelihoodFieldModelProb
+ * @brief likelihood prob model laser sensor
+ */
+class LikelihoodFieldModelProb : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  LikelihoodFieldModelProb(
+    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,
+    size_t max_beams, map_t * map);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  /*
+   * @brief Perform the update function
+   * @param data Laser data to use
+   * @param pf Particle filter to use
+   * @return if it was succesful
+   */
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+  bool do_beamskip_;
+  double beam_skip_distance_;
+  double beam_skip_threshold_;
+  double beam_skip_error_threshold_;
+};
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__SENSORS__LASER__LASER_HPP_