Пример #1
0
void HeaderManipulation::inputCB(const topic_tools::ShapeShifter::ConstPtr &input)
{
    ROS_DEBUG("HeaderManipulation inputCB");
    ros::Time start_time(ros::Time::now());
    // Initialize.
    if (!output_advertised_) {
        ROS_INFO("Output not advertised. Setting up publisher now.");
        ros::AdvertiseOptions opts("output", 1, input->getMD5Sum(),
                                   input->getDataType(), input->getMessageDefinition());
        boost::mutex::scoped_lock pub_lock(pub_mutex_);
        generic_pub_ = private_nh_.advertise(opts);
        output_advertised_ = true;
    }
    // Copy shape shifter data to array.
    uint8_t msg_buffer[input->size()];
    ros::serialization::OStream o_stream(msg_buffer, input->size());
    input->write(o_stream);
    // Manipulate (header) data.
    manipulateRawData(msg_buffer);
    // Read data from array to StampedMsg
    boost::shared_ptr<StampedMsg> stamped_msg(new StampedMsg());
    stamped_msg->second = start_time;
    ros::serialization::IStream i_stream(msg_buffer, input->size());
    stamped_msg->first.read(i_stream);
    // Push StampedMsg to Buffer for multithreaded publishing.
    boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
    stamped_msg_buffer_.push(stamped_msg);
    ROS_DEBUG("all done");
}
 void AttitudeIndicatorPlugin::handleMessage(const topic_tools::ShapeShifter::ConstPtr& msg)
 {
   if (IS_INSTANCE(msg, nav_msgs::Odometry))
   {
     AttitudeCallbackOdom(msg->instantiate<nav_msgs::Odometry>());
   }
   else if (IS_INSTANCE(msg, sensor_msgs::Imu))
   {
     AttitudeCallbackImu(msg->instantiate<sensor_msgs::Imu>());
   }
   else if (IS_INSTANCE(msg, geometry_msgs::Pose))
   {
     AttitudeCallbackPose(msg->instantiate<geometry_msgs::Pose>());
   }
   else
   {
     PrintError("Unknown message type: " + msg->getDataType());
   }
 }
Пример #3
0
void RobustTopicRelay::MessageCallback(
    const std::string topic_name, const topic_tools::ShapeShifter::ConstPtr &message) {
  RobustTopicRelay::RelayedTopic &relayed_topic = relayed_topics_[topic_name];
  {
    boost::mutex::scoped_lock lock(mutex_);
    if (!relayed_topic.connected) {
      ROS_DEBUG("Connected to topic: %s", topic_name.c_str());
      relayed_topic.connected = true;
    }
    ros::Time next_expiration_time = ros::Time::now() + relayed_topic.expected_delay;
    expiring_topics_.erase(relayed_topic.expiration_time);
    relayed_topic.expiration_time = next_expiration_time;
    expiring_topics_[next_expiration_time] = topic_name;
  }
  if (!relayed_topic.publisher) {
    relayed_topic.publisher = message->advertise(
        node_handle_, relayed_topic.output_topic_name, 10);
  }
  relayed_topic.publisher.publish(message);
}