Class MapScanner

Class Documentation

class MapScanner

Class for scanning maps to detect invisible humans.

Public Functions

MapScanner()

Default constructor for MapScanner class.

~MapScanner()

Default destructor for MapScanner class.

void initialize()

Initializes the MapScanner node and sets up ROS.

Private Functions

void loadRosParamFromNodeHandle(const ros::NodeHandle &private_nh)

Loads ROS parameters from the node handle.

Parameters:

private_nh – Private node handle containing parameters

void mapCB(const nav_msgs::OccupancyGrid &grid)

Callback for map updates.

Parameters:

grid – The occupancy grid message containing map data

bool locateInvHumans(Coordinates c1, Coordinates c2, std::vector<char> direction, geometry_msgs::TransformStamped &footprint_transform)

Locates invisible humans based on the detected corner sets.

Parameters:

  • c1 – First set of corners

  • c2 – Second set of corners

  • direction – Directions vector (positive or negative to select the right or left point)

  • footprint_transform – Transform of the robot footprint to the world frame

Returns:

True if invisible humans are located successfully, false otherwise

void detectOccludedCorners(const ros::TimerEvent &event)

Detects occluded corners in the map where humans might be present.

Parameters:

event – Timer event triggering the detection

void publishInvisibleHumans(const geometry_msgs::PoseArray &corners, const geometry_msgs::PoseArray &poses, std::vector<std::vector<double>> &inv_humans)

Publishes detected invisible humans (corners, poses and obstacles message)

Parameters:

  • corners – Detected corners

  • poses – Detected poses

  • inv_humans – Invisible humans info for Obstacle Msg

void detectPassages(geometry_msgs::PoseArray detections)

Detects different kinds of passages in the map.

Parameters:

detections – Array of detected invisible humans

inline bool worldToMap(double wx, double wy, int &mx, int &my) const

Converts world coordinates to map coordinates.

Parameters:

  • wx – World x-coordinate

  • wy – World y-coordinate

  • mx – Map x-coordinate (output)

  • my – Map y-coordinate (output)

Returns:

True if conversion is successful, false otherwise

inline unsigned int getIndex(unsigned int mx, unsigned int my) const

Gets the index of a map cell based on its coordinates.

Parameters:

  • mx – Map x-coordinate

  • my – Map y-coordinate

Returns:

Index of the map cell

Private Members

ros::Timer get_robot_pose_

Timer for periodically updating the robot’s pose.

geometry_msgs::PoseStamped robot_pose_

Current pose of the robot.

tf2_ros::Buffer tf_

Buffer for storing TF2 transformations.

ros::Subscriber map_sub_

Subscriber for receiving map updates.

ros::Publisher scan_pub_

Publisher for scan data.

ros::Publisher pub_invis_human_viz_

Publisher for visualizing invisible humans.

ros::Publisher pub_invis_human_

Publisher for invisible human obstacle msg.

ros::Publisher pub_invis_human_corners_

Publisher for invisible human corner data.

ros::Publisher pub_invis_humans_pos_

Publisher for invisible human positions.

ros::Publisher passage_detect_pub_

Publisher for detected passages.

nav_msgs::OccupancyGrid map_

Occupancy grid map data.

std::vector<float> ranges_

Ranges from sensor data.

std::vector<double> corner_ranges_

Ranges for detected corners.

int samples_

Number of samples for scanning.

int scan_resolution_

Resolution of the scan.

int size_x_

Size of the map in the x-direction.

int size_y_

Size of the map in the y-direction.

double origin_x_

Origin of the map in the x-direction.

double origin_y_

Origin of the map in the y-direction.

double resolution_

Resolution of the map.

double angle_min_

Minimum angle for scanning.

double angle_max_

Maximum angle for scanning.

double range_min_

Minimum range for scanning.

double range_max_

Maximum range for scanning.

sensor_msgs::LaserScan scan_msg_

Laser scan message.

bool publish_scan_

Flag to indicate whether to publish scan data.

double human_radius_

Radius of a human for detection.

std::string ns_

Namespace of the node.

Eigen::Vector2d robot_vec_

Unit vector in the direction of the robot.

Private Static Functions

static inline Point getLeftPoint(Point p1, Point p2, Point p3, double dist = 1)

Calculates a point perpendicular to p1, p2 and passing through p3 on the left of \vec(p1p2)

Parameters:

  • p1 – First point

  • p2 – Second point

  • p3 – Third point

  • dist – Distance for calculation

Returns:

Calculated point to the left of p3 at a given distance

static inline Point getRightPoint(Point p1, Point p2, Point p3, double dist = 1)

Calculates a point perpendicular to p1, p2 and passing through p3 on the right of \vec(p1p2)

Parameters:

  • p1 – First point

  • p2 – Second point

  • p3 – Third point

  • dist – Distance for calculation

Returns:

Calculated point to the right of p3 at a given distance

static inline std::vector<Point> getTwoPoints(Point p1, Point p2, double radius)

Calculates points perpendicular to p1, p2 and passing through p2 on the right and left of \vec(p1p2)

Parameters:

  • p1 – First point

  • p2 – Second point

  • radius – Radius of human for calculation

Returns:

Vector containing the two calculated points