Class AgentPathPrediction

Nested Relationships

Nested Types

Class Documentation

class AgentPathPrediction

Public Functions

AgentPathPrediction() = default

Default constructor for AgentPathPrediction class.

~AgentPathPrediction() = default

Default destructor for AgentPathPrediction class.

void initialize()

Initializes the AgentPathPrediction node and sets up ROS communication.

void setParams(double velobs_mul, double velobs_min_rad, double velobs_max_rad, double velobs_max_rad_time, bool velobs_use_ang)

Sets parameters for velocity obstacle-based prediction.

Parameters:

  • velobs_mul – Velocity obstacle multiplier

  • velobs_min_rad – Minimum radius for velocity obstacle

  • velobs_max_rad – Maximum radius for velocity obstacle

  • velobs_max_rad_time – Time for maximum radius velocity obstacle calculation

  • velobs_use_ang – Whether to use angular velocity in predictions

Private Functions

void trackedAgentsCB(const cohan_msgs::TrackedAgents &tracked_agents)

Callback for tracked agents updates.

Parameters:

tracked_agents – The tracked agents message containing current agent states

void externalPathsCB(const cohan_msgs::AgentPathArray::ConstPtr &external_paths)

Callback for external path updates.

Parameters:

external_paths – Array of external paths for agents

void predictedGoalCB(const agent_path_prediction::PredictedGoals::ConstPtr &predicted_goal)

Callback for predicted goal updates.

Parameters:

predicted_goal – The predicted goals message

bool predictAgents(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res)

Service to predict agent paths using default prediction method.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

Returns:

True if prediction successful, false otherwise

bool predictAgentsVelObs(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res) const

Service to predict agent paths using velocity obstacle method.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

Returns:

True if prediction successful, false otherwise

bool predictAgentsExternal(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res)

Service to predict agent paths using external path information.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

Returns:

True if prediction successful, false otherwise

bool predictAgentsBehind(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res)

Service to predict paths for agents assuming their goal is behind the robot.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

Returns:

True if prediction successful, false otherwise

bool predictAgentsGoal(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res)

Service to predict agent paths based on predicted goals.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

Returns:

True if prediction successful, false otherwise

bool predictAgentsFromPaths(agent_path_prediction::AgentPosePredict::Request &req, agent_path_prediction::AgentPosePredict::Response &res, const std::vector<AgentPathVel> &path_vels)

Service to predict agent paths from existing path data. This method is called internally at the end of different prediction mechanisms.

Parameters:

  • req – Service request containing agent data

  • res – Service response with predicted paths

  • path_vels – Vector of agent paths and velocities

Returns:

True if prediction successful, false otherwise

bool setGoal(agent_path_prediction::AgentGoal::Request &req, agent_path_prediction::AgentGoal::Response &res)

Service to set goals for agents.

Parameters:

  • req – Service request containing goal data

  • res – Service response

Returns:

True if goal setting successful, false otherwise

bool resetPredictionSrvs(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)

Service to reset all prediction services.

Parameters:

  • req – Empty service request

  • res – Empty service response

Returns:

True if reset successful, false otherwise

void reconfigureCB(agent_path_prediction::AgentPathPredictionConfig &config, uint32_t level)
void loadRosParamFromNodeHandle(const ros::NodeHandle &private_nh)

Loads ROS parameters from the node handle.

Parameters:

private_nh – Private node handle containing parameters

bool transformPoseTwist(const cohan_msgs::TrackedAgents &tracked_agents, const uint64_t &agent_id, const std::string &to_frame, geometry_msgs::PoseStamped &pose, geometry_msgs::TwistStamped &twist) const

Transforms pose and twist of agent to a target frame.

Parameters:

  • tracked_agents – Tracked agents data

  • agent_id – ID of the agent to transform

  • to_frame – Target frame for transformation

  • pose – Output transformed pose

  • twist – Output transformed twist

Returns:

True if transformation successful, false otherwise

