planner - detection of the robot's path crossing by surrounding hum…

…an(s) logged

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();