Show v instead of p

Author: TadaKazuto

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -300,25 +300,32 @@ void MultiObjectTracker::runProcess(
       char buf[200];
       auto tracker_itr = std::next(list_tracker.begin(), meas_tracker_idx->second);
       (*tracker_itr)->getTrackedObject(measurement_time, track_object);
-      snprintf(buf, sizeof(buf), "detect->track link idx[%ld]->[%d], x[%.3f]->x[%.3f], y[%.3f]->y[%.3f], cls[%d]->cls[%d]", 
-              measurement_idx, meas_tracker_idx->second,
-              measurement_object.kinematics.pose_with_covariance.pose.position.x, track_object.kinematics.pose_with_covariance.pose.position.x,
-              measurement_object.kinematics.pose_with_covariance.pose.position.y, track_object.kinematics.pose_with_covariance.pose.position.y,
-              measurement_object.classification.at(0).label, track_object.classification.at(0).label);
-      debug_message += buf;
-      RCLCPP_INFO(this->get_logger(), "object links:%s", debug_message.c_str());
-      debug_message = "";
+      if ( abs(measurement_object.kinematics.twist_with_covariance.twist.linear.x) > 0.3 or 
+           abs(measurement_object.kinematics.twist_with_covariance.twist.linear.y) > 0.3 ) {
+
+        snprintf(buf, sizeof(buf), "detect->track link idx[%ld]->[%d], vx[%.3f]->vx[%.3f], vy[%.3f ]->vy[%.3f], cls[%d]->cls[%d]", 
+                measurement_idx, meas_tracker_idx->second,
+                measurement_object.kinematics.twist_with_covariance.twist.linear.x, track_object.kinematics.twist_with_covariance.twist.linear.x,
+                measurement_object.kinematics.twist_with_covariance.twist.linear.y, track_object.kinematics.twist_with_covariance.twist.linear.y,
+                measurement_object.classification.at(0).label, track_object.classification.at(0).label);
+        debug_message += buf;
+        RCLCPP_INFO(this->get_logger(), "object links:%s", debug_message.c_str());
+        debug_message = "";
+      }
     }
     else {
-      char buf[120];
-      snprintf(buf, sizeof(buf), "detect->track link idx[%ld]->[nan], x[%.3f]->x[nan], y[%.3f]->y[nan], cls[%d]->cls[nan]", 
-              measurement_idx,
-              measurement_object.kinematics.pose_with_covariance.pose.position.x,
-              measurement_object.kinematics.pose_with_covariance.pose.position.y, 
-              measurement_object.classification.at(0).label);
-      debug_message += buf;
-      RCLCPP_INFO(this->get_logger(), "object links:%s", debug_message.c_str());
-      debug_message = "";
+      if ( abs(measurement_object.kinematics.twist_with_covariance.twist.linear.x) > 0.3 or 
+           abs(measurement_object.kinematics.twist_with_covariance.twist.linear.y) > 0.3 ) {
+        char buf[200];
+        snprintf(buf, sizeof(buf), "detect->track link idx[%ld]->[nan], vx[%.3f]->vx[nan], vy[%.3f ]->vy[nan], cls[%d]->cls[nan]", 
+                measurement_idx, 
+                measurement_object.kinematics.twist_with_covariance.twist.linear.x,
+                measurement_object.kinematics.twist_with_covariance.twist.linear.y,
+                measurement_object.classification.at(0).label);
+        debug_message += buf;
+        RCLCPP_INFO(this->get_logger(), "object links:%s", debug_message.c_str());
+        debug_message = "";
+      }
     }
   }