/* Copyright 2019-2020 The MathWorks, Inc. */
/**
* @file
* External C-API interfaces for Adaptive Monte Carlo Localization (AMCL).
* To fully support code generation, note that this file needs to be fully
* compliant with the C89/C90 (ANSI) standard.
*/
#ifndef MCLCODEGEN_API_H_
#define MCLCODEGEN_API_H_
#ifdef BUILDING_LIBMWMCLCODEGEN
#include "mclcodegen/mclcodegen_util.hpp"
#else
/* To deal with the fact that PackNGo has no include file hierarchy during test */
#include "mclcodegen_util.hpp"
#endif
/**
* @brief Initialize algorithm with random see
*
* @param[in] seed Global random seed used for resampling and initialization.
* @return Initialized Monte Carlo Localization object.
*/
EXTERN_C MCL_CODEGEN_API void* mclInitializeWithSeed_real64(real64_T seed);
/**
* @brief Free memory and reset to initial state
*/
EXTERN_C MCL_CODEGEN_API void mclCleanup_real64(void* mclObj);
/**
* @brief Set the motion model.
*
* This method creates an odometry differential drive motion model and
* assigns the values to its error parameters.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] motionParams Four element array of error parameters.
*/
EXTERN_C MCL_CODEGEN_API void mclSetMotionModel_real64(void* mclObj, const real64_T* motionParams);
/**
* @brief Sets the occupancy grid map.
*
* This method assigns the values from a MATLAB occupancy grid to
* the AMCL implementation of an occupancy grid.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] width Width of the Occupancy Grid in number of cells.
* @param[in] height Height of the Occupancy Grid in number of cells.
* @param[in] resolution Resolution of the Occupancy Grid in meters/cell.
* @param[in] originx X location for origin. The origin is the center of the map.
* @param[in] originy Y location for origin. The origin is the center of the map.
* @param[in] gridCells Occupancy grid data in row-major format 1D array.
*/
EXTERN_C MCL_CODEGEN_API void mclSetOccupancyGrid_real64(void* mclObj,
real64_T width,
real64_T height,
real64_T resolution,
real64_T originx,
real64_T originy,
const real64_T* gridCells);
/**
* @brief Set the sensor model.
*
* This method creates a likelihood field sensor model for range sensors
* and assigns the sensor parameters.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] max_beams Max beams to be used for likelihood computation
* @param[in] z_hit Weight for probability of correct measurement
* @param[in] z_rand Weight for probability of random measurement
* @param[in] sigma_hit Standard deviation for noise in correct measurement
* @param[in] laser_likelihood_max_dist Max distance to search for obstacles
* @param[in] sensorLimits Min and Max measurement limits of the range sensor, [min max]
* @param[in] sensorPoseParams Sensor location relative to robot base [x y yaw]
*/
EXTERN_C MCL_CODEGEN_API void mclSetSensorModel_real64(void* mclObj,
real64_T max_beams,
real64_T z_hit,
real64_T z_rand,
real64_T sigma_hit,
real64_T laser_likelihood_max_dist,
const real64_T* sensorLimits,
const real64_T* sensorPoseParams);
/**
* @brief Initialize particle filter.
*
* This method initializes the particle filter and assigns its
* parameters. All the particles are initialized to zero.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] minParticles Minimum number of particles
* @param[in] maxParticles Maximum number of particles
* @param[in] alphaSlow Parameter that affects recovery behavior
* @param[in] alphaFast Parameter that affects recovery behavior
* @param[in] kldErr KLD sampling parameter - error bound
* @param[in] kldZ KLD sampling parameter - probability bound
*/
EXTERN_C MCL_CODEGEN_API void mclInitializePf_real64(void* mclObj,
real64_T minParticles,
real64_T maxParticles,
real64_T alphaSlow,
real64_T alphaFast,
real64_T kldErr,
real64_T kldZ);
/**
* @brief Set initial pose and covariance.
*
* This method initializes particles based on given initial pose and
* covariance.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] poseParams Initial pose of the robot 3 element array.
* @param[in] covParams Initial covariance 9 element array in column major format.
*/
EXTERN_C MCL_CODEGEN_API void mclSetInitialPose_real64(void* mclObj,
const real64_T* poseParams,
const real64_T* covParams);
/**
* @brief Single call update, calls predict, correct and resample.
*
* The method calls the predict, correct and resample methods respectively.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] rangeCount Number of ranges in laser scan.
* @param[in] rangesParams Range values from laser scanner.
* @param[in] anglesParams Angles corresponding to the ranges.
* @param[in] poseParams Odometry pose of the robot [x, y, theta].
*/
EXTERN_C MCL_CODEGEN_API void mclUpdate_real64(void* mclObj,
real64_T rangeCount,
const real64_T* rangesParams,
const real64_T* anglesParams,
const real64_T* poseParams);
/**
* @brief Get mean and covariance estimate.
*
* The method returns mean and covariance of the maximum weighted
* cluster of particles.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[out] maxWeightHyp 12 elements of mean and covariance in row major format
*/
EXTERN_C MCL_CODEGEN_API void mclGetHypothesis_real64(void* mclObj, real64_T* maxWeightHyp);
/**
* @brief Set particles and weights.
*
* This function allows to set particles and their weights for testing
* purposes.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] particles Particle cloud where rows represent states, [x y theta]
* @param[in] weights Particle weights as an array (one per particle)
*/
EXTERN_C MCL_CODEGEN_API void mclSetParticles_real64(void* mclObj,
real64_T* particles,
real64_T* weights);
/**
* @brief Get number of particles currently used in particle filter.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @return Number of particles
*/
EXTERN_C MCL_CODEGEN_API uint32_T mclGetNumParticles_real64(void* mclObj);
/**
* @brief Get states for all particles.
*
* This method returns [x y theta] states of all particles in a
* vector form. The returned vector is a 3*N vector representing
* [x, y, theta] states for N particles.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[out] particleInfo A vector representing [x, y, theta] states of particles in column major
* format
*/
EXTERN_C MCL_CODEGEN_API void mclGetParticles_real64(void* mclObj, real64_T* particleInfo);
/**
* @brief Motion update on particle filter.
*
* This is the prediction step of the particle filter. This function updates
* the particles based on the motion model and the input odometry pose.
* The particles are not updated for first call or if robot has moved
* less then update thresholds.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] poseParams Odometry pose of the robot [x y theta].
*/
EXTERN_C MCL_CODEGEN_API void mclPredict_real64(void* mclObj, const real64_T* poseParams);
/**
* @brief Sensor update on particle filter.
*
* This is the correction step of the particle filter. This function
* updates the particle weights based on sensor data. The weights are
* not updated if the robot has moved less then update thresholds.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] rangeCount Number of ranges in laser scan.
* @param[in] rangesParams Range values from laser scanner.
* @param[in] anglesParams Angles corresponding to the ranges.
*/
EXTERN_C MCL_CODEGEN_API void mclCorrect_real64(void* mclObj,
real64_T rangeCount,
const real64_T* rangesParams,
const real64_T* anglesParams);
/**
* @brief Resample particles.
*
* This is the resampling step of the particle filter. It resamples
* particles if resampling interval is satisfied.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
*/
EXTERN_C MCL_CODEGEN_API void mclResample_real64(void* mclObj);
/**
* @brief Set resampling interval.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] interval Resampling interval.
*/
EXTERN_C MCL_CODEGEN_API void mclSetResamplingInterval_real64(void* mclObj, real64_T interval);
/**
* @brief Get resampling interval.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
* @return resampling interval.
*/
EXTERN_C MCL_CODEGEN_API real64_T mclGetResamplingInterval_real64(void* mclObj);
/**
* @brief Get occupancy value from occupancy grid using world coordinates
*
* This function is used for testing purposes.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] x X world location to check for occupancy
* @param[in] y Y world location to check for occupancy
* @return Occupancy value -1 for free, 1 for occupied
*/
EXTERN_C MCL_CODEGEN_API real64_T mclGetOccupancyWorld_real64(void* mclObj, real64_T x, real64_T y);
/**
* @brief Set state change thresholds to trigger update.
*
* This method sets thresholds for states [x y theta] change
* based on which the motion and sensor updates are triggered.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] x Threshold for change in state X
* @param[in] y Threshold for change in state Y
* @param[in] theta Threshold for change in state Theta
*/
EXTERN_C MCL_CODEGEN_API void mclSetUpdateThresholds_real64(void* mclObj,
real64_T x,
real64_T y,
real64_T theta);
/**
* @brief Get update thresholds.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[out] x Threshold for change in state X
* @param[out] y Threshold for change in state Y
* @param[out] theta Threshold for change in state Theta
*/
EXTERN_C MCL_CODEGEN_API void mclGetUpdateThresholds_real64(void* mclObj,
real64_T* x,
real64_T* y,
real64_T* theta);
/**
* @brief Returns true if particle filter is updated.
*
* This function returns true if the particle filter was updated
* in the last call to "update". This means "predict" and "correct"
* methods were executed.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @retval TRUE if particle filter was updated
* @retval FALSE if it was not updated
*/
EXTERN_C MCL_CODEGEN_API boolean_T mclIsUpdated_real64(void* mclObj);
/**
* @brief Returns true if particle filter is resampled.
*
* This function returns true if the mean estimate was updated
* in the last call to "update" as a result of resampling.
* @param[in] mclObj MonteCarloLocalizationInternal object
* @retval TRUE if particle filter was resampled
* @retval FALSE if it was not resampled
*/
EXTERN_C MCL_CODEGEN_API boolean_T mclIsResampled_real64(void* mclObj);
/**
* @brief Initialize particles uniformly in entire map.
*
* This method initializes particles uniformly in the entire map.
* This is used to start the global localization.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
*/
EXTERN_C MCL_CODEGEN_API void mclGlobalLocalization_real64(void* mclObj);
/**
* @brief Set the test hook value
*
* This function allows setting the testHookForMemory property.
* The test hook enables negative tests for memory error.
*
* @param[in] mclObj MonteCarloLocalizationInternal object
* @param[in] value Boolean value for test hook setting
*/
EXTERN_C MCL_CODEGEN_API void mclSetTestHookForMemory_real64(void* mclObj, boolean_T value);
#endif /* MCLCODEGEN_API_H_ */