diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp index 5acb060eda96709831b141137028743e22699a86..68cc76ad9592c8fa9ea94438d8086f95cf1843df 100644 --- a/src/subscriber_landmarks.cpp +++ b/src/subscriber_landmarks.cpp @@ -51,7 +51,7 @@ void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& top void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg) { - ROS_INFO("SubscriberLandmarks::callback"); + ROS_INFO("SubscriberLandmarks::callback: %lu detections", msg->detections.size()); updateLastHeader(msg->header); @@ -91,6 +91,8 @@ void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray:: } else cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data()); + + std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ", meas: " << meas.transpose() << std::endl; // fill capture makePosDef(cov);