Пример #1
0
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();
}
Пример #2
0
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);
}