void callback(const tPointCloud::ConstPtr& rgb_cloud, 
                const tImage::ConstPtr& rgb_image, 
                const tCameraInfo::ConstPtr& rgb_caminfo,
                const tImage::ConstPtr& depth_image, 
                const tPointCloud::ConstPtr& cloud
                )
  {
    if (max_update_rate_ > 0.0)
    {
      NODELET_DEBUG("update set to %f", max_update_rate_);
      if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
      {
        NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
        return;
      }
    }
    else
      NODELET_DEBUG("update_rate unset continuing");

    last_update_ = ros::Time::now();

    rgb_cloud_pub_.publish(rgb_cloud);
    rgb_image_pub_.publish(rgb_image);
    rgb_caminfo_pub_.publish(rgb_caminfo);
    depth_image_pub_.publish(depth_image);
    cloud_pub_.publish(cloud);
  }

    void controller_nodelet::cmdvel_cb(const geometry_msgs::Twist::ConstPtr& msg)
    {
        NODELET_DEBUG("Sending velocity commands: [%f] [%f]", msg->linear.x, msg->angular.z);

        ros::Time current_time = ros::Time::now();

        right_setpt_msg.timestamp = current_time;
        right_setpt_msg.node_name = "right_wheel";
        right_setpt_msg.mode = 2;

        left_setpt_msg.timestamp = current_time;
        left_setpt_msg.node_name = "left_wheel";
        left_setpt_msg.mode = 2;

        // Set velocity in m/s
        right_setpt_msg.velocity = msg->linear.x + (wheelbase / 2.f) * msg->angular.z;
        left_setpt_msg.velocity = msg->linear.x - (wheelbase / 2.f) * msg->angular.z;

        // Convert velocity to rad/s
        right_setpt_msg.velocity /= right_wheel_radius;
        left_setpt_msg.velocity /= left_wheel_radius;

        // Fix wheel direction
        right_setpt_msg.velocity *= right_wheel_direction
                                    * external_to_internal_wheelbase_encoder_direction;
        left_setpt_msg.velocity *= left_wheel_direction
                                    * external_to_internal_wheelbase_encoder_direction;

        NODELET_DEBUG("Parameters: [%f] [%f] [%f]", wheelbase, right_wheel_radius, left_wheel_radius);
        NODELET_DEBUG("Velocities: [%f] [%f]", right_setpt_msg.velocity, left_setpt_msg.velocity);

        // Publish the setpoints
        right_setpt_pub.publish(right_setpt_msg);
        left_setpt_pub.publish(left_setpt_msg);
    }

  void JointStateStaticFilter::jointStateCallback(
    const sensor_msgs::JointState::ConstPtr& msg)
  {
    boost::mutex::scoped_lock lock(mutex_);
    NODELET_DEBUG("jointCallback");
    // filter out joints based on joint names
    std::vector<double> joints = filterJointState(msg);
    if (joints.size() == 0) {
      NODELET_DEBUG("cannot find the joints from the input message");
      return;
    }
    joint_vital_->poke();

    // check the previous state...
    if (previous_joints_.size() > 0) {
      // compute velocity
      for (size_t i = 0; i < previous_joints_.size(); i++) {
        // NODELET_INFO("[%s] diff: %f", joint_names_[i].c_str(),
        //              fabs(previous_joints_[i] - joints[i]));
        if (fabs(previous_joints_[i] - joints[i]) > eps_) {
          buf_.push_front(boost::make_tuple<ros::Time, bool>(
                            msg->header.stamp, false));
          previous_joints_ = joints;
          return;
        }
      }
      buf_.push_front(boost::make_tuple<ros::Time, bool>(
                        msg->header.stamp, true));
    }
    previous_joints_ = joints;
  }

void
pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud, 
                                                    const PointCloudConstPtr &cloud_target)
{
  if (pub_output_.getNumSubscribers () <= 0)
    return;

  if (!isValid (cloud) || !isValid (cloud_target, "target")) 
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
    PointCloud output;
    output.header = cloud->header;
    pub_output_.publish (output.makeShared ());
    return;
  }

  NODELET_DEBUG ("[%s::input_indices_callback]\n"
                 "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                 "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
                 getName ().c_str (),
                 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
                 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());

  impl_.setInputCloud (cloud);
  impl_.setTargetCloud (cloud_target);

  PointCloud output;
  impl_.segment (output);

  pub_output_.publish (output.makeShared ());
  NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
                     output.points.size (), fromPCL(output.header).stamp.toSec (), pnh_->resolveName ("output").c_str ());
}

