Example #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());
   }
 }