Function aliveIDsCallback

opt_utils/apps/ros2udp_converter_objects.cpp:125–176

}

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;
    }
}