diff --git a/include/dynamic_gap/Planner.h b/include/dynamic_gap/Planner.h index e7695b78..958fc273 100644 --- a/include/dynamic_gap/Planner.h +++ b/include/dynamic_gap/Planner.h @@ -438,6 +438,14 @@ 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::Publisher pnCand0Pub_; + ros::Publisher pnCand1Pub_; + ros::Publisher pnCand2Pub_; + ros::Publisher bestPnTrajPub_; + + + ros::Subscriber tfSub_; /**< Subscriber to TF tree */ ros::Subscriber laserSub_; /**< Subscriber to incoming laser scan */ diff --git a/include/dynamic_gap/config/DynamicGapConfig.h b/include/dynamic_gap/config/DynamicGapConfig.h index 91d09d43..92d3be9a 100644 --- a/include/dynamic_gap/config/DynamicGapConfig.h +++ b/include/dynamic_gap/config/DynamicGapConfig.h @@ -66,6 +66,8 @@ namespace dynamic_gap bool projection_operator = true; /**< Boolean for if planner should apply projection operator */ 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 = .05; /**< 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/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/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/rviz/arena.rviz b/rviz/arena.rviz index ad88d58f..3e5b08ec 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,19 +388,19 @@ 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 - Class: rviz/Marker @@ -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 @@ -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 @@ -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: diff --git a/src/Planner.cpp b/src/Planner.cpp index 4c853500..f8eec027 100644 --- a/src/Planner.cpp +++ b/src/Planner.cpp @@ -115,6 +115,13 @@ 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); + bestPnTrajPub_ = nh_.advertise("best_pn_traj", 1); + + // Visualization Setup // mpcInputPublisher_ = nh_.advertise("mpc_input", 1); @@ -1008,7 +1015,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(); @@ -1018,39 +1025,215 @@ 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: add to config + 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; + + 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_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", + " [TEST] first candidate traj cost = " << totalCost0); + + // Store as best by default + bestTraj = cand0; + 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) + { + // === 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; + + } + 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) + { + // === 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; + } + geometry_msgs::PoseArray msg2 = cand2.getPathRbtFrame(); + msg2.header.stamp = ros::Time::now(); + msg2.header.frame_id = cfg_.robot_frame_id; + pnCand2Pub_.publish(msg2); + } + } + } + // === 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) + // { + // // 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, gap); + pursuitGuidancePoseCost = pursuitGuidanceTerminalPoseCost + + 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); + } + } + + + + else { ROS_INFO_STREAM_NAMED("GapTrajectoryGeneratorV2", " running pursuit guidance (not available)"); pursuitGuidanceTraj = gapTrajGenerator_->generateIdlingTrajectoryV2(gap, currPose, runningTraj); - // rbtPoseInOdomFrame_); - // pursuitGuidanceTraj = gapTrajGenerator_->processIdlingTrajectory(pursuitGuidanceTraj, - // runningTraj); + pursuitGuidanceTraj = gapTrajGenerator_->processTrajectory(pursuitGuidanceTraj); // currPose, // currVel, // false); + + trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx, gap); + + // rbtPoseInOdomFrame_); + // pursuitGuidanceTraj = gapTrajGenerator_->processIdlingTrajectory(pursuitGuidanceTraj, + // runningTraj); + // if (j == (gapTube->size() - 1)) // { // // prune trajectory @@ -1058,7 +1241,7 @@ void Planner::jointPoseAccCB(const nav_msgs::Odometry::ConstPtr & rbtOdomMsg, // pursuitGuidanceTraj = gapTrajGenerator_->pruneTrajectory(pursuitGuidanceTraj); // } - trajEvaluator_->evaluateTrajectory(pursuitGuidanceTraj, pursuitGuidancePoseCosts, pursuitGuidanceTerminalPoseCost, futureScans, scanIdx); + // 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); @@ -1178,11 +1361,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("GapTrajectoryGeneratorV2", " pursuitGuidancePoseCost: " << pursuitGuidancePoseCost); @@ -1227,7 +1411,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(); @@ -1500,7 +1684,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); @@ -1589,7 +1773,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 (length: " << reducedCurrentPathRobotFrame.poses.size() << " received a subcost of: " << reducedCurrentPathSubCost); diff --git a/src/trajectory_evaluation/TrajectoryEvaluator.cpp b/src/trajectory_evaluation/TrajectoryEvaluator.cpp index 091ac2be..0773ad98 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,61 @@ 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) + { + if (gap) + { + + leftGapPtIsDynamic = gap->getLeftGapPt()->getUngapID()>=0; + if (leftGapPtIsDynamic) + { + + 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); + + } + } + } ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " [evaluateTrajectory()]"); // Requires LOCAL FRAME @@ -37,6 +88,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 +108,37 @@ namespace dynamic_gap for (int i = 0; i < posewiseCosts.size(); i++) { - ROS_INFO_STREAM_NAMED("TrajectoryEvaluator", " pose " << i << " (total scan idx: " << (scanIdx + 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; + 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(scanIdx + i)); // / 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 +156,145 @@ 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) + { + 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()); + + + 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); + } + } + } + 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; + 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_); @@ -147,4 +368,17 @@ 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 + 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(); + + return cost; +} + } \ No newline at end of file diff --git a/src/trajectory_generation/GapTrajectoryGenerator.cpp b/src/trajectory_generation/GapTrajectoryGenerator.cpp index d8711d94..b3636058 100644 --- a/src/trajectory_generation/GapTrajectoryGenerator.cpp +++ b/src/trajectory_generation/GapTrajectoryGenerator.cpp @@ -174,6 +174,82 @@ 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; + // ROS_ERROR_STREAM_NAMED("GapTrajectoryGeneratorV2", "cfg_->sensor_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; + + 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]}; + + 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)