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