C++ NODELET_DEBUG函数代码示例

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