sampler

Defines a custom sampler which only samples an state with the heading angle below the threshold.

namespace slope_constrained_planner

class Sampler : public StateSampler

#include <sampler.h>

Defines a custom sampler which only samples an state with the heading angle below the threshold.

Description: A custom sampler which randomly samples a state position within map bounderies. Regarding the state orientation, the yaw angle would be chosen randomly within (-pi, pi), roll and pitch angles are found by getting the normal vector of the state cell in the map. In order to get the desired orientation, state frame rotates along its z-axis until the heading angle gets below the threshold.

Public Functions

Sampler(const ob::StateSpace *si, const std::shared_ptr<Map> &map, const ParamsConstPtr &params)

Constructor for Sampler class. Defines the space bounderies using the grid map info.

Parameters:

• space – state space in which the sampler would find the states

• map – grid map of the environment

• params – contains required info about the map layers

virtual void sampleUniform(ob::State *state) override

Overriden function from the base class StateSampler, uses samplePositionInMap() function to find state position. Uses getNormal(ind) function to find roll and pitch angles for the state orientation. Keeps rotating the state frame along its z-axis until the heading angle gets below threshold. If previously found state was valid, use its reverse orientation for the next state.

Parameters:

state – The sampled state data

virtual void sampleUniformNear(ob::State*, const ob::State*, double) override

Not implemented in this software architecture.

virtual void sampleGaussian(ob::State*, const ob::State*, double) override

Not implemented in this software architecture.

bool checkSampleDistance(const ob::ScopedState<> &sample)

Private Functions

grid_map::Position samplePositionInMap()

Helper function to sample the state position within map bounderies and finit normal vector.

Returns:

pos state position info

Private Members

ParamsConstPtr params_

Parameters containing the data for map layers.

ob::StateSpacePtr space_real_vec_

Defines the states in R(3)

ob::StateSpacePtr space_rot_

Defines the states in SO3.

std::shared_ptr<ob::RealVectorBounds> bounds_se3_

Sets R(3) state space bounderies.

ob::ScopedState<ob::RealVectorStateSpace> state_pos_

An scoped state in R(3)

grid_map::Position prev_pos_

Represents the position of previously found valid state.

bool sample_is_near_robot_

An attribute to specify if the found sample is near robot.

ob::ScopedState<ob::SE3StateSpace> state_rot_

An scopes state in SE3.

ob::RealVectorStateSampler base_real_vec_

State sampler in R(2)

std::shared_ptr<Map> map_

Grid map data.

mutable std::mutex map_mutex_

Grid map process mutex.

bool prev_state_is_valid_

specifies whether the previously found state was valid

double prev_yaw_

Represents the yaw angle for previous valid state.

double prev_phi_

Represents the heading angle for previous valid state.

class SamplerAllocator

#include <sampler.h>

Allocates the custom sampler class in the planner.

Public Functions

inline SamplerAllocator(const ParamsConstPtr &params)

void setMap(const std::shared_ptr<Map> &map)

Sets the grid map data in the corresponding class attribute.

Parameters:

map – grid map of the environment

std::shared_ptr<Sampler> getSampler(const ob::StateSpace *space)

Gets the custom sampler class to allocate it in the planner.

Parameters:

space – state space in which the sampler would find the states

Returns:

Sampler a shared pointer to the custom sampler class

Private Members

ParamsConstPtr params_

Parameters containing the data for map layers.

std::shared_ptr<Map> map_

Grid map data.