diff --git a/src/humap_planner.cpp b/src/humap_planner.cpp index 81eecfc9..b378563f 100644 --- a/src/humap_planner.cpp +++ b/src/humap_planner.cpp @@ -463,7 +463,14 @@ base_local_planner::Trajectory HumapPlanner::findBestTrajectory( auto traj_person = person.getTrajectoryPrediction(); trajs_people.push_back(traj_person); } - crossing_.detect(traj_robot_g, trajs_people); + if (crossing_.detect(traj_robot_g, trajs_people)) { + ROS_WARN_NAMED( + "HumapPlanner", + "Detected the intersection of robot's and %lu people's trajectories. Gap to the closest person is %5.2f m.", + crossing_.getCrossingPeopleData().size(), + crossing_.getGapToClosestPerson() + ); + } geometry_msgs::PoseStamped goal_pose; goal_pose.pose = goal_.getAsMsgPose();