void
jsk_pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
{
  // No subscribers, no work
  if (pub_output_.getNumSubscribers () <= 0)
    return;

  // If cloud is given, check if it's valid
  if (!isValid (cloud))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
    return;
  }
  // If indices are given, check if they are valid
  if (indices && !isValid (indices))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
    return;
  }

  /// DEBUG
  if (indices)
    NODELET_DEBUG ("[%s::input_indices_callback]\n"
                   "                                 - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
                   "                                 - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
                   getName ().c_str (), 
                   cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
                   indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
  else
    NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
  ///

  // Check whether the user has given a different input TF frame
  tf_input_orig_frame_ = cloud->header.frame_id;
  PointCloud2::ConstPtr cloud_tf;
  if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
  {
    NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
    // Save the original frame ID
    // Convert the cloud into the different frame
    PointCloud2 cloud_transformed;
    tf_listener_.waitForTransform(cloud->header.frame_id, tf_input_frame_, cloud->header.stamp, ros::Duration(5.0)); 
    if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
    {
      NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
      return;
    }
    cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
  }
  else
    cloud_tf = cloud;

  // Need setInputCloud () here because we have to extract x/y/z
  IndicesPtr vindices;
  if (indices)
    vindices.reset (new std::vector<int> (indices->indices));

  computePublish (cloud_tf, vindices);
}

 void ImageResizer::config_cb ( ImageResizerConfig &config, uint32_t level) {
   NODELET_INFO("config_cb");
   resize_x_ = config.resize_scale_x;
   resize_y_ = config.resize_scale_y;
   period_ = ros::Duration(1.0/config.msg_par_second);
   verbose_ = config.verbose;
   NODELET_DEBUG("resize_scale_x : %f", resize_x_);
   NODELET_DEBUG("resize_scale_y : %f", resize_y_);
   NODELET_DEBUG("message period : %f", period_.toSec());
 }

void
Edge_detector_nodelet::callback(const sensor_msgs::ImageConstPtr& input_msg_image){
	NODELET_DEBUG("callback");

	if(pub_.getNumSubscribers() == 0) return;

	cv_bridge::CvImagePtr cv_ptr;
	try{
	   cv_ptr = cv_bridge::toCvCopy(input_msg_image, input_msg_image->encoding);
	}
	catch (cv_bridge::Exception& e)
	{
	   ROS_ERROR_NAMED(node_name_, "cv_bridge exception: %s", e.what());
	   return;
	}

	cv::Mat src_gray, dst_gray, dst_color;

	if(input_msg_image->encoding == sensor_msgs::image_encodings::MONO8){
		src_gray = cv_ptr->image;
	}
	else{
		cvtColor( cv_ptr->image, src_gray, CV_BGR2GRAY );
	}

	try{
		switch(config_.filter){
			case 0:
				cv::Canny( src_gray, dst_gray, config_.threshold1, config_.threshold2, config_.kernel_size, config_.L2gradient );
				break;
			case 1:
				cv::Laplacian( src_gray, dst_gray, CV_16S, config_.kernel_size, 1 , 0 );
				break;
			default :
				ROS_ERROR_NAMED(node_name_, "Filter not implemented, select filter between 0 and 3:");
		}
	}catch (cv::Exception &e){
		ROS_ERROR_NAMED(node_name_,"cv_bridge exception: %s", e.what());
	}

	cv_bridge::CvImage image_edge;

	if(config_.publish_color){
		 cvtColor(dst_gray, dst_color, CV_GRAY2BGR);
		 image_edge = cv_bridge::CvImage(cv_ptr->header, input_msg_image->encoding, dst_color);
	}
	else{
		image_edge = cv_bridge::CvImage(cv_ptr->header, sensor_msgs::image_encodings::MONO8, dst_gray);
	}

	pub_.publish(image_edge.toImageMsg());

	NODELET_DEBUG("callback end");
}

 void Relay::disconnectCb()
 {
   boost::mutex::scoped_lock lock(mutex_);
   NODELET_DEBUG("disconnectCb");
   if (advertised_) {
     if (pub_.getNumSubscribers() == 0) {
       if (subscribing_) {
         NODELET_DEBUG("disconnect");
         sub_.shutdown();
         subscribing_ = false;
       }
     }
   }
 }

