Function aliveIDsCallback

opt_utils/apps/ros2udp_converter.cpp:224–275

void
aliveIDsCallback(const opt_msgs::IDArray::ConstPtr& alive_ids_msg)
{
  ros::Time msg_time = ros::Time(alive_ids_msg->header.stamp.sec, alive_ids_msg->header.stamp.nsec);
  if ((msg_time - last_heartbeat_time).toSec() > heartbeat_interval)
  {
    /// Create JSON-formatted message:
    Jzon::Object root, header, stamp;

    /// Add header:
    header.Add("seq", int(alive_ids_msg->header.seq));
    stamp.Add("sec", int(alive_ids_msg->header.stamp.sec));
    stamp.Add("nsec", int(alive_ids_msg->header.stamp.nsec));
    header.Add("stamp", stamp);
    header.Add("frame_id", "heartbeat");
    root.Add("header", header);

    Jzon::Array alive_IDs;
    for (unsigned int i = 0; i < alive_ids_msg->ids.size(); i++)
    {
      alive_IDs.Add(alive_ids_msg->ids[i]);
    }
    root.Add("alive_IDs", alive_IDs);
    root.Add("max_ID", alive_ids_msg->max_ID);

    /// Convert JSON object to string:
    Jzon::Format message_format = Jzon::StandardFormat;
    message_format.indentSize = json_indent_size;
    message_format.newline = json_newline;
    message_format.spacing = json_spacing;
    message_format.useTabs = json_use_tabs;
    Jzon::Writer writer(root, message_format);
    writer.Write();
    std::string json_string = writer.GetResult();
    //  std::cout << "String sent: " << json_string << std::endl;

    /// Copy string to message buffer:
    udp_data.si_num_byte_ = json_string.length()+1;
    char buf[udp_data.si_num_byte_];
    for (unsigned int i = 0; i < udp_data.si_num_byte_; i++)
    {
      buf[i] = 0;
    }
    sprintf(buf, "%s", json_string.c_str());
    udp_data.pc_pck_ = buf;         // buffer where the message is written

    /// Send message:
    udp_messaging.sendFromSocketUDP(&udp_data);

    last_heartbeat_time = msg_time;
  }
}

typedef unsigned long uint32;
// convert a string represenation of an IP address into its numeric equivalent