Class Agents

Class Documentation

class Agents

Class representing agents and their states.

Public Functions

Agents()

Default constructor for Agents class.

Agents(tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)

Constructor with parameters for tf buffer and costmap.

Parameters:

  • tf – Pointer to tf buffer

  • costmap_ros – Pointer to costmap

~Agents() = default

Destructor of the class.

inline void setState(AgentState state, int id)

Set the state of an agent.

Parameters:

  • state – The state to set

  • id – The ID of the agent

void resetAgents()

Reset all agent states.

inline bool isAgentStuck() const

Check if any agent is stuck.

Returns:

True if an agent is stuck, false otherwise

inline void resetStuckAgent()

Reset the stuck agent ID.

inline std::map<int, geometry_msgs::Pose> getAgents()

Get the poses of all agents.

Returns:

Map of agent IDs to their poses

inline AgentState agentState(int id)

Get the state of the agent.

Parameters:

id – The index of agent whose state is required

Returns:

State of the given agent

inline std::map<int, double> getNominalVels()

Get nominal velocities of all agents based on a moving average filter.

Returns:

Map of agent IDs to their nominal velocities

Private Functions

void trackedAgentsCB(const cohan_msgs::TrackedAgents &tracked_agents)

Callback for tracked agents updates.

Parameters:

tracked_agents – The tracked agents message containing agent states

std::vector<int> filterVisibleAgents(std::map<int, geometry_msgs::Pose> tr_agents, std::vector<int> sorted_ids, std::map<int, double> agents_radii, geometry_msgs::Pose2D robot_pose)

Filters agents that are in the FOV of the robot (for e.g., in simulation or mocap)

Parameters:

  • tr_agents – Transformed agent poses

  • sorted_ids – Vector of sorted agent IDs

  • agents_radii – Agent radii

  • robot_pose – Current robot pose

Returns:

Vector of visible agent IDs in the FOV of the robot

void loadRosParamFromNodeHandle(const ros::NodeHandle &private_nh)

Loads parameters from ROS parameter server.

Parameters:

private_nh – Private node handle containing parameters

Private Members

cohan_msgs::TrackedAgents tracked_agents_

Latest tracked agents message.

std::map<int, geometry_msgs::Pose> agents_
std::map<int, geometry_msgs::Pose> prev_agents_

Current and previous agent poses.

std::map<int, bool> agent_still_

Map tracking if agents are stationary.

std::map<int, std::vector<double>> agent_vels_

List of agent velocities over time.

std::map<int, double> agent_nominal_vels_

Nominal velocities based on moving average.

std::map<int, AgentState> agents_states_
std::map<int, AgentState> prev_agents_states_

Current and previous agent states.

std::vector<int> visible_agent_ids_

List of visible agent IDs.

std::string ns_

Namespace for multiple agents.

std::string tracked_agents_sub_topic_

Topic for tracked agents subscription.

bool initialized_

Initialization status flag.

bool stuck_

Flag indicating if agent is stuck.

int stuck_agent_id_

ID of agent blocking robot’s path.

int window_moving_avg_

Window size for moving average calculation.

int planning_mode_

Mode of planning (different strategies)

double human_radius_

Radius considered for human agents.

double robot_radius_

Robot’s physical radius.

double planning_radius_

Radius used for planning.

std::string base_link_frame_

Robot’s base link frame ID.

std::string map_frame_

Map frame ID.

std::string odom_frame_

Odometry frame ID.

bool use_simulated_fov_

Flag for using simulated field of view.

ros::Publisher agents_info_pub_

Publisher for agent information.

ros::Subscriber tracked_agents_sub_

Subscriber for tracked agents.

tf2_ros::Buffer *tf_

Pointer to tf buffer.

costmap_2d::Costmap2DROS *costmap_ros_

Pointer to the costmap ros wrapper.

costmap_2d::Costmap2D *costmap_

Pointer to the 2d costmap.

double inflation_radius_

Inflation radius for costmap.