void
pcl_ros::Feature::config_callback (FeatureConfig &config, uint32_t level)
{
  if (k_ != config.k_search)
  {
    k_ = config.k_search;
    NODELET_DEBUG ("[config_callback] Setting the number of K nearest neighbors to use for each point: %d.", k_);
  }
  if (search_radius_ != config.radius_search)
  {
    search_radius_ = config.radius_search;
    NODELET_DEBUG ("[config_callback] Setting the nearest neighbors search radius for each point: %f.", search_radius_);
  }
}

void
jsk_pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
{
  // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
  if (tf_input_frame_ != config.input_frame)
  {
    tf_input_frame_ = config.input_frame;
    NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
  }
  if (tf_output_frame_ != config.output_frame)
  {
    tf_output_frame_ = config.output_frame;
    NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
  }
}

 void callback(const PointCloud::ConstPtr& cloud)
 {
   if (max_update_rate_ > 0.0)
   {
     NODELET_DEBUG("update set to %f", max_update_rate_);
     if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
     {
       NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
       return;
     }
   }
   else
     NODELET_DEBUG("update_rate unset continuing");
   last_update_ = ros::Time::now();
   pub_.publish(cloud);
 }

void
pcl_ros::SegmentDifferences::onInit ()
{
  // Call the super onInit ()
  PCLNodelet::onInit ();

  pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);

  // Subscribe to the input using a filter
  sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
  sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);

  // Enable the dynamic reconfigure service
  srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
  dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f =  boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
  srv_->setCallback (f);

  if (approximate_sync_)
  {
    sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
    sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
    sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
  }
  else
  {
    sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
    sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
    sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
  }

  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
                 " - max_queue_size    : %d",
                 getName ().c_str (),
                 max_queue_size_);
}

  bool PolygonPointsSampler::isValidMessage(
    const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
    const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
  {
    if (polygon_msg->polygons.size() == 0) {
      NODELET_DEBUG("empty polygons");
      return false;
    }
    if (coefficients_msg->coefficients.size() != polygon_msg->polygons.size()) {
      NODELET_ERROR("The size of coefficients and polygons are not same");
      return false;
    }

    std::string frame_id = polygon_msg->header.frame_id;
    for (size_t i = 0; i < polygon_msg->polygons.size(); i++) {
      if (frame_id != polygon_msg->polygons[i].header.frame_id) {
        NODELET_ERROR("Frame id of polygon is not same: %s, %s",
                      frame_id.c_str(),
                      polygon_msg->polygons[i].header.frame_id.c_str());
        return false;
      }
    }
    for (size_t i = 0; i < coefficients_msg->coefficients.size(); i++) {
      if (frame_id != coefficients_msg->coefficients[i].header.frame_id) {
        NODELET_ERROR("Frame id of coefficient is not same: %s, %s",
                      frame_id.c_str(),
                      coefficients_msg->coefficients[i].header.frame_id.c_str());
        return false;
      }
    }
    return true;
  }

    virtual void onInit ()
    {
      NODELET_DEBUG ("Initializing nodelet...");
      exiting_ = false;
      thread_ = boost::make_shared<boost::thread>
	(boost::bind (&TrackerNodelet::spin, this));
    }

void
pcl_ros::BAGReader::onInit ()
{
  boost::shared_ptr<ros::NodeHandle> pnh_;
  pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
  // ---[ Mandatory parameters
  if (!pnh_->getParam ("file_name", file_name_))
  {
    NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
    return;
  }
   if (!pnh_->getParam ("topic_name", topic_name_))
  {
    NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); 
    return;
  }
  // ---[ Optional parameters
  int max_queue_size = 1;
  pnh_->getParam ("publish_rate", publish_rate_);
  pnh_->getParam ("max_queue_size", max_queue_size);

  ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);

  NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
                 " - file_name    : %s\n"
                 " - topic_name   : %s",
                 file_name_.c_str (), topic_name_.c_str ());

  if (!open (file_name_, topic_name_))
    return;
  PointCloud output;
  output_ = boost::make_shared<PointCloud> (output);
  output_->header.stamp    = ros::Time::now ();

  // Continous publishing enabled?
  while (pnh_->ok ())
  {
    PointCloudConstPtr cloud = getNextCloud ();
    NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
    output_->header.stamp = ros::Time::now ();

    pub_output.publish (output_);

    ros::Duration (publish_rate_).sleep ();
    ros::spinOnce ();
  }
}

 void subscribe()
 {
   NODELET_DEBUG("Subscribing to image topic.");
   if (config_.use_camera_info)
     cam_sub_ = it_->subscribeCamera("image", 3, &FindContoursNodelet::imageCallbackWithInfo, this);
   else
     img_sub_ = it_->subscribe("image", 3, &FindContoursNodelet::imageCallback, this);
 }

