void throttle_cb(const std_msgs::Float64::ConstPtr &req) { float throttle_normalized = req->data; if (reverse_throttle) if ( throttle_normalized < -1.0 || throttle_normalized > 1.0 ) { ROS_DEBUG_ONCE("Warning: Not normalized values of throttle! Values should be between -1.0 and 1.0"); return; } else send_attitude_throttle(throttle_normalized); else if ( throttle_normalized < 0.0 || throttle_normalized > 1.0 ) { ROS_DEBUG_ONCE("Warning: Not normalized values of throttle! Values should be between 0.0 and 1.0"); return; } else send_attitude_throttle(throttle_normalized); }
void ImageMask::maskImage(cv::Mat& image) { if (m_useMask) { if (image.cols == m_mask.cols && image.rows == m_mask.rows){ #if DEBUG_MASK cv::imshow("Original", image); #endif cv::bitwise_and(image, m_mask, image); #if DEBUG_MASK cv::imshow("Mask", m_mask); cv::imshow("Masked", image); cv::waitKey(10); #endif } else { ROS_ERROR("Image and Mask do not have the same size!"); } } else { ROS_DEBUG_ONCE("I do not mask."); } }
void pcl_ros::PCDReader::onInit () { ros::NodeHandle private_nh = getMTPrivateNodeHandle (); // Provide a latched topic ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true); private_nh.getParam ("publish_rate", publish_rate_); private_nh.getParam ("tf_frame", tf_frame_); NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n" " - publish_rate : %f\n" " - tf_frame : %s", getName ().c_str (), publish_rate_, tf_frame_.c_str ()); PointCloud2Ptr output_new; output_ = boost::make_shared<PointCloud2> (); output_new = boost::make_shared<PointCloud2> (); // Wait in a loop until someone connects do { ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ()); ros::spinOnce (); ros::Duration (0.01).sleep (); } while (private_nh.ok () && pub_output.getNumSubscribers () == 0); std::string file_name; while (private_nh.ok ()) { // Get the current filename parameter. If no filename set, loop if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ()) { ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ()); ros::Duration (0.01).sleep (); ros::spinOnce (); continue; } // If the filename parameter holds a different value than the last one we read if (file_name_.compare (file_name) != 0 && !file_name_.empty ()) { NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ()); file_name = file_name_; PointCloud2 cloud; if (impl_.read (file_name_, cloud) < 0) { NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ()); return; } output_ = boost::make_shared<PointCloud2> (cloud); output_->header.stamp = ros::Time::now (); output_->header.frame_id = tf_frame_; } // We do not publish empty data if (output_->data.size () == 0) continue; if (publish_rate_ == 0) { if (output_ != output_new) { NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ()); pub_output.publish (output_); output_new = output_; } ros::Duration (0.01).sleep (); } else { NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().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 (); // Update parameters from server private_nh.getParam ("publish_rate", publish_rate_); private_nh.getParam ("tf_frame", tf_frame_); } }