317 lines
13 KiB
C++
317 lines
13 KiB
C++
/* 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_ */
|