Class PathHandler

Class Documentation

class PathHandler

Manager of incoming reference paths for transformation and processing.

Public Functions

PathHandler() = default

Constructor for mppi::PathHandler.

~PathHandler() = default

Destructor for mppi::PathHandler.

void initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr<nav2_costmap_2d::Costmap2DROS>, std::shared_ptr<tf2_ros::Buffer>, ParametersHandler*)

Initialize path handler on bringup.

Parameters:

  • parent – WeakPtr to node

  • name – Name of plugin

  • costmap_ros – Costmap2DROS object of environment

  • tf – TF buffer for transformations

  • dynamic_parameter_handler – Parameter handler object

void setPath(const nav_msgs::msg::Path &plan)

Set new reference path.

Parameters:

Plan – Path to use

nav_msgs::msg::Path &getPath()

Get reference path.

Returns:

Path

nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped &robot_pose)

transform global plan to local applying constraints, then prune global plan

Parameters:

robot_pose – Pose of robot

Returns:

global plan in local frame

Protected Functions

bool transformPose(const std::string &frame, const geometry_msgs::msg::PoseStamped &in_pose, geometry_msgs::msg::PoseStamped &out_pose) const

Transform a pose to another frame.

Parameters:

  • frame – Frame to transform to

  • in_pose – Input pose

  • out_pose – Output pose

Returns:

Bool if successful

double getMaxCostmapDist()

Get largest dimension of costmap (radially)

Returns:

Max distance from center of costmap to edge

geometry_msgs::msg::PoseStamped transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped &pose)

Transform a pose to the global reference frame.

Parameters:

pose – Current pose

Returns:

output poose in global reference frame

std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(const geometry_msgs::msg::PoseStamped &global_pose)

Get global plan within window of the local costmap size.

Parameters:

global_pose – Robot pose

Returns:

plan transformed in the costmap frame and iterator to the first pose of the global plan (for pruning)

void prunePlan(nav_msgs::msg::Path &plan, const PathIterator end)

Prune a path to only interesting portions.

Parameters:

  • plan – Plan to prune

  • end – Final path iterator

bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped &robot_pose)

Check if the robot pose is within the set inversion tolerances.

Parameters:

robot_pose – Robot’s current pose to check

Returns:

bool If the robot pose is within the set inversion tolerances

Protected Attributes

std::string name_
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_
std::shared_ptr<tf2_ros::Buffer> tf_buffer_
ParametersHandler *parameters_handler_
nav_msgs::msg::Path global_plan_
nav_msgs::msg::Path global_plan_up_to_inversion_
rclcpp::Logger logger_ = {rclcpp::get_logger("MPPIController")}
double max_robot_pose_search_dist_ = {0}
double prune_distance_ = {0}
double transform_tolerance_ = {0}
float inversion_xy_tolerance_ = {0.2}
float inversion_yaw_tolerance = {0.4}
bool enforce_path_inversion_ = {false}
unsigned int inversion_locale_ = {0u}