void
pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
{
  if (!isValid (cloud))
    return;
  
  getMTPrivateNodeHandle ().getParam ("filename", file_name_);

  NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
 
  std::string fname;
  if (file_name_.empty ())
    fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
  else
    fname = file_name_;
  impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);

  NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
}

  ~PointGreyCameraNodelet()
  {
    if(pubThread_)
    {
      pubThread_->interrupt();
      pubThread_->join();
    }

    try
    {
      NODELET_DEBUG("Stopping camera capture.");
      pg_.stop();
      NODELET_DEBUG("Disconnecting from camera.");
      pg_.disconnect();
    }
    catch(std::runtime_error& e)
    {
      NODELET_ERROR("%s", e.what());
    }
  }

  /*!
  * \brief Function that allows reconfiguration of the camera.
  *
  * This function serves as a callback for the dynamic reconfigure service.  It simply passes the configuration object to the driver to allow the camera to reconfigure.
  * \param config  camera_library::CameraConfig object passed by reference.  Values will be changed to those the driver is currently using.
  * \param level driver_base reconfiguration level.  See driver_base/SensorLevels.h for more information.
  */
  void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
  {
    try
    {
      NODELET_DEBUG("Dynamic reconfigure callback with level: %d", level);
      pg_.setNewConfiguration(config, level);

      // Store needed parameters for the metadata message
      gain_ = config.gain;
      wb_blue_ = config.white_balance_blue;
      wb_red_ = config.white_balance_red;

      // Store CameraInfo binning information
      if(config.video_mode == "640x480_mono8" || config.video_mode == "format7_mode1")
      {
        binning_x_ = 2;
        binning_y_ = 2;
      }
      else if(config.video_mode == "format7_mode2")
      {
        binning_x_ = 0;
        binning_y_ = 2;
      }
      else
      {
        binning_x_ = 0;
        binning_y_ = 0;
      }

      // Store CameraInfo RegionOfInterest information
      if(config.video_mode == "format7_mode0" || config.video_mode == "format7_mode1" || config.video_mode == "format7_mode2")
      {
        roi_x_offset_ = config.format7_x_offset;
        roi_y_offset_ = config.format7_y_offset;
        roi_width_ = config.format7_roi_width;
        roi_height_ = config.format7_roi_height;
        do_rectify_ = true; // Set to true if an ROI is used.
      }
      else
      {
        // Zeros mean the full resolution was captured.
        roi_x_offset_ = 0;
        roi_y_offset_ = 0;
        roi_height_ = 0;
        roi_width_ = 0;
        do_rectify_ = false; // Set to false if the whole image is captured.
      }
    }
    catch(std::runtime_error& e)
    {
      NODELET_ERROR("Reconfigure Callback failed with error: %s", e.what());
    }
  }

void
pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
{
  // \Note Zoli, shouldn't this be implemented in MLS too?
  /*if (k_ != config.k_search)
  {
    k_ = config.k_search;
    NODELET_DEBUG ("[config_callback] Setting the k_search to: %d.", k_); 
  }*/
  if (search_radius_ != config.search_radius)
  {
    search_radius_ = config.search_radius;
    NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
    impl_.setSearchRadius (search_radius_);
  }
  if (spatial_locator_type_ != config.spatial_locator)
  {
    spatial_locator_type_ = config.spatial_locator;
    NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
  }
  if (use_polynomial_fit_ != config.use_polynomial_fit)
  {
    use_polynomial_fit_ = config.use_polynomial_fit;
    NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
    impl_.setPolynomialFit (use_polynomial_fit_);
  }
  if (polynomial_order_ != config.polynomial_order)
  {
    polynomial_order_ = config.polynomial_order;
    NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
    impl_.setPolynomialOrder (polynomial_order_);
  }
  if (gaussian_parameter_ != config.gaussian_parameter)
  {
    gaussian_parameter_ = config.gaussian_parameter;
    NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
    impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
  }
}