Class LineFootprint

Inheritance Relationships

Base Type

Class Documentation

class LineFootprint : public hateb_local_planner::BaseFootprintModel

Class that approximates the robot/human with line segment (zero-width)

Public Functions

inline LineFootprint(const geometry_msgs::Point &line_start, const geometry_msgs::Point &line_end)

Default constructor of the abstract obstacle class.

Parameters:

  • line_start – start coordinates (only x and y) of the line (w.r.t. robot/human center at (0,0))

  • line_end – end coordinates (only x and y) of the line (w.r.t. robot/human center at (0,0))

inline LineFootprint(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, const double min_obstacle_dist)

Default constructor of the abstract obstacle class (Eigen Version)

Parameters:

  • line_start – start coordinates (only x and y) of the line (w.r.t. robot/human center at (0,0))

  • line_end – end coordinates (only x and y) of the line (w.r.t. robot/human center at (0,0))

~LineFootprint() override = default

Virtual destructor.

inline void setLine(const geometry_msgs::Point &line_start, const geometry_msgs::Point &line_end)

Set vertices of the contour/footprint.

Parameters:

vertices – footprint vertices (only x and y) around the robot/human center (0,0) (do not repeat the first and last vertex at the end)

inline void setLine(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end)

Set vertices of the contour/footprint (Eigen version)

Parameters:

vertices – footprint vertices (only x and y) around the robot/human center (0,0) (do not repeat the first and last vertex at the end)

inline virtual double calculateDistance(const PoseSE2 &current_pose, const Obstacle *obstacle) const override

Calculate the distance between the robot/human and an obstacle.

Parameters:

  • current_pose – Current robot/human pose

  • obstacle – Pointer to the obstacle

Returns:

Euclidean distance to the robot/human

inline virtual double estimateSpatioTemporalDistance(const PoseSE2 &current_pose, const Obstacle *obstacle, double t) const override

Estimate the distance between the robot/human and the predicted location of an obstacle at time t.

Parameters:

  • current_pose – robot/human pose, from which the distance to the obstacle is estimated

  • obstacle – Pointer to the dynamic obstacle (constant velocity model is assumed)

  • t – time, for which the predicted distance to the obstacle is calculated

Returns:

Euclidean distance to the robot/human

inline virtual void visualizeModel(const PoseSE2 &current_pose, std::vector<visualization_msgs::Marker> &markers, const std_msgs::ColorRGBA &color) const override

Visualize the robot/human using a markers.

Fill a marker message with all necessary information (type, pose, scale and color). The header, namespace, id and marker lifetime will be overwritten.

Parameters:

  • current_pose – Current robot/human pose

  • markers – [out] container of marker messages describing the robot/human shape

  • color – Color of the footprint

inline virtual double getInscribedRadius() override

Compute the inscribed radius of the footprint model.

Returns:

inscribed radius

inline virtual double getCircumscribedRadius() const override

Compute the circumscribed radius of the footprint model.

Returns:

circumscribed radius

Private Functions

inline void transformToWorld(const PoseSE2 &current_pose, Eigen::Vector2d &line_start_world, Eigen::Vector2d &line_end_world) const

Transforms a line to the world frame manually.

Parameters:

  • current_pose – Current robot/human pose

  • line_start – [out] line_start_ in the world frame

  • line_end – [out] line_end_ in the world frame

Private Members

Eigen::Vector2d line_start_
Eigen::Vector2d line_end_
const double min_obstacle_dist_ = 0.0