本文整理汇总了C++中NODELET_DEBUG函数的典型用法代码示例。如果您正苦于以下问题:C++ NODELET_DEBUG函数的具体用法?C++ NODELET_DEBUG怎么用?C++ NODELET_DEBUG使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了NODELET_DEBUG函数的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: callback
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);
}
示例2: NODELET_DEBUG
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);
}
示例3: lock
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;
}
示例4: NODELET_ERROR
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 ());
}
示例5: NODELET_ERROR
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);
}
示例6: NODELET_INFO
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());
}
示例7: NODELET_DEBUG
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");
}
示例8: lock
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;
}
}
}
}
示例9: NODELET_DEBUG
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_);
}
}
示例10: NODELET_DEBUG
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 ());
}
}
示例11: callback
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);
}
示例12: NODELET_DEBUG
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_);
}
示例13: NODELET_DEBUG
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;
}
示例14: onInit
virtual void onInit ()
{
NODELET_DEBUG ("Initializing nodelet...");
exiting_ = false;
thread_ = boost::make_shared<boost::thread>
(boost::bind (&TrackerNodelet::spin, this));
}
示例15: NODELET_ERROR
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 ();
}
}