void callback(const ros::MessageEvent<variant_topic_tools::Message>&
messageEvent) {
boost::shared_ptr<const variant_topic_tools::Message> message =
messageEvent.getConstMessage();
boost::shared_ptr<const ros::M_string> connectionHeader =
messageEvent.getConnectionHeaderPtr();
if (!publisher) {
bool latch = false;
if (connectionHeader) {
ros::M_string::const_iterator it = connectionHeader->find("latching");
if ((it != connectionHeader->end()) && (it->second == "1"))
latch = true;
}
ros::AdvertiseOptions options(publisherTopic, publisherQueueSize,
message->getType().getMD5Sum(), message->getType().getDataType(),
message->getType().getDefinition(), connectCallback);
options.latch = latch;
publisher = nodeHandle->advertise<variant_msgs::Variant>(publisherTopic,
publisherQueueSize, connectCallback, ros::SubscriberStatusCallback(),
ros::VoidConstPtr(), latch);
}
if(!lazy || publisher.getNumSubscribers()) {
boost::shared_ptr<const variant_msgs::Variant> variantMessage =
message->toVariantMessage();
publisher.publish(variantMessage);
}
else
subscriber = ros::Subscriber();
}
void MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
std::string authority = marker_evt.getPublisherName();
std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
void callback(const ros::MessageEvent<tf::tfMessage const>& msg_evt)
{
const tf::tfMessage& message = *(msg_evt.getConstMessage());
std::string authority = msg_evt.getPublisherName(); // lookup the authority
double average_offset = 0;
boost::mutex::scoped_lock my_lock(map_lock_);
for (unsigned int i = 0; i < message.transforms.size(); i++)
{
frame_authority_map[message.transforms[i].child_frame_id] = authority;
double offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec();
average_offset += offset;
std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id);
if (it == delay_map.end())
{
delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
}
else
{
it->second.push_back(offset);
if (it->second.size() > 1000)
it->second.erase(it->second.begin());
}
}
average_offset /= max((size_t) 1, message.transforms.size());
//create the authority log
std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
if (it2 == authority_map.end())
{
authority_map[authority] = std::vector<double>(1,average_offset);
}
else
{
it2->second.push_back(average_offset);
if (it2->second.size() > 1000)
it2->second.erase(it2->second.begin());
}
//create the authority frequency log
std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
if (it3 == authority_frequency_map.end())
{
authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec());
}
else
{
it3->second.push_back(ros::Time::now().toSec());
if (it3->second.size() > 1000)
it3->second.erase(it3->second.begin());
}
};
void MarkerDisplayCustom::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
/// Changed in indigo
// std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
// setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
std::string authority = marker_evt.getPublisherName();
std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
{
boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage();
if (!g_advertised)
{
g_pub = msg->advertise(*g_node, g_output_topic, 10, true);
foreign_advertise(msg_event.getConnectionHeader()["type"]);
g_advertised = true;
printf("advertised as %s\n", g_output_topic.c_str());
}
g_pub.publish(msg);
}
