search

wg-perception / people

269 stars 197 forks source link

Dropping connection. error (data type issue) #106

Closed CAI23sbP closed 1 year ago

CAI23sbP commented 1 year ago

[ERROR] [1681224244.397135875, 0.600000006]: Client [/people_tracker] wants topic /people_tracker_measurements to have datatype/md5sum [people_msgs/PositionMeasurement/54fa938b4ec28728e01575b79eb0ec7c], but our version has [people_msgs/PositionMeasurementArray/59c860d40aa739ec920eb3ad24ae019e]. Dropping connection. Hi @DLu ! I have a error that you can see above line. Would you like fix this bug, and update new version in noetic? or the other verison ? please

CAI23sbP commented 1 year ago

This is my code (simply modify) @DLu

  1. people_tracking_node.cpp

include <people_tracking_filter/people_tracking_node.h>

include <people_tracking_filter/tracker_particle.h>

include <people_tracking_filter/tracker_kalman.h>

include <people_tracking_filter/state_pos_vel.h>

include <people_tracking_filter/rgb.h>

include <people_msgs/PositionMeasurement.h>

include <people_msgs/PositionMeasurementArray.h>

include

include

include

include

static const double sequencer_delay = 0.8; // TODO(noidea): this is probably too big, it was 0.8 static const unsigned int sequencer_internal_buffer = 100; static const unsigned int sequencer_subscribe_buffer = 10; static const unsigned int num_particles_tracker = 1000; static const double tracker_init_dist = 4.0;

namespace estimation { // constructor PeopleTrackingNode::PeopleTrackingNode(ros::NodeHandle nh) : nh_(nh), robotstate(), trackercounter(0) { // initialize meascloud.points = std::vector(1); meascloud.points[0].x = 0; meascloud.points[0].y = 0; meascloud.points[0].z = 0; // get parameters ros::NodeHandle local_nh("~"); local_nh.param("fixed_frame", fixedframe, std::string("default")); localnh.param("freq", freq, 1.0); local_nh.param("start_distance_min", start_distancemin, 0.0); local_nh.param("reliability_threshold", reliabilitythreshold, 1.0); local_nh.param("sys_sigma_pos_x", syssigma.pos_[0], 0.0); local_nh.param("sys_sigma_pos_y", syssigma.pos_[1], 0.0); local_nh.param("sys_sigma_pos_z", syssigma.pos_[2], 0.0); local_nh.param("sys_sigma_vel_x", syssigma.vel_[0], 0.0); local_nh.param("sys_sigma_vel_y", syssigma.vel_[1], 0.0); local_nh.param("sys_sigma_vel_z", syssigma.vel_[2], 0.0); local_nh.param("follow_one_person", follow_oneperson, false);

// advertise filter output people_filterpub = nh_.advertise("people_tracker_filter", 10);

// advertise visualization people_filter_vispub = nh_.advertise("people_tracker_filter_visualization", 10); people_tracker_vispub = nh_.advertise("people_tracker_measurements_visualization", 10);

// register message sequencer people_meassub = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this); }

// destructor PeopleTrackingNode::~PeopleTrackingNode() { // delete sequencer delete messagesequencer;

// delete all trackers for (std::list<Tracker>::iterator it = trackers.begin(); it != trackers.end(); it++) delete it; };

// callback for messages void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurementArray::ConstPtr& message) { if (message->people.size() > 0) { for (uint ipa = 0; ipa < message->people.size(); ipa++) { ROS_WARN("Tracking node got a people position measurement (%f,%f,%f)", message->people[ipa].pos.x, message->people[ipa].pos.y, message->people[ipa].pos.z); tf::Stamped meas_rel, meas;

meas_rel.setData(
  tf::Vector3(message->people[ipa].pos.x, message->people[ipa].pos.y, message->people[ipa].pos.z));
meas_rel.stamp_ = message->people[ipa].header.stamp;
meas_rel.frame_id_ = message->people[ipa].header.frame_id;
robot_state_.transformPoint(fixed_frame_, meas_rel, meas);
BFL::SymmetricMatrix cov(3);
for (unsigned int i = 0; i < 3; i++)
  for (unsigned int j = 0; j < 3; j++)
    cov(i + 1, j + 1) = message->people[ipa].covariance[3 * i + j];
boost::mutex::scoped_lock lock(filter_mutex_);

for (std::list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
  if ((*it)->getName() == message->people[ipa].object_id)
  {
    (*it)->updatePrediction(message->people[ipa].header.stamp.toSec());
    (*it)->updateCorrection(meas, cov);

  }
    if (message->people[ipa].object_id == "" && message->people[ipa].reliability > reliability_threshold_)
{
  double closest_tracker_dist = start_distance_min_;
  BFL::StatePosVel est;
  for (std::list<Tracker*>::iterator it = trackers_.begin(); it != trackers_.end(); it++)
  {
    (*it)->getEstimate(est);
    double dst = sqrt(pow(est.pos_[0] - meas[0], 2) + pow(est.pos_[1] - meas[1], 2));
    if (dst < closest_tracker_dist)
      closest_tracker_dist = dst;
  }
  // initialize a new tracker
  if (follow_one_person_)
    std::cout << "Following one person" << std::endl;
  if (message->people[ipa].initialization == 1 &&
      ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) ||
      (follow_one_person_ && trackers_.empty())))
  {
    // if (closest_tracker_dist >= start_distance_min_ || people[ipa].initialization == 1){
    // if (people[ipa].initialization == 1 && trackers_.empty()){
    ROS_INFO("Passed crazy conditional.");
    tf::Point pt;
    tf::pointMsgToTF(message->people[ipa].pos, pt);
    tf::Stamped<tf::Point> loc(pt, message->people[ipa].header.stamp, message->people[ipa].header.frame_id);
    robot_state_.transformPoint("base_link", loc, loc);
    float cur_dist;
    if ((cur_dist = pow(loc[0], 2.0) + pow(loc[1], 2.0)) < tracker_init_dist)
    {
      std::cout << "starting new tracker" << std::endl;
      std::stringstream tracker_name;
      BFL::StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)),
                                              sqrt(cov(2, 2)),
                                              sqrt(cov(3, 3))),
                                  tf::Vector3(0.0000001, 0.0000001, 0.0000001));
      tracker_name << "person " << tracker_counter_++;
      Tracker* new_tracker = new TrackerKalman(tracker_name.str(),
          sys_sigma_);
      // Tracker* new_tracker = new TrackerParticle(tracker_name.str(), num_particles_tracker, sys_sigma_);
      new_tracker->initialize(meas, prior_sigma,
                              message->people[ipa].header.stamp.toSec());
      trackers_.push_back(new_tracker);
      ROS_INFO("Initialized new tracker %s", tracker_name.str().c_str());
    }
    else
      ROS_INFO("Found a person, but he/she is not close enough to start following. "
              "Person is %f away, and must be less than %f away.", cur_dist, tracker_init_dist);
  }
  else
    ROS_INFO("Failed crazy conditional.");
}
lock.unlock();
// ------ LOCKED ------

// visualize measurement
meas_cloud_.points[ipa].x = meas[0];
meas_cloud_.points[ipa].y = meas[1];
meas_cloud_.points[ipa].z = meas[2];
meas_cloud_.header.frame_id = meas.frame_id_;
people_tracker_vis_pub_.publish(meas_cloud_);
}

} } // callback for dropped messages void PeopleTrackingNode::callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message) { ROS_INFO("DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!", message->object_id.c_str(), message->name.c_str(), (ros::Time::now() - message->header.stamp).toSec()); }

// filter loop void PeopleTrackingNode::spin() { ROS_INFO("People tracking manager started.");

while (ros::ok()) { // ------ LOCKED ------ boost::mutex::scoped_lock lock(filtermutex);

// visualization variables
std::vector<geometry_msgs::Point32> filter_visualize(trackers_.size());
std::vector<float> weights(trackers_.size());
sensor_msgs::ChannelFloat32 channel;

// loop over trackers
unsigned int i = 0;
std::list<Tracker*>::iterator it = trackers_.begin();
while (it != trackers_.end())
{
  // update prediction up to delayed time
  (*it)->updatePrediction(ros::Time::now().toSec() - sequencer_delay);

  // publish filter result
  people_msgs::PositionMeasurement est_pos;
  (*it)->getEstimate(est_pos);
  est_pos.header.frame_id = fixed_frame_;

  ROS_DEBUG("Publishing people tracker filter.");
  people_filter_pub_.publish(est_pos);

  // visualize filter result
  filter_visualize[i].x = est_pos.pos.x;
  filter_visualize[i].y = est_pos.pos.y;
  filter_visualize[i].z = est_pos.pos.z;

  // calculate weight
  {
    int quality = static_cast<int>(trunc((*it)->getQuality() * 999.0));
    int index = std::min(998, 999 - std::max(1, quality));
    weights[i] = *(float*) & (rgb[index]);  // NOLINT(readability/casting)
  }

  // remove trackers that have zero quality
  ROS_INFO("Quality of tracker %s = %f", (*it)->getName().c_str(), (*it)->getQuality());
  if ((*it)->getQuality() <= 0)
  {
    ROS_INFO("Removing tracker %s", (*it)->getName().c_str());
    delete *it;
    trackers_.erase(it++);
  }
  else it++;
  i++;
}
lock.unlock();
// ------ LOCKED ------

// visualize all trackers
channel.name = "rgb";
channel.values = weights;
sensor_msgs::PointCloud  people_cloud;
people_cloud.channels.push_back(channel);
people_cloud.header.frame_id = fixed_frame_;
people_cloud.points  = filter_visualize;
people_filter_vis_pub_.publish(people_cloud);

// sleep
usleep(1e6 / freq_);

ros::spinOnce();

} }; } // namespace estimation

// ---------- // -- MAIN -- // ---------- int main(int argc, char **argv) { // Initialize ROS ros::init(argc, argv, "people_tracker"); ros::NodeHandle(nh);

// create tracker node estimation::PeopleTrackingNode my_tracking_node(nh);

// wait for filter to finish my_tracking_node.spin();

return 0; }

2. people_tracking_node.h

ifndef PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H

define PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H

include

include

include <boost/thread/mutex.hpp>

// ros stuff

include <ros/ros.h>

include <tf/tf.h>

include <tf/transform_listener.h>

include <people_msgs/PositionMeasurementArray.h>

// people tracking stuff

include <people_tracking_filter/tracker.h>

include <people_tracking_filter/detector_particle.h>

include <people_tracking_filter/gaussian_vector.h>

// messages

include <sensor_msgs/PointCloud.h>

include <people_msgs/PositionMeasurement.h>

include <message_filters/time_sequencer.h>

include <message_filters/subscriber.h>

// log files

include

namespace estimation {

class PeopleTrackingNode { public: /// constructor explicit PeopleTrackingNode(ros::NodeHandle nh);

/// destructor virtual ~PeopleTrackingNode();

/// callback for messages void callbackRcv(const people_msgs::PositionMeasurementArray::ConstPtr& message);

/// callback for dropped messages void callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message);

/// tracker loop void spin();

private: ros::NodeHandle nh_;

ros::Publisher people_filterpub; ros::Publisher people_filter_vispub; ros::Publisher people_tracker_vispub;

ros::Subscriber people_meassub;

/// message sequencer message_filters::TimeSequencer* messagesequencer;

/// trackers std::list<Tracker*> trackers_;

// tf listener tf::TransformListener robotstate;

unsigned int trackercounter; double freq_, start_distancemin, reliabilitythreshold; BFL::StatePosVel syssigma; std::string fixedframe; boost::mutex filtermutex;

sensor_msgs::PointCloud meascloud; unsigned int meas_visualizecounter; std::vector cloudArray_;

// Track only one person who the robot will follow. bool follow_oneperson; }; // class } // namespace estimation

endif // PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H


How to fixed?
1. leg_detector send a topic as a array , but people_tracking_node take the topic as one data
2. Using iteration solved!