Function synchronizedCallback

opt_utils/apps/ros2udp_converter_pose.cpp:195–235

}

void
synchronizedCallback(
    const opt_msgs::SkeletonTrackArrayConstPtr& skel_track_msg,
    const opt_msgs::StandardSkeletonTrackArrayConstPtr& st_skel_msg,
    const opt_msgs::PoseRecognitionArrayConstPtr& pr_array_msg)
{
  // ROS_INFO_STREAM("Synchronized!");
  std::string buffer("");
  for (int i = 0, end = skel_track_msg->tracks.size(); i < end; ++i)
  {
    Jzon::Object current_track;
    const opt_msgs::SkeletonTrack& t = skel_track_msg->tracks[i];
    const opt_msgs::StandardSkeletonTrack& st_t = st_skel_msg->tracks[i];
    const opt_msgs::PoseRecognition& pr = pr_array_msg->poses[i];
    std::string jsonmsg = toJsonPoseMsg(pr_array_msg->header,
                                        skel_track_msg->header.frame_id,
                                        t, st_t, pr);
    if (jsonmsg.length() + 1 > udp_buffer_length)
    {
      ROS_WARN_STREAM("Unexpected: json message doesn’t fit in payload "
                      << jsonmsg);
      if (not buffer.empty())
      {
        sendPacket(buffer);
        buffer = "";
      }
      sendPacket(jsonmsg);
      continue;
    }
    if (jsonmsg.length() + 1 + buffer.length() + 1 > udp_buffer_length)
    {
      sendPacket(buffer);
      buffer = "";
    }
    buffer += jsonmsg;
  }
  // jb
  // send remaining buffer
  if (not buffer.empty())
    sendPacket(buffer);
}


typedef unsigned long uint32;