Private Members

ros::Publisher predicted_agents_pub_
ros::Publisher front_pose_pub_
ros::Subscriber tracked_agents_sub_
ros::Subscriber external_paths_sub_
ros::Subscriber predicted_goal_sub_
ros::ServiceServer predict_agents_server_
ros::ServiceServer set_goal_srv_
ros::ServiceServer reset_prediction_services_server_
ros::ServiceClient get_plan_client_
tf::TransformListener tf_
dynamic_reconfigure::Server<agent_path_prediction::AgentPathPredictionConfig> *dsrv_
cohan_msgs::TrackedAgents tracked_agents_

Current state of tracked agents in the environment.

cohan_msgs::AgentPathArray::ConstPtr external_paths_

External paths provided for agents.

agent_path_prediction::PredictedGoals predicted_goals_

Collection of predicted goals for agents.

std::vector<agent_path_prediction::AgentPose> external_goals_

Vector storing external goals for agents.

std::vector<AgentPathVel> path_vels_

Vector storing path and velocity information for agents.

std::vector<int> path_vels_pos_

Vector storing positions in the path velocity array.

std::vector<agent_path_prediction::PredictedPoses> last_predicted_poses_

Vector storing the last predicted poses for agents.

std::map<uint64_t, size_t> last_prune_indices_

Map storing last pruning indices for each agent.

std::map<uint64_t, int> last_markers_size_map_

Map storing last marker sizes for each agent.

visualization_msgs::MarkerArray predicted_agents_markers_

Visualization markers for predicted agent paths.

std::string tracked_agents_sub_topic_
std::string external_paths_sub_topic_
std::string predict_service_name_
std::string predicted_agents_markers_pub_topic_
std::string get_plan_srv_name_
std::string predicted_goal_topic_

ROS topic names for subscribers and publishers.

std::string robot_frame_id_
std::string map_frame_id_

Frame IDs for coordinate transformations.

double velobs_mul_
double velobs_min_rad_
double velobs_max_rad_
double velobs_max_rad_time_

Velocity obstacle parameters.

double agent_dist_behind_robot_
double agent_angle_behind_robot_

Parameters for agents behind robot detection.

bool velobs_use_ang_
bool check_path_

Flags for velocity obstacles and path checking.

bool publish_markers_
bool showing_markers_

Flags for marker visualization.

bool got_new_agent_paths_
bool got_external_goal_

Flags for path updates.

int default_agent_part_

Default body part to track for agents.

geometry_msgs::Transform behind_pose_

Transform for behind pose calculation.

std::string ns_

Namespace for the node.

Private Static Functions

static nav_msgs::Path setFixedPath(const geometry_msgs::PoseStamped &start_pose)

Creates a fixed path from a start pose. It adds constant velocity based prediction for 0.5m infront of the human.

Parameters:

start_pose – The starting pose for the path (human’s current position)

Returns:

The generated fixed path

static size_t prunePath(size_t begin_index, const geometry_msgs::Pose &pose, const std::vector<geometry_msgs::PoseWithCovarianceStamped> &path)

Prunes a path starting from a given index based on pose.

Parameters:

  • begin_index – Starting index for pruning

  • pose – Reference pose for pruning

  • path – Vector of poses to prune

Returns:

Index of the given pose in the vector

static inline double checkdist(geometry_msgs::Pose agent, geometry_msgs::Pose robot)

Calculates Euclidean distance between agent and robot poses.

Parameters:

  • agent – Agent pose

  • robot – Robot pose

Returns:

Distance between agent and robot

struct AgentPathVel

Structure to store agent path and velocity information.

Public Members

uint64_t id
nav_msgs::Path path
geometry_msgs::TwistWithCovariance start_vel
struct AgentStartPoseVel

Structure to store agent’s initial pose and velocity.

Public Members

uint64_t id
geometry_msgs::PoseStamped pose
geometry_msgs::TwistWithCovariance vel