diff --git a/src/world_modeling/prediction/src/trajectory_predictor.cpp b/src/world_modeling/prediction/src/trajectory_predictor.cpp index 7fadc48ba..b370e4821 100644 --- a/src/world_modeling/prediction/src/trajectory_predictor.cpp +++ b/src/world_modeling/prediction/src/trajectory_predictor.cpp @@ -246,44 +246,152 @@ std::vector TrajectoryPredictor::generateVehicleHypotheses return hypotheses; } -std::vector TrajectoryPredictor::generatePedestrianHypotheses( +std::vectorTrajectoryPredictor::generatePedestrianHypotheses( const vision_msgs::msg::Detection3D & detection) { std::vector hypotheses; + hypotheses.reserve(3); // small optimization + // Guard against invalid configuration + if (time_step_ <= 0.0) { + RCLCPP_ERROR(node_->get_logger(), "Prediction time_step must be > 0"); + return hypotheses; + } + + // Current ROS time for trajectory timestamps rclcpp::Time current_time = node_->get_clock()->now(); - std::string frame_id = "map"; + std::string frame_id = "map"; // must match detection frame upstream + + // Extract pedestrian state + const double start_x = detection.bbox.center.position.x; + const double start_y = detection.bbox.center.position.y; + const double start_z = detection.bbox.center.position.z; + + // Heading is noisy, but still useful as a nominal direction + const double heading_yaw = extractYaw(detection.bbox.center.orientation); + + // Typical pedestrian walking speed (m/s) + const double nominal_walk_speed = 1.4; + + // Precompute orientation aligned with heading (planner-friendly) + geometry_msgs::msg::Quaternion aligned_orientation; + aligned_orientation.w = std::cos(heading_yaw * 0.5); + aligned_orientation.z = std::sin(heading_yaw * 0.5); + + // Hypothesis 1: Confident forward walking + TrajectoryHypothesis confident_walk; + confident_walk.header.stamp = current_time; + confident_walk.header.frame_id = frame_id; + confident_walk.intent = Intent::CONTINUE_STRAIGHT; + confident_walk.probability = 0.55; + + for (double time_sec = time_step_; + time_sec <= prediction_horizon_; + time_sec += time_step_) { + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame_id; + pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec); + + // Constant-velocity forward propagation + pose.pose.position.x = + start_x + nominal_walk_speed * std::cos(heading_yaw) * time_sec; + pose.pose.position.y = + start_y + nominal_walk_speed * std::sin(heading_yaw) * time_sec; + pose.pose.position.z = start_z; + + // Align orientation with direction of travel + pose.pose.orientation = aligned_orientation; + + confident_walk.poses.push_back(pose); + } - TrajectoryHypothesis walk_hyp; - walk_hyp.header.stamp = current_time; - walk_hyp.header.frame_id = frame_id; - walk_hyp.intent = Intent::CONTINUE_STRAIGHT; - walk_hyp.probability = 0.0; + hypotheses.push_back(confident_walk); - double current_x = detection.bbox.center.position.x; - double current_y = detection.bbox.center.position.y; - double current_z = detection.bbox.center.position.z; - double heading = extractYaw(detection.bbox.center.orientation); + // Hypothesis 2: Hesitant / wandering behavior + // Models uncertainty without lateral acceleration + TrajectoryHypothesis hesitant_walk; + hesitant_walk.header.stamp = current_time; + hesitant_walk.header.frame_id = frame_id; + hesitant_walk.intent = Intent::UNKNOWN; + hesitant_walk.probability = 0.30; - // Estimate speed from bounding box size heuristic - double length = detection.bbox.size.x; - double velocity = (length > 3.5) ? 5.0 : 1.4; + double accumulated_forward_distance = 0.0; - for (double t = time_step_; t <= prediction_horizon_; t += time_step_) { - geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header.frame_id = frame_id; - pose_stamped.header.stamp = current_time + rclcpp::Duration::from_seconds(t); - pose_stamped.pose.position.x = current_x + velocity * std::cos(heading) * t; - pose_stamped.pose.position.y = current_y + velocity * std::sin(heading) * t; - pose_stamped.pose.position.z = current_z; - pose_stamped.pose.orientation = detection.bbox.center.orientation; + for (double time_sec = time_step_; + time_sec <= prediction_horizon_; + time_sec += time_step_) { + + // Slower forward progress due to hesitation + accumulated_forward_distance += + 0.6 * nominal_walk_speed * time_step_; + + // Lateral uncertainty grows with time (non-cumulative) + double lateral_offset = 0.15 * std::sqrt(time_sec); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame_id; + pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec); + + // Base forward motion + pose.pose.position.x = + start_x + accumulated_forward_distance * std::cos(heading_yaw); + pose.pose.position.y = + start_y + accumulated_forward_distance * std::sin(heading_yaw); - walk_hyp.poses.push_back(pose_stamped); + // Apply lateral deviation relative to forward path + pose.pose.position.x += + lateral_offset * std::cos(heading_yaw + M_PI / 2.0); + pose.pose.position.y += + lateral_offset * std::sin(heading_yaw + M_PI / 2.0); + + pose.pose.position.z = start_z; + pose.pose.orientation = aligned_orientation; + + hesitant_walk.poses.push_back(pose); } - hypotheses.push_back(walk_hyp); + hypotheses.push_back(hesitant_walk); + + // Hypothesis 3: Stop / yield + TrajectoryHypothesis stop_behavior; + stop_behavior.header.stamp = current_time; + stop_behavior.header.frame_id = frame_id; + stop_behavior.intent = Intent::STOP; + stop_behavior.probability = 0.15; + + for (double time_sec = time_step_; + time_sec <= prediction_horizon_; + time_sec += time_step_) { + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = frame_id; + pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec); + + // Pedestrian remains stationary + pose.pose.position.x = start_x; + pose.pose.position.y = start_y; + pose.pose.position.z = start_z; + pose.pose.orientation = aligned_orientation; + + stop_behavior.poses.push_back(pose); + } + + hypotheses.push_back(stop_behavior); + + // Normalize probabilities + double probability_sum = 0.0; + for (const auto & hypothesis : hypotheses) { + probability_sum += hypothesis.probability; + } + for (auto & hypothesis : hypotheses) { + hypothesis.probability /= probability_sum; + } - RCLCPP_DEBUG_ONCE(node_->get_logger(), "Pedestrian prediction: constant velocity"); + RCLCPP_DEBUG( + node_->get_logger(), + "Generated %zu pedestrian trajectory hypotheses (bug-fixed, behavior-aware)", + hypotheses.size()); return hypotheses; }