From 4f380bddcd2258a6cd0ca30a5dcac227c1c8d62e Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Fri, 18 Apr 2025 11:16:50 -0700 Subject: [PATCH 01/13] added social cost function to newest dgap gap_prop version --- include/dynamic_gap/config/DynamicGapConfig.h | 2 + .../TrajectoryEvaluator.h | 16 +- include/dynamic_gap/utils/UngapPoint.h | 7 +- src/Planner.cpp | 17 +- .../TrajectoryEvaluator.cpp | 325 +++++++++++++++++- 5 files changed, 354 insertions(+), 13 deletions(-) diff --git a/include/dynamic_gap/config/DynamicGapConfig.h b/include/dynamic_gap/config/DynamicGapConfig.h index 4e9d63b6..e7b3acce 100644 --- a/include/dynamic_gap/config/DynamicGapConfig.h +++ b/include/dynamic_gap/config/DynamicGapConfig.h @@ -64,6 +64,8 @@ namespace dynamic_gap bool gap_feasibility_check = true; /**< Flag for enacting gap feasibility checking */ bool perfect_gap_models = false; /**< Flag for using perfect gap models */ int halt_size = 5; /**< Size of command velocity buffer */ + int social_cost_function = 0; /**< 0 - old without cost function, 1 - new */ + int social_cost_weight = .1; /**< weight for social cost during pose evaluation for traj selection */ } planning; /** diff --git a/include/dynamic_gap/trajectory_evaluation/TrajectoryEvaluator.h b/include/dynamic_gap/trajectory_evaluation/TrajectoryEvaluator.h index ac890714..a7b68c4f 100644 --- a/include/dynamic_gap/trajectory_evaluation/TrajectoryEvaluator.h +++ b/include/dynamic_gap/trajectory_evaluation/TrajectoryEvaluator.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -56,7 +57,20 @@ namespace dynamic_gap std::vector & posewiseCosts, float & terminalPoseCost, const std::vector & futureScans, - const int & scanIdx); + const int & scanIdx, + dynamic_gap::Gap* gap = nullptr); + + void evaluateUngapTrajectory(const Trajectory & traj, + std::vector & posewiseCosts, + float & terminalPoseCost, + const std::vector & futureScans, + const int & scanIdx, + dynamic_gap::Ungap* ungap = nullptr); + + float relativeVelocityCost(Eigen::Vector2f relativeVel, + Eigen::Vector2f relativeGapPos, + Eigen::Vector2f trajPos, + Eigen::Vector2f robotVel); private: /** diff --git a/include/dynamic_gap/utils/UngapPoint.h b/include/dynamic_gap/utils/UngapPoint.h index 709c07c0..8f61abc1 100644 --- a/include/dynamic_gap/utils/UngapPoint.h +++ b/include/dynamic_gap/utils/UngapPoint.h @@ -47,7 +47,12 @@ namespace dynamic_gap Estimator * getModel() const { return model_; - } + } + + int getUngapID() const + { + return ungapID_; + } private: diff --git a/src/Planner.cpp b/src/Planner.cpp index 1afc47ec..adc812ad 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -1055,7 +1055,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, currPose, currVel, false); - trajEvaluator_->evaluateTrajectory(goToGoalTraj, goToGoalPoseCosts, goToGoalTerminalPoseCost, futureScans, scanIdx); + trajEvaluator_->evaluateTrajectory(goToGoalTraj, goToGoalPoseCosts, goToGoalTerminalPoseCost, futureScans, scanIdx, gap); goToGoalCost = goToGoalTerminalPoseCost + std::accumulate(goToGoalPoseCosts.begin(), goToGoalPoseCosts.end(), float(0)) / goToGoalPoseCosts.size(); @@ -1074,7 +1074,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, currPose, currVel, false); - trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx); + trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx, gap); pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + std::accumulate(pursuitGuidancePoseCosts.begin(), pursuitGuidancePoseCosts.end(), float(0)) / pursuitGuidancePoseCosts.size(); ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " pursuitGuidancePoseCost: " << pursuitGuidancePoseCost); } else @@ -1083,7 +1083,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, pursuitGuidanceTraj = gapTrajGenerator_->generateIdlingTrajectoryV2(gap, currPose, rbtPoseInOdomFrame_); - trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx); + trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx, gap); pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + std::accumulate(pursuitGuidancePoseCosts.begin(), pursuitGuidancePoseCosts.end(), float(0)) / pursuitGuidancePoseCosts.size(); ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " pursuitGuidancePoseCost: " << pursuitGuidancePoseCost); @@ -1279,11 +1279,12 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, pursuitGuidanceTraj = ungapTrajGenerator_->processTrajectory(pursuitGuidanceTraj, rbtPoseInSensorFrame_, true); - trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, + trajEvaluator_->evaluateUngapTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, - 0); + 0, + ungaps.at(i)); pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + std::accumulate(pursuitGuidancePoseCosts.begin(), pursuitGuidancePoseCosts.end(), float(0)) / pursuitGuidancePoseCosts.size(); ROS_INFO_STREAM_NAMED("GapTrajectoryGenerator", " pursuitGuidancePoseCost: " << pursuitGuidancePoseCost); @@ -1329,7 +1330,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, std::vector idlingPoseCosts; float idlingTerminalPoseCost, idlingCost; - trajEvaluator_->evaluateTrajectory(idlingTrajectory, idlingPoseCosts, idlingTerminalPoseCost, futureScans, 0); + trajEvaluator_->evaluateTrajectory(idlingTrajectory, idlingPoseCosts, idlingTerminalPoseCost, futureScans, 0, 0); idlingCost = idlingTerminalPoseCost + std::accumulate(idlingPoseCosts.begin(), idlingPoseCosts.end(), float(0)) / idlingPoseCosts.size(); @@ -1598,7 +1599,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " evaluating incoming trajectory"); std::vector incomingPathPoseCosts; float incomingPathTerminalPoseCost; - trajEvaluator_->evaluateTrajectory(incomingTraj, incomingPathPoseCosts, incomingPathTerminalPoseCost, futureScans, 0); + trajEvaluator_->evaluateTrajectory(incomingTraj, incomingPathPoseCosts, incomingPathTerminalPoseCost, futureScans, 0, incomingGap); float incomingPathCost = incomingPathTerminalPoseCost + std::accumulate(incomingPathPoseCosts.begin(), incomingPathPoseCosts.end(), float(0)) / incomingPathPoseCosts.size(); ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " incoming trajectory received a cost of: " << incomingPathCost); @@ -1679,7 +1680,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, Trajectory reducedCurrentTraj(reducedCurrentPathRobotFrame, reducedCurrentPathTiming); std::vector reducedCurrentPathPoseCosts; float reducedCurrentPathTerminalPoseCost; - trajEvaluator_->evaluateTrajectory(reducedCurrentTraj, reducedCurrentPathPoseCosts, reducedCurrentPathTerminalPoseCost, futureScans, updatedCurrentPathPoseIdx); + trajEvaluator_->evaluateTrajectory(reducedCurrentTraj, reducedCurrentPathPoseCosts, reducedCurrentPathTerminalPoseCost, futureScans, updatedCurrentPathPoseIdx, incomingGap); float reducedCurrentPathSubCost = reducedCurrentPathTerminalPoseCost + std::accumulate(reducedCurrentPathPoseCosts.begin(), reducedCurrentPathPoseCosts.end(), float(0)) / reducedCurrentPathPoseCosts.size(); ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " reduced current trajectory received a subcost of: " << reducedCurrentPathSubCost); diff --git a/src/trajectory_evaluation/TrajectoryEvaluator.cpp b/src/trajectory_evaluation/TrajectoryEvaluator.cpp index b9506648..021cab23 100644 --- a/src/trajectory_evaluation/TrajectoryEvaluator.cpp +++ b/src/trajectory_evaluation/TrajectoryEvaluator.cpp @@ -1,4 +1,5 @@ #include +#include namespace dynamic_gap @@ -25,11 +26,84 @@ namespace dynamic_gap std::vector & posewiseCosts, float & terminalPoseCost, const std::vector & futureScans, - const int & scanIdx) + const int & scanIdx, + dynamic_gap::Gap* gap) { try { - + + Eigen::Vector2f leftGapRelVel(0, 0); + geometry_msgs::TwistStamped RbtVelMsg; + Eigen::Vector2f RbtVel(0, 0); + Eigen::Vector2f leftGapRelPos(0, 0); + Eigen::Vector2f rightGapRelVel(0, 0); + Eigen::Vector2f rightGapRelPos(0, 0); + bool leftGapPtIsDynamic = false; + bool rightGapPtIsDynamic = false; + + if(cfg_->planning.social_cost_function == 1) + { + if(gap) + { + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getRightGapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getRightGapPt()->getUngapID()); + + // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap: " << gap); + // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap left ID: " << gap->getLeftGapPt()->getUngapID()); + // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap right ID: " << gap->getRightGapPt()->getUngapID()); + + leftGapPtIsDynamic = gap->getLeftGapPt()->getUngapID()>=0; + if(leftGapPtIsDynamic) + { + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); + gap->getLeftGapPt()->getModel()->isolateGapDynamics(); + // gap->leftGapPt__model_->isolateGapDynamics(); + leftGapRelVel = gap->getLeftGapPt()->getModel()->getGapVelocity(); + RbtVelMsg = gap->getLeftGapPt()->getModel()->getRobotVel(); + RbtVel << RbtVelMsg.twist.linear.x, RbtVelMsg.twist.linear.y; + // ROS_ERROR_STREAM_NAMED("evalTraj", "leftGapRelVel: "); + // ROS_ERROR_STREAM_NAMED("evalTraj", leftGapRelVel); + leftGapRelPos = gap->getLeftGapPt()->getModel()->getState().head<2>(); //distance from robot to gap. + // ROS_ERROR_STREAM_NAMED("evalTraj", "gap->leftGapPtModel_->getState(): "); + // ROS_ERROR_STREAM_NAMED("evalTraj", leftGapRelPos); + + } + + rightGapPtIsDynamic = gap->getRightGapPt()->getUngapID()>=0; + if(rightGapPtIsDynamic) + { + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getRightGapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getRightGapPt()->getUngapID()); + gap->getRightGapPt()->getModel()->isolateGapDynamics(); + // gap->rightGapPt__model_->isolateGapDynamics(); + rightGapRelVel = gap->getRightGapPt()->getModel()->getGapVelocity(); + + // ROS_ERROR_STREAM_NAMED("evalTraj", "rightGapRelVel: "); + // ROS_ERROR_STREAM_NAMED("evalTraj", rightGapRelVel); + rightGapRelPos = gap->getRightGapPt()->getModel()->getState().head<2>(); //distance from robot to gap. + // ROS_ERROR_STREAM_NAMED("evalTraj", "gap->rightGapPtModel_->getState(): "); + // ROS_ERROR_STREAM_NAMED("evalTraj", rightGapRelPos); + + } + + + + // int i = 0; //ATODO DELETE + // for(i = 0; i < 1; i++) + // { + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getRightGapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getRightGapPt()->getUngapID()); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); + // INFO("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); + + // } + } + } ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " [evaluateTrajectory()]"); // Requires LOCAL FRAME @@ -37,6 +111,10 @@ namespace dynamic_gap std::vector pathTiming = traj.getPathTiming(); posewiseCosts = std::vector(path.poses.size()); + + float leftGapPtCost = 0; + float rightGapPtCost = 0; + float weight = cfg_->planning.social_cost_weight; if (path.poses.size() > futureScans.size()) { @@ -53,10 +131,43 @@ namespace dynamic_gap for (int i = 0; i < posewiseCosts.size(); i++) { + if (leftGapPtIsDynamic){ // if(leftGapPtIsDynamic){ + geometry_msgs::Point posUncoverted = path.poses.at(i).position; + Eigen::Vector2f Pos; + Pos.x() = posUncoverted.x; + Pos.y() = posUncoverted.y; + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "leftGapRelPos (which is distance of robot to gap): "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftGapRelPos); + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "Pos: "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", Pos); + + leftGapPtCost = relativeVelocityCost(leftGapRelVel, leftGapRelPos, Pos, RbtVel); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(leftGapRelVel, leftGapRelPos, RbtVel)"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftGapPtCost); + } + + + if(rightGapPtIsDynamic){ //(rightGapPtIsDynamic){ + geometry_msgs::Point posUncoverted = path.poses.at(i).position; + Eigen::Vector2f Pos; + Pos.x() = posUncoverted.x; + Pos.y() = posUncoverted.y; + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "rightGapRelPos: "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", rightGapRelPos); + + rightGapPtCost = relativeVelocityCost(rightGapRelVel, rightGapRelPos, Pos, RbtVel); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(rightGapRelVel, rightGapRelPos, RbtVel)"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", rightGapPtCost); + } + // std::cout << "regular range at " << i << ": "; - posewiseCosts.at(i) = evaluatePose(path.poses.at(i), futureScans.at(i + scanIdx)); // / posewiseCosts.size() + posewiseCosts.at(i) = evaluatePose(path.poses.at(i), futureScans.at(i + scanIdx)) + weight * leftGapPtCost + weight * rightGapPtCost; // / posewiseCosts.size() ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " pose " << i << " score: " << posewiseCosts.at(i)); } + float totalTrajCost = std::accumulate(posewiseCosts.begin(), posewiseCosts.end(), float(0)) / posewiseCosts.size(); ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " avg pose-wise cost: " << totalTrajCost); @@ -74,6 +185,170 @@ namespace dynamic_gap return; } + + void TrajectoryEvaluator::evaluateUngapTrajectory(const Trajectory & traj, + std::vector & posewiseCosts, + float & terminalPoseCost, + const std::vector & futureScans, + const int & scanIdx, + dynamic_gap::Ungap* ungap) + { + try + { + + Eigen::Vector2f leftUngapRelVel(0, 0); + geometry_msgs::TwistStamped RbtVelMsg; + Eigen::Vector2f RbtVel(0, 0); + Eigen::Vector2f leftUngapRelPos(0, 0); + Eigen::Vector2f rightUngapRelVel(0, 0); + Eigen::Vector2f rightUngapRelPos(0, 0); + bool leftUngapPtIsDynamic = false; + bool rightUngapPtIsDynamic = false; + + if(cfg_->planning.social_cost_function == 1) + { + if(ungap) + { + ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getLeftUngapPt()->getUngapID()"); + ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getLeftUngapPt()->getUngapID()); + + ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getRightUngapPt()->getUngapID()"); + ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getRightUngapPt()->getUngapID()); + + // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap: " << ungap); + // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap left ID: " << ungap->getLeftUngapPt()->getUngapID()); + // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap right ID: " << ungap->getRightUngapPt()->getUngapID()); + + leftUngapPtIsDynamic = ungap->getLeftUngapPt()->getUngapID()>=0; + if(leftUngapPtIsDynamic) + { + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getLeftUngapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getLeftUngapPt()->getUngapID()); + ungap->getLeftUngapPt()->getModel()->isolateGapDynamics(); + // ungap->leftUngapPt__model_->isolateUngapDynamics(); + leftUngapRelVel = ungap->getLeftUngapPt()->getModel()->getGapVelocity(); + RbtVelMsg = ungap->getLeftUngapPt()->getModel()->getRobotVel(); + RbtVel << RbtVelMsg.twist.linear.x, RbtVelMsg.twist.linear.y; + // ROS_ERROR_STREAM_NAMED("evalTraj", "leftUngapRelVel: "); + // ROS_ERROR_STREAM_NAMED("evalTraj", leftUngapRelVel); + leftUngapRelPos = ungap->getLeftUngapPt()->getModel()->getState().head<2>(); //distance from robot to ungap. + // ROS_ERROR_STREAM_NAMED("evalTraj", "ungap->leftUngapPtModel_->getState(): "); + // ROS_ERROR_STREAM_NAMED("evalTraj", leftUngapRelPos); + + } + + rightUngapPtIsDynamic = ungap->getRightUngapPt()->getUngapID()>=0; + if(rightUngapPtIsDynamic) + { + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getRightUngapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getRightUngapPt()->getUngapID()); + ungap->getRightUngapPt()->getModel()->isolateGapDynamics(); + // ungap->rightUngapPt__model_->isolateUngapDynamics(); + rightUngapRelVel = ungap->getRightUngapPt()->getModel()->getGapVelocity(); + + // ROS_ERROR_STREAM_NAMED("evalTraj", "rightUngapRelVel: "); + // ROS_ERROR_STREAM_NAMED("evalTraj", rightUngapRelVel); + rightUngapRelPos = ungap->getRightUngapPt()->getModel()->getState().head<2>(); //distance from robot to ungap. + // ROS_ERROR_STREAM_NAMED("evalTraj", "ungap->rightUngapPtModel_->getState(): "); + // ROS_ERROR_STREAM_NAMED("evalTraj", rightUngapRelPos); + + } + + + + // int i = 0; //ATODO DELETE + // for(i = 0; i < 1; i++) + // { + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getRightUngapPt()->getUngapID()"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getRightUngapPt()->getUngapID()); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getLeftUngapPt()->getUngapID()"); + // INFO("TrajectoryEvaluator", ungap->getLeftUngapPt()->getUngapID()); + + // } + } + } + ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " [evaluateTrajectory()]"); + // Requires LOCAL FRAME + + geometry_msgs::PoseArray path = traj.getPathRbtFrame(); + std::vector pathTiming = traj.getPathTiming(); + + posewiseCosts = std::vector(path.poses.size()); + + float leftUngapPtCost = 0; + float rightUngapPtCost = 0; + float weight = cfg_->planning.social_cost_weight; + + if (path.poses.size() > futureScans.size()) + { + ROS_WARN_STREAM_NAMED("TrajectoryEvaluator", " posewiseCosts-futureScans size mismatch: " << posewiseCosts.size() << " vs " << futureScans.size()); + return; + } + + if (posewiseCosts.size() != path.poses.size()) + { + ROS_WARN_STREAM_NAMED("TrajectoryEvaluator", " posewiseCosts-pathPoses size mismatch: " << posewiseCosts.size() << " vs " << path.poses.size()); + return; + } + + + for (int i = 0; i < posewiseCosts.size(); i++) + { + if (leftUngapPtIsDynamic){ // if(leftUngapPtIsDynamic){ + geometry_msgs::Point posUncoverted = path.poses.at(i).position; + Eigen::Vector2f Pos; + Pos.x() = posUncoverted.x; + Pos.y() = posUncoverted.y; + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "leftUngapRelPos (which is distance of robot to ungap): "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftUngapRelPos); + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "Pos: "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", Pos); + + leftUngapPtCost = relativeVelocityCost(leftUngapRelVel, leftUngapRelPos, Pos, RbtVel); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(leftUngapRelVel, leftUngapRelPos, RbtVel)"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftUngapPtCost); + } + + + if(rightUngapPtIsDynamic){ //(rightUngapPtIsDynamic){ + geometry_msgs::Point posUncoverted = path.poses.at(i).position; + Eigen::Vector2f Pos; + Pos.x() = posUncoverted.x; + Pos.y() = posUncoverted.y; + + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "rightUngapRelPos: "); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", rightUngapRelPos); + + rightUngapPtCost = relativeVelocityCost(rightUngapRelVel, rightUngapRelPos, Pos, RbtVel); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(rightUngapRelVel, rightUngapRelPos, RbtVel)"); + // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", rightUngapPtCost); + } + + // std::cout << "regular range at " << i << ": "; + posewiseCosts.at(i) = evaluatePose(path.poses.at(i), futureScans.at(i + scanIdx)) + weight * leftUngapPtCost + weight * rightUngapPtCost; // / posewiseCosts.size() + ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " pose " << i << " score: " << posewiseCosts.at(i)); + } + + float totalTrajCost = std::accumulate(posewiseCosts.begin(), posewiseCosts.end(), float(0)) / posewiseCosts.size(); + ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " avg pose-wise cost: " << totalTrajCost); + + // obtain terminalGoalCost, scale by Q + terminalPoseCost = cfg_->traj.Q_f * terminalGoalCost(path.poses.back()); + + ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " terminal cost: " << terminalPoseCost); + + // ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", "evaluateTrajectory time taken:" << ros::WallTime::now().toSec() - start_time); + } catch (const std::out_of_range& e) + { + ROS_WARN_STREAM_NAMED("TrajectoryEvaluator", " evaluateTrajectory out of range exception: " << e.what()); + } + + return; + } + float TrajectoryEvaluator::terminalGoalCost(const geometry_msgs::Pose & pose) { boost::mutex::scoped_lock planlock(globalPlanMutex_); @@ -148,4 +423,48 @@ namespace dynamic_gap return cfg_->traj.Q * std::exp(-cfg_->traj.pen_exp_weight * inflRbtToScanDist); } + + + float TrajectoryEvaluator::relativeVelocityCost(Eigen::Vector2f relativeVel, + Eigen::Vector2f relativeGapPos, + Eigen::Vector2f trajPos, + Eigen::Vector2f robotVel){ +Eigen::Vector2f relVel = -relativeVel; // so velocity and position vector point in the same direction for the dot product + +// ROS_ERROR_STREAM_NAMED("relvel cost: ", relVel << ", relativeGapPos: " << relativeGapPos << ", robotVel: " << robotVel); + +// ROS_ERROR_STREAM_NAMED("relvel cost", "relativeGapPos: "); +// ROS_ERROR_STREAM_NAMED("relvel cost", relativeGapPos); + +Eigen::Vector2f posToGapPtDist = relativeGapPos - trajPos;// distance between the current pos we're looking at and the gap point (which represents the dynamic obstacle) +// ROS_ERROR_STREAM_NAMED("relvel cost", "posToGapPtDist: "); +// ROS_ERROR_STREAM_NAMED("relvel cost", posToGapPtDist); +float cost = (std::max(relVel.dot(relativeGapPos), 0.0f) + robotVel.norm()) / posToGapPtDist.norm(); + + + + + + + + + + + + + + + +// ROS_ERROR_STREAM_NAMED("relvel cost", "std::max(relVel.dot(relativeGapPos), 0.0f"); +// ROS_ERROR_STREAM_NAMED("relvel cost", std::max(relVel.dot(relativeGapPos), 0.0f)); + +// ROS_ERROR_STREAM_NAMED("relvel cost", "relVel.dot(relativeGapPos)"); +// ROS_ERROR_STREAM_NAMED("relvel cost", relVel.dot(relativeGapPos)); + +// ROS_ERROR_STREAM_NAMED("GapTrajectoryGenerator", "relativeVelocityCost() !!UNWEIGHTED!! cost: "); +// ROS_ERROR_STREAM_NAMED("GapTrajectoryGenerator", cost); + +return cost; +} + } \ No newline at end of file From a107ffd2d39265d5f194eeeecfced96aeb0d2391 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Mon, 12 May 2025 06:43:56 -0700 Subject: [PATCH 02/13] previous had social_cost_function flag off --- include/dynamic_gap/config/DynamicGapConfig.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dynamic_gap/config/DynamicGapConfig.h b/include/dynamic_gap/config/DynamicGapConfig.h index e7b3acce..369b1a6c 100644 --- a/include/dynamic_gap/config/DynamicGapConfig.h +++ b/include/dynamic_gap/config/DynamicGapConfig.h @@ -64,7 +64,7 @@ namespace dynamic_gap bool gap_feasibility_check = true; /**< Flag for enacting gap feasibility checking */ bool perfect_gap_models = false; /**< Flag for using perfect gap models */ int halt_size = 5; /**< Size of command velocity buffer */ - int social_cost_function = 0; /**< 0 - old without cost function, 1 - new */ + int social_cost_function = 1; /**< 0 - old without cost function, 1 - new */ int social_cost_weight = .1; /**< weight for social cost during pose evaluation for traj selection */ } planning; From ab003fa88bb13945e5ba67f8debe0dcaf987dc63 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 14 May 2025 07:36:02 -0700 Subject: [PATCH 03/13] removed some comments --- .../TrajectoryEvaluator.cpp | 91 +------------------ 1 file changed, 5 insertions(+), 86 deletions(-) diff --git a/src/trajectory_evaluation/TrajectoryEvaluator.cpp b/src/trajectory_evaluation/TrajectoryEvaluator.cpp index 021cab23..05501996 100644 --- a/src/trajectory_evaluation/TrajectoryEvaluator.cpp +++ b/src/trajectory_evaluation/TrajectoryEvaluator.cpp @@ -45,21 +45,11 @@ namespace dynamic_gap { if(gap) { - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getRightGapPt()->getUngapID()"); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getRightGapPt()->getUngapID()); - - // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap: " << gap); - // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap left ID: " << gap->getLeftGapPt()->getUngapID()); - // ROS_ERROR_STREAM("inside trajectoryEvaluator() gap right ID: " << gap->getRightGapPt()->getUngapID()); leftGapPtIsDynamic = gap->getLeftGapPt()->getUngapID()>=0; if(leftGapPtIsDynamic) { - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); + gap->getLeftGapPt()->getModel()->isolateGapDynamics(); // gap->leftGapPt__model_->isolateGapDynamics(); leftGapRelVel = gap->getLeftGapPt()->getModel()->getGapVelocity(); @@ -92,16 +82,6 @@ namespace dynamic_gap - // int i = 0; //ATODO DELETE - // for(i = 0; i < 1; i++) - // { - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getRightGapPt()->getUngapID()"); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", gap->getRightGapPt()->getUngapID()); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "gap->getLeftGapPt()->getUngapID()"); - // INFO("TrajectoryEvaluator", gap->getLeftGapPt()->getUngapID()); - - // } } } ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " [evaluateTrajectory()]"); @@ -136,13 +116,6 @@ namespace dynamic_gap Eigen::Vector2f Pos; Pos.x() = posUncoverted.x; Pos.y() = posUncoverted.y; - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "leftGapRelPos (which is distance of robot to gap): "); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftGapRelPos); - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "Pos: "); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", Pos); - leftGapPtCost = relativeVelocityCost(leftGapRelVel, leftGapRelPos, Pos, RbtVel); // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(leftGapRelVel, leftGapRelPos, RbtVel)"); // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftGapPtCost); @@ -154,7 +127,6 @@ namespace dynamic_gap Eigen::Vector2f Pos; Pos.x() = posUncoverted.x; Pos.y() = posUncoverted.y; - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "rightGapRelPos: "); // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", rightGapRelPos); @@ -215,9 +187,6 @@ namespace dynamic_gap ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getRightUngapPt()->getUngapID()"); ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getRightUngapPt()->getUngapID()); - // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap: " << ungap); - // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap left ID: " << ungap->getLeftUngapPt()->getUngapID()); - // ROS_ERROR_STREAM("inside trajectoryEvaluator() ungap right ID: " << ungap->getRightUngapPt()->getUngapID()); leftUngapPtIsDynamic = ungap->getLeftUngapPt()->getUngapID()>=0; if(leftUngapPtIsDynamic) @@ -254,18 +223,6 @@ namespace dynamic_gap } - - - // int i = 0; //ATODO DELETE - // for(i = 0; i < 1; i++) - // { - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getRightUngapPt()->getUngapID()"); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", ungap->getRightUngapPt()->getUngapID()); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "ungap->getLeftUngapPt()->getUngapID()"); - // INFO("TrajectoryEvaluator", ungap->getLeftUngapPt()->getUngapID()); - - // } } } ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " [evaluateTrajectory()]"); @@ -300,13 +257,6 @@ namespace dynamic_gap Eigen::Vector2f Pos; Pos.x() = posUncoverted.x; Pos.y() = posUncoverted.y; - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "leftUngapRelPos (which is distance of robot to ungap): "); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftUngapRelPos); - - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "Pos: "); - // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", Pos); - leftUngapPtCost = relativeVelocityCost(leftUngapRelVel, leftUngapRelPos, Pos, RbtVel); // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", "relativeVelocityCost(leftUngapRelVel, leftUngapRelPos, RbtVel)"); // ROS_ERROR_STREAM_NAMED("TrajectoryEvaluator", leftUngapPtCost); @@ -429,42 +379,11 @@ namespace dynamic_gap Eigen::Vector2f relativeGapPos, Eigen::Vector2f trajPos, Eigen::Vector2f robotVel){ -Eigen::Vector2f relVel = -relativeVel; // so velocity and position vector point in the same direction for the dot product - -// ROS_ERROR_STREAM_NAMED("relvel cost: ", relVel << ", relativeGapPos: " << relativeGapPos << ", robotVel: " << robotVel); - -// ROS_ERROR_STREAM_NAMED("relvel cost", "relativeGapPos: "); -// ROS_ERROR_STREAM_NAMED("relvel cost", relativeGapPos); - -Eigen::Vector2f posToGapPtDist = relativeGapPos - trajPos;// distance between the current pos we're looking at and the gap point (which represents the dynamic obstacle) -// ROS_ERROR_STREAM_NAMED("relvel cost", "posToGapPtDist: "); -// ROS_ERROR_STREAM_NAMED("relvel cost", posToGapPtDist); -float cost = (std::max(relVel.dot(relativeGapPos), 0.0f) + robotVel.norm()) / posToGapPtDist.norm(); - + Eigen::Vector2f relVel = -relativeVel; // so velocity and position vector point in the same direction for the dot product + Eigen::Vector2f posToGapPtDist = relativeGapPos - trajPos;// distance between the current pos we're looking at and the gap point (which represents the dynamic obstacle) + float cost = (std::max(relVel.dot(relativeGapPos), 0.0f) + robotVel.norm()) / posToGapPtDist.norm(); - - - - - - - - - - - - - -// ROS_ERROR_STREAM_NAMED("relvel cost", "std::max(relVel.dot(relativeGapPos), 0.0f"); -// ROS_ERROR_STREAM_NAMED("relvel cost", std::max(relVel.dot(relativeGapPos), 0.0f)); - -// ROS_ERROR_STREAM_NAMED("relvel cost", "relVel.dot(relativeGapPos)"); -// ROS_ERROR_STREAM_NAMED("relvel cost", relVel.dot(relativeGapPos)); - -// ROS_ERROR_STREAM_NAMED("GapTrajectoryGenerator", "relativeVelocityCost() !!UNWEIGHTED!! cost: "); -// ROS_ERROR_STREAM_NAMED("GapTrajectoryGenerator", cost); - -return cost; + return cost; } } \ No newline at end of file From 02b5da8837e651e6e032febefbf033497a34b871 Mon Sep 17 00:00:00 2001 From: Max Asselmeier Date: Wed, 14 May 2025 17:43:30 -0400 Subject: [PATCH 04/13] reset vis for benchmarking --- rviz/arena.rviz | 78 ++++++++++++++++++++++++------------------------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/rviz/arena.rviz b/rviz/arena.rviz index ad88d58f..54c5ad32 100644 --- a/rviz/arena.rviz +++ b/rviz/arena.rviz @@ -170,7 +170,7 @@ Visualization Manager: Color map offset: 0 Color transform: Constant color Delete after no. cycles: 100 - Enabled: false + Enabled: true Excluded person IDs: "" Font color: 255; 255; 255 Font color style: Same color @@ -204,7 +204,7 @@ Visualization Manager: Value: Bounding boxes Topic: /pedsim_visualizer/tracked_persons Unreliable: false - Value: false + Value: true Z offset: Use Z position from message: false Value: 0 @@ -332,13 +332,13 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Marker - Enabled: true + Enabled: false Marker Topic: /rto/raw_gaps Name: Marker Namespaces: - raw: true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/Marker Enabled: false Marker Topic: /rto/simp_gaps @@ -348,21 +348,21 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/Marker - Enabled: false + Enabled: true Marker Topic: /rto/manip_gaps Name: Marker Namespaces: {} Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /rto/raw_gap_model_positions Name: MarkerArray Namespaces: - raw: true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /rto/raw_gap_model_velocities @@ -388,21 +388,21 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/manip_gap_model_positions Name: MarkerArray Namespaces: - {} + manip: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/manip_gap_model_velocities Name: MarkerArray Namespaces: - {} + manip: true Queue Size: 100 - Value: false + Value: true - Class: rviz/Marker Enabled: false Marker Topic: /rto/global_path_local_waypoint @@ -412,13 +412,13 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/gap_goal_positions Name: MarkerArray Namespaces: - {} + gap_goal: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray Enabled: false Marker Topic: /rto/gap_goal_velocities @@ -428,13 +428,13 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/gap_tubes Name: MarkerArray Namespaces: - {} + manip_initial: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray Enabled: false Marker Topic: /rto/propagated_gap_points @@ -480,21 +480,21 @@ Visualization Manager: Queue Size: 100 Value: false - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/candidate_ungap_trajectories Name: MarkerArray Namespaces: - {} + allTraj: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray - Enabled: false + Enabled: true Marker Topic: /rto/candidate_tube_trajectories Name: MarkerArray Namespaces: - {} + allTraj: true Queue Size: 100 - Value: false + Value: true Enabled: true Name: Trajectories - Class: rviz/Group @@ -527,21 +527,21 @@ Visualization Manager: Enabled: true Name: Tracking - Class: rviz/Marker - Enabled: false + Enabled: true Marker Topic: /rto/planning_loop_idx Name: Marker Namespaces: - {} + planning_loop_idx: true Queue Size: 100 - Value: false + Value: true - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /rto/pedestrian_histories Name: MarkerArray Namespaces: {} Queue Size: 100 - Value: true + Value: false Enabled: true Name: Dynamic Gap - Alpha: 1 @@ -811,18 +811,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 88.0296630859375 + Scale: 77.46610260009766 Target Frame: rto/base_link - X: -0.21646548807621002 - Y: -0.13326548039913177 + X: 1.2442842721939087 + Y: 0.2725197970867157 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false Height: 1016 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000022d0000039efc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000022d0000039efc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000005050000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: From dc844f499dff66f2291044e16ae76cf644c823b7 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 24 Sep 2025 05:50:10 -0400 Subject: [PATCH 05/13] test --- .../dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h index 2ee8a8c5..3c3cc089 100644 --- a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h +++ b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h @@ -18,7 +18,7 @@ #include #include #include - +//testing namespace dynamic_gap { From 32880bb204338e0eb50eca61858701d10ce1dacc Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 24 Sep 2025 05:51:02 -0400 Subject: [PATCH 06/13] first --- .../dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h index 3c3cc089..2ee8a8c5 100644 --- a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h +++ b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h @@ -18,7 +18,7 @@ #include #include #include -//testing + namespace dynamic_gap { From 221974038812e2b6356fdacae93eea2c1bd974f1 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 24 Sep 2025 12:11:13 -0400 Subject: [PATCH 07/13] added multi gap but still naven't added the prune for the last gap for the multi robot case --- .../GapTrajectoryGenerator.h | 7 ++ src/Planner.cpp | 79 ++++++++++++++----- .../GapTrajectoryGenerator.cpp | 72 +++++++++++++++++ 3 files changed, 139 insertions(+), 19 deletions(-) diff --git a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h index 2ee8a8c5..e59d4095 100644 --- a/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h +++ b/include/dynamic_gap/trajectory_generation/GapTrajectoryGenerator.h @@ -44,10 +44,17 @@ namespace dynamic_gap * \param runGoToGoal boolean for if go to goal trajectory method should be run * \return trajectory through gap */ + Trajectory generateTrajectoryV2(Gap * selectedGap, const geometry_msgs::PoseStamped & currPose, // const geometry_msgs::TwistStamped & currVel, const geometry_msgs::PoseStamped & globalGoalRobotFrame); + + + std::vector generateMultiTrajectoryV2(Gap * selectedGap, + const geometry_msgs::PoseStamped & currPose, + // const geometry_msgs::TwistStamped & currVel, + const geometry_msgs::PoseStamped & globalGoalRobotFrame); // /** // * \brief generate local collision-free trajectory through gap diff --git a/src/Planner.cpp b/src/Planner.cpp index 4c853500..10336aa8 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -1018,27 +1018,68 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, // Run pursuit guidance behavior if (gap->isAvailable()) { - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (available)"); - pursuitGuidanceTraj = gapTrajGenerator_->generateTrajectoryV2(gap, - currPose, - // currVel, - globalGoalRobotFrame_); - pursuitGuidanceTraj = gapTrajGenerator_->processTrajectory(pursuitGuidanceTraj); - // currPose, - // currVel, - // false); - - if (j == (gapTube->size() - 1)) + ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (available)"); + + bool generate_multi_traj = true; // TODO: config flag + if (generate_multi_traj) { - // prune trajectory - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " pruning pursuit guidance (available) traj"); - pursuitGuidanceTraj = gapTrajGenerator_->pruneTrajectory(pursuitGuidanceTraj); + std::vector candTrajs = + gapTrajGenerator_->generateMultiTrajectoryV2(gap, currPose, globalGoalRobotFrame_); + + float bestCost = std::numeric_limits::infinity(); + Trajectory bestTraj; + std::vector bestPoseCosts; + float bestTerminalCost = 0.0f; + + for (auto &cand : candTrajs) + { + // Evaluate + std::vector poseCostsTmp; + float terminalCostTmp; + trajEvaluator_->evaluateTrajectory(cand, poseCostsTmp, terminalCostTmp, futureScans, scanIdx); + + float avgCost = std::accumulate(poseCostsTmp.begin(), poseCostsTmp.end(), 0.0f) / poseCostsTmp.size(); + float totalCost = terminalCostTmp + avgCost; + + ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", + " candidate traj cost = " << totalCost); + + if (totalCost < bestCost) + { + bestCost = totalCost; + bestTraj = cand; + bestPoseCosts = poseCostsTmp; + bestTerminalCost = terminalCostTmp; + } + } + + pursuitGuidanceTraj = bestTraj; + pursuitGuidancePoseCosts = bestPoseCosts; + pursuitGuidanceTerminalPoseCost = bestTerminalCost; + pursuitGuidancePoseCost = bestCost; } - - trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx); - pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + std::accumulate(pursuitGuidancePoseCosts.begin(), pursuitGuidancePoseCosts.end(), float(0)) / pursuitGuidancePoseCosts.size(); - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " pursuitGuidancePoseCost: " << pursuitGuidancePoseCost); - } else + else + { + pursuitGuidanceTraj = gapTrajGenerator_->generateTrajectoryV2(gap, currPose, globalGoalRobotFrame_); + pursuitGuidanceTraj = gapTrajGenerator_->processTrajectory(pursuitGuidanceTraj); + + if (j == (gapTube->size() - 1)) + { + pursuitGuidanceTraj = gapTrajGenerator_->pruneTrajectory(pursuitGuidanceTraj); + } + + trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, + pursuitGuidanceTerminalPoseCost, futureScans, scanIdx); + pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + + std::accumulate(pursuitGuidancePoseCosts.begin(), + pursuitGuidancePoseCosts.end(), 0.0f) / + pursuitGuidancePoseCosts.size(); + } + } + + + + else { ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (not available)"); pursuitGuidanceTraj = gapTrajGenerator_->generateIdlingTrajectoryV2(gap, diff --git a/src/trajectory_generation/GapTrajectoryGenerator.cpp b/src/trajectory_generation/GapTrajectoryGenerator.cpp index d8711d94..18c4cb6a 100644 --- a/src/trajectory_generation/GapTrajectoryGenerator.cpp +++ b/src/trajectory_generation/GapTrajectoryGenerator.cpp @@ -174,6 +174,78 @@ namespace dynamic_gap return traj; } + std::vector GapTrajectoryGenerator::generateMultiTrajectoryV2( + Gap * selectedGap, + const geometry_msgs::PoseStamped & currPose, + const geometry_msgs::PoseStamped & globalGoalRobotFrame) +{ + std::vector allTrajs; // store multiple + + // set up initial state (same as before) + geometry_msgs::PoseArray path; + std::vector pathTiming; + path.header.stamp = currPose.header.stamp; + path.header.frame_id = cfg_->sensor_frame_id; + + Eigen::Vector4f rbtState(currPose.pose.position.x, + currPose.pose.position.y, + 0.0, 0.0); + + float xLeft = 0.0, yLeft = 0.0, xRight = 0.0, yRight = 0.0; + selectedGap->getManipulatedLCartesian(xLeft, yLeft); + selectedGap->getManipulatedRCartesian(xRight, yRight); + + Eigen::Vector2f initialGoal = selectedGap->getGoal()->getOrigGoalPos(); + Eigen::Vector2f goalPtVel = selectedGap->getGoal()->getOrigGoalVel(); + Eigen::Vector2f leftGapPtVel = selectedGap->getManipulatedLVelocity(); + Eigen::Vector2f rightGapPtVel = selectedGap->getManipulatedRVelocity(); + Eigen::Vector2f terminalGoal = selectedGap->getGoal()->getTermGoalPos(); + + // ---- MULTIPLE GAMMAS ---- + float baseGamma = selectedGap->getGammaInterceptGoal(); + std::vector gammaCandidates = { + baseGamma - 0.2f, + baseGamma, + baseGamma + 0.2f + }; + + for (float gamma : gammaCandidates) + { + geometry_msgs::PoseArray path_i; + std::vector pathTiming_i; + + robotAndGapState x = {rbtState[0], rbtState[1], + xLeft, yLeft, xRight, yRight, + initialGoal[0], initialGoal[1]}; + + Eigen::Quaternionf desiredQ = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) * + Eigen::AngleAxisf(gamma, Eigen::Vector3f::UnitZ()); + + TrajectoryLogger logger(path_i, pathTiming_i, cfg_->robot_frame_id, desiredQ); + + ParallelNavigation pn(gamma, + cfg_->rbt.vx_absmax, + cfg_->rbt.r_inscr, + leftGapPtVel, + rightGapPtVel, + goalPtVel, + terminalGoal); + + float t_max = std::min(selectedGap->getGapLifespan(), cfg_->traj.integrate_maxt); + + boost::numeric::odeint::integrate_const( + boost::numeric::odeint::euler(), + pn, x, 0.0f, t_max, cfg_->traj.integrate_stept, logger); + + // store trajectory + allTrajs.emplace_back(path_i, pathTiming_i); + } + + return allTrajs; +} + + Trajectory GapTrajectoryGenerator::generateIdlingTrajectoryV2(Gap * gap, const geometry_msgs::PoseStamped & rbtPoseInSensorFrame, const Trajectory & runningTraj) From 3e82547d53aeab912978cf56317473939897bb68 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 24 Sep 2025 18:55:31 -0400 Subject: [PATCH 08/13] for debugging onlygit add . got rid of if statement and brute forced the steps for each traj. and it works lol --- src/Planner.cpp | 99 +++++++++++++++---- .../GapTrajectoryGenerator.cpp | 4 + 2 files changed, 82 insertions(+), 21 deletions(-) diff --git a/src/Planner.cpp b/src/Planner.cpp index 10336aa8..1ce0dc8e 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -1031,27 +1031,84 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, std::vector bestPoseCosts; float bestTerminalCost = 0.0f; - for (auto &cand : candTrajs) - { - // Evaluate - std::vector poseCostsTmp; - float terminalCostTmp; - trajEvaluator_->evaluateTrajectory(cand, poseCostsTmp, terminalCostTmp, futureScans, scanIdx); - - float avgCost = std::accumulate(poseCostsTmp.begin(), poseCostsTmp.end(), 0.0f) / poseCostsTmp.size(); - float totalCost = terminalCostTmp + avgCost; - - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", - " candidate traj cost = " << totalCost); - - if (totalCost < bestCost) - { - bestCost = totalCost; - bestTraj = cand; - bestPoseCosts = poseCostsTmp; - bestTerminalCost = terminalCostTmp; - } - } + if (candTrajs.size() > 0) +{ + // === FIRST CANDIDATE TRAJECTORY === + Trajectory cand0 = candTrajs.at(0); + + std::vector poseCosts0; + float terminalCost0; + + trajEvaluator_->evaluateTrajectory(cand0, poseCosts0, terminalCost0, futureScans, scanIdx); + + float avgCost0 = poseCosts0.empty() ? 0.0f : + std::accumulate(poseCosts0.begin(), poseCosts0.end(), 0.0f) / poseCosts0.size(); + float totalCost0 = terminalCost0 + avgCost0; + + ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] first candidate traj cost = " << totalCost0); + + // Store as best by default + bestTraj = cand0; + bestPoseCosts = poseCosts0; + bestTerminalCost = terminalCost0; + bestCost = totalCost0; +} + +if (candTrajs.size() > 1) +{ + // === SECOND CANDIDATE TRAJECTORY === + Trajectory cand1 = candTrajs.at(1); + + std::vector poseCosts1; + float terminalCost1; + + trajEvaluator_->evaluateTrajectory(cand1, poseCosts1, terminalCost1, futureScans, scanIdx); + + float avgCost1 = poseCosts1.empty() ? 0.0f : + std::accumulate(poseCosts1.begin(), poseCosts1.end(), 0.0f) / poseCosts1.size(); + float totalCost1 = terminalCost1 + avgCost1; + + ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] second candidate traj cost = " << totalCost1); + + // Optionally overwrite bestTraj with second one + // For now, you can either always take the second one: + // bestTraj = cand1; + // bestPoseCosts = poseCosts1; + // bestTerminalCost = terminalCost1; + // bestCost = totalCost1; + + // Or only take it if it's cheaper: + if (totalCost1 < bestCost) { + bestTraj = cand1; + bestPoseCosts = poseCosts1; + bestTerminalCost = terminalCost1; + bestCost = totalCost1; + } +} + + // for (auto &cand : candTrajs) + // { + // // Evaluate + // std::vector poseCostsTmp; + // float terminalCostTmp; + // trajEvaluator_->evaluateTrajectory(cand, poseCostsTmp, terminalCostTmp, futureScans, scanIdx); + + // float avgCost = std::accumulate(poseCostsTmp.begin(), poseCostsTmp.end(), 0.0f) / poseCostsTmp.size(); + // float totalCost = terminalCostTmp + avgCost; + + // ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", + // " candidate traj cost = " << totalCost); + + // if (totalCost < bestCost) + // { + // bestCost = totalCost; + // bestTraj = cand; + // bestPoseCosts = poseCostsTmp; + // bestTerminalCost = terminalCostTmp; + // } + // } pursuitGuidanceTraj = bestTraj; pursuitGuidancePoseCosts = bestPoseCosts; diff --git a/src/trajectory_generation/GapTrajectoryGenerator.cpp b/src/trajectory_generation/GapTrajectoryGenerator.cpp index 18c4cb6a..eeb8386d 100644 --- a/src/trajectory_generation/GapTrajectoryGenerator.cpp +++ b/src/trajectory_generation/GapTrajectoryGenerator.cpp @@ -186,6 +186,7 @@ namespace dynamic_gap std::vector pathTiming; path.header.stamp = currPose.header.stamp; path.header.frame_id = cfg_->sensor_frame_id; + ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", "cfg_->sensor_frame_id: " + cfg_->sensor_frame_id); Eigen::Vector4f rbtState(currPose.pose.position.x, currPose.pose.position.y, @@ -214,6 +215,9 @@ namespace dynamic_gap geometry_msgs::PoseArray path_i; std::vector pathTiming_i; + path_i.header.stamp = currPose.header.stamp; + path_i.header.frame_id = cfg_->sensor_frame_id; + robotAndGapState x = {rbtState[0], rbtState[1], xLeft, yLeft, xRight, yRight, initialGoal[0], initialGoal[1]}; From b546a1986e37d14dc5f9c0ecd42e84f62bf25bc4 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Wed, 24 Sep 2025 19:52:32 -0400 Subject: [PATCH 09/13] added third brute force traj and it works --- src/Planner.cpp | 149 +++++++++++------- .../GapTrajectoryGenerator.cpp | 2 +- 2 files changed, 95 insertions(+), 56 deletions(-) diff --git a/src/Planner.cpp b/src/Planner.cpp index 1ce0dc8e..27b8d92c 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -1032,61 +1032,100 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, float bestTerminalCost = 0.0f; if (candTrajs.size() > 0) -{ - // === FIRST CANDIDATE TRAJECTORY === - Trajectory cand0 = candTrajs.at(0); - - std::vector poseCosts0; - float terminalCost0; - - trajEvaluator_->evaluateTrajectory(cand0, poseCosts0, terminalCost0, futureScans, scanIdx); - - float avgCost0 = poseCosts0.empty() ? 0.0f : - std::accumulate(poseCosts0.begin(), poseCosts0.end(), 0.0f) / poseCosts0.size(); - float totalCost0 = terminalCost0 + avgCost0; - - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", - " [TEST] first candidate traj cost = " << totalCost0); - - // Store as best by default - bestTraj = cand0; - bestPoseCosts = poseCosts0; - bestTerminalCost = terminalCost0; - bestCost = totalCost0; -} - -if (candTrajs.size() > 1) -{ - // === SECOND CANDIDATE TRAJECTORY === - Trajectory cand1 = candTrajs.at(1); - - std::vector poseCosts1; - float terminalCost1; - - trajEvaluator_->evaluateTrajectory(cand1, poseCosts1, terminalCost1, futureScans, scanIdx); - - float avgCost1 = poseCosts1.empty() ? 0.0f : - std::accumulate(poseCosts1.begin(), poseCosts1.end(), 0.0f) / poseCosts1.size(); - float totalCost1 = terminalCost1 + avgCost1; - - ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", - " [TEST] second candidate traj cost = " << totalCost1); - - // Optionally overwrite bestTraj with second one - // For now, you can either always take the second one: - // bestTraj = cand1; - // bestPoseCosts = poseCosts1; - // bestTerminalCost = terminalCost1; - // bestCost = totalCost1; - - // Or only take it if it's cheaper: - if (totalCost1 < bestCost) { - bestTraj = cand1; - bestPoseCosts = poseCosts1; - bestTerminalCost = terminalCost1; - bestCost = totalCost1; - } -} + { + // === FIRST CANDIDATE TRAJECTORY === + Trajectory cand0 = candTrajs.at(0); + + std::vector poseCosts0; + float terminalCost0; + + trajEvaluator_->evaluateTrajectory(cand0, poseCosts0, terminalCost0, futureScans, scanIdx); + + float avgCost0 = poseCosts0.empty() ? 0.0f : + std::accumulate(poseCosts0.begin(), poseCosts0.end(), 0.0f) / poseCosts0.size(); + float totalCost0 = terminalCost0 + avgCost0; + + ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] first candidate traj cost = " << totalCost0); + + // Store as best by default + bestTraj = cand0; + bestPoseCosts = poseCosts0; + bestTerminalCost = terminalCost0; + bestCost = totalCost0; + } + + if (candTrajs.size() > 1) + { + // === SECOND CANDIDATE TRAJECTORY === + Trajectory cand1 = candTrajs.at(1); + + std::vector poseCosts1; + float terminalCost1; + + trajEvaluator_->evaluateTrajectory(cand1, poseCosts1, terminalCost1, futureScans, scanIdx); + + float avgCost1 = poseCosts1.empty() ? 0.0f : + std::accumulate(poseCosts1.begin(), poseCosts1.end(), 0.0f) / poseCosts1.size(); + float totalCost1 = terminalCost1 + avgCost1; + + ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] second candidate traj cost = " << totalCost1); + + // Optionally overwrite bestTraj with second one + // For now, you can either always take the second one: + // bestTraj = cand1; + // bestPoseCosts = poseCosts1; + // bestTerminalCost = terminalCost1; + // bestCost = totalCost1; + + // Or only take it if it's cheaper: + if (totalCost1 < bestCost) { + bestTraj = cand1; + bestPoseCosts = poseCosts1; + bestTerminalCost = terminalCost1; + bestCost = totalCost1; + } + } + if (candTrajs.size() > 2) + { + // === THIRD CANDIDATE TRAJECTORY === + Trajectory cand2 = candTrajs.at(2); + + if (cand2.getPathRbtFrame().poses.size() > 1) + { + std::vector poseCosts2; + float terminalCost2; + + trajEvaluator_->evaluateTrajectory(cand2, poseCosts2, terminalCost2, futureScans, scanIdx); + + if (!poseCosts2.empty()) + { + float avgCost2 = std::accumulate(poseCosts2.begin(), poseCosts2.end(), 0.0f) / poseCosts2.size(); + float totalCost2 = terminalCost2 + avgCost2; + + ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] third candidate traj cost = " << totalCost2); + + // Optionally overwrite bestTraj with third one + // For testing, always take it: + // bestTraj = cand2; + // bestPoseCosts = poseCosts2; + // bestTerminalCost = terminalCost2; + // bestCost = totalCost2; + + // Or only if cheaper: + if (totalCost2 < bestCost) + { + bestTraj = cand2; + bestPoseCosts = poseCosts2; + bestTerminalCost = terminalCost2; + bestCost = totalCost2; + } + } + } + } + // for (auto &cand : candTrajs) // { diff --git a/src/trajectory_generation/GapTrajectoryGenerator.cpp b/src/trajectory_generation/GapTrajectoryGenerator.cpp index eeb8386d..b3636058 100644 --- a/src/trajectory_generation/GapTrajectoryGenerator.cpp +++ b/src/trajectory_generation/GapTrajectoryGenerator.cpp @@ -186,7 +186,7 @@ namespace dynamic_gap std::vector pathTiming; path.header.stamp = currPose.header.stamp; path.header.frame_id = cfg_->sensor_frame_id; - ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", "cfg_->sensor_frame_id: " + cfg_->sensor_frame_id); + // ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", "cfg_->sensor_frame_id: " + cfg_->sensor_frame_id); Eigen::Vector4f rbtState(currPose.pose.position.x, currPose.pose.position.y, From 4a289bb23b4afeb0dea4ac1ac10a29dca952fad1 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Thu, 25 Sep 2025 19:56:05 -0400 Subject: [PATCH 10/13] multi_traj set to false for testinggit add . ! I added a parallel nav visual to the code in the else statement and it works --- include/dynamic_gap/Planner.h | 2 ++ src/Planner.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/include/dynamic_gap/Planner.h b/include/dynamic_gap/Planner.h index e7695b78..4ac1433e 100644 --- a/include/dynamic_gap/Planner.h +++ b/include/dynamic_gap/Planner.h @@ -438,6 +438,8 @@ namespace dynamic_gap ros::Publisher mpcInputPublisher_; /**< ROS publisher for mpc input terms */ ros::Subscriber mpcOutputSubscriber_; /**< ROS subscriber for mpc output */ + ros::Publisher pnTrajPub_; /**< for visualizing pn traj */ + ros::Subscriber tfSub_; /**< Subscriber to TF tree */ ros::Subscriber laserSub_; /**< Subscriber to incoming laser scan */ diff --git a/src/Planner.cpp b/src/Planner.cpp index 27b8d92c..40469585 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -115,6 +115,8 @@ namespace dynamic_gap pedOdomSub_ = nh_.subscribe(cfg_.ped_topic, 10, &Planner::pedOdomCB, this); + pnTrajPub_ = nh_.advertise("pn_traj", 1); //for visualizing pn traj + // Visualization Setup // mpcInputPublisher_ = nh_.advertise("mpc_input", 1); @@ -1020,7 +1022,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, { ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (available)"); - bool generate_multi_traj = true; // TODO: config flag + bool generate_multi_traj = false; // TODO: config flag if (generate_multi_traj) { std::vector candTrajs = @@ -1170,6 +1172,12 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, std::accumulate(pursuitGuidancePoseCosts.begin(), pursuitGuidancePoseCosts.end(), 0.0f) / pursuitGuidancePoseCosts.size(); + + //visualization for debugging + geometry_msgs::PoseArray vizMsg = pursuitGuidanceTraj.getPathRbtFrame(); + vizMsg.header.stamp = ros::Time::now(); + vizMsg.header.frame_id = cfg_.robot_frame_id; + pnTrajPub_.publish(vizMsg); } } From 50341a4807a1980672accc64d6e1df56e8d7fea6 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Thu, 25 Sep 2025 20:10:14 -0400 Subject: [PATCH 11/13] added visuals to the multi traj and they works --- include/dynamic_gap/Planner.h | 4 ++++ src/Planner.cpp | 20 +++++++++++++++++++- 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/include/dynamic_gap/Planner.h b/include/dynamic_gap/Planner.h index 4ac1433e..3378d83d 100644 --- a/include/dynamic_gap/Planner.h +++ b/include/dynamic_gap/Planner.h @@ -439,6 +439,10 @@ namespace dynamic_gap ros::Subscriber mpcOutputSubscriber_; /**< ROS subscriber for mpc output */ ros::Publisher pnTrajPub_; /**< for visualizing pn traj */ + ros::Publisher pnCand0Pub_; + ros::Publisher pnCand1Pub_; + ros::Publisher pnCand2Pub_; + ros::Subscriber tfSub_; /**< Subscriber to TF tree */ ros::Subscriber laserSub_; /**< Subscriber to incoming laser scan */ diff --git a/src/Planner.cpp b/src/Planner.cpp index 40469585..bc8a098b 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -116,6 +116,10 @@ namespace dynamic_gap pedOdomSub_ = nh_.subscribe(cfg_.ped_topic, 10, &Planner::pedOdomCB, this); pnTrajPub_ = nh_.advertise("pn_traj", 1); //for visualizing pn traj + pnCand0Pub_ = nh_.advertise("pn_traj_cand0", 1); + pnCand1Pub_ = nh_.advertise("pn_traj_cand1", 1); + pnCand2Pub_ = nh_.advertise("pn_traj_cand2", 1); + // Visualization Setup @@ -1022,7 +1026,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, { ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (available)"); - bool generate_multi_traj = false; // TODO: config flag + bool generate_multi_traj = true; // TODO: config flag if (generate_multi_traj) { std::vector candTrajs = @@ -1055,6 +1059,11 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, bestPoseCosts = poseCosts0; bestTerminalCost = terminalCost0; bestCost = totalCost0; + + geometry_msgs::PoseArray msg0 = cand0.getPathRbtFrame(); + msg0.header.stamp = ros::Time::now(); + msg0.header.frame_id = cfg_.robot_frame_id; + pnCand0Pub_.publish(msg0); } if (candTrajs.size() > 1) @@ -1087,7 +1096,12 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, bestPoseCosts = poseCosts1; bestTerminalCost = terminalCost1; bestCost = totalCost1; + } + geometry_msgs::PoseArray msg1 = cand1.getPathRbtFrame(); + msg1.header.stamp = ros::Time::now(); + msg1.header.frame_id = cfg_.robot_frame_id; + pnCand1Pub_.publish(msg1); } if (candTrajs.size() > 2) { @@ -1124,6 +1138,10 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, bestTerminalCost = terminalCost2; bestCost = totalCost2; } + geometry_msgs::PoseArray msg2 = cand2.getPathRbtFrame(); + msg2.header.stamp = ros::Time::now(); + msg2.header.frame_id = cfg_.robot_frame_id; + pnCand2Pub_.publish(msg2); } } } From 3dc374d3b672523be3bceeb683ddf016ae81e432 Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Fri, 26 Sep 2025 06:29:27 -0400 Subject: [PATCH 12/13] best gap visual --- include/dynamic_gap/Planner.h | 2 ++ src/Planner.cpp | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/include/dynamic_gap/Planner.h b/include/dynamic_gap/Planner.h index 3378d83d..958fc273 100644 --- a/include/dynamic_gap/Planner.h +++ b/include/dynamic_gap/Planner.h @@ -442,6 +442,8 @@ namespace dynamic_gap ros::Publisher pnCand0Pub_; ros::Publisher pnCand1Pub_; ros::Publisher pnCand2Pub_; + ros::Publisher bestPnTrajPub_; + ros::Subscriber tfSub_; /**< Subscriber to TF tree */ diff --git a/src/Planner.cpp b/src/Planner.cpp index bc8a098b..7d0e5e8f 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -119,6 +119,7 @@ namespace dynamic_gap pnCand0Pub_ = nh_.advertise("pn_traj_cand0", 1); pnCand1Pub_ = nh_.advertise("pn_traj_cand1", 1); pnCand2Pub_ = nh_.advertise("pn_traj_cand2", 1); + bestPnTrajPub_ = nh_.advertise("best_pn_traj", 1); // Visualization Setup @@ -1145,6 +1146,20 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, } } } + // === Publish best PN trajectory for visualization === + if (bestTraj.getPathRbtFrame().poses.size() > 1) + { + geometry_msgs::PoseArray bestMsg = bestTraj.getPathRbtFrame(); + bestMsg.header.stamp = ros::Time::now(); + bestMsg.header.frame_id = cfg_.robot_frame_id; + bestPnTrajPub_.publish(bestMsg); + } + else + { + ROS_WARN_STREAM_NAMED("GapTrajectoryGeneratorV2", + "No valid best PN trajectory to publish (too few poses)"); + } + // for (auto &cand : candTrajs) From 48bee90098adaba38158edb83b42b193ed3eca8d Mon Sep 17 00:00:00 2001 From: abdelzaro Date: Sun, 28 Sep 2025 19:49:11 -0400 Subject: [PATCH 13/13] c --- include/dynamic_gap/config/DynamicGapConfig.h | 2 +- rviz/arena.rviz | 4 ++-- src/Planner.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/dynamic_gap/config/DynamicGapConfig.h b/include/dynamic_gap/config/DynamicGapConfig.h index 79b7a3d1..92d3be9a 100644 --- a/include/dynamic_gap/config/DynamicGapConfig.h +++ b/include/dynamic_gap/config/DynamicGapConfig.h @@ -67,7 +67,7 @@ namespace dynamic_gap bool gap_feasibility_check = true; /**< Flag for enacting gap feasibility checking */ bool perfect_gap_models = false; /**< Flag for using perfect gap models */ bool social_cost_function = true; /**< false - old without cost function, true - new */ - float social_cost_weight = .1; /**< weight for social cost during pose evaluation for traj selection */ + float social_cost_weight = .05; /**< weight for social cost during pose evaluation for traj selection */ } planning; /** diff --git a/rviz/arena.rviz b/rviz/arena.rviz index 54c5ad32..3e5b08ec 100644 --- a/rviz/arena.rviz +++ b/rviz/arena.rviz @@ -402,7 +402,7 @@ Visualization Manager: Namespaces: manip: true Queue Size: 100 - Value: true + Value: false - Class: rviz/Marker Enabled: false Marker Topic: /rto/global_path_local_waypoint @@ -749,7 +749,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 diff --git a/src/Planner.cpp b/src/Planner.cpp index fe1c1dd2..f8eec027 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -1027,7 +1027,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, { ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (available)"); - bool generate_multi_traj = true; // TODO: config flag + bool generate_multi_traj = true; // TODO: add to config if (generate_multi_traj) { std::vector candTrajs =