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 Relay::connectCb()
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_DEBUG("connectCB");
if (advertised_) {
if (pub_.getNumSubscribers() > 0) {
if (!subscribing_) {
NODELET_DEBUG("suscribe");
sub_ = pnh_.subscribe<topic_tools::ShapeShifter>("input", 1,
&Relay::inputCallback, this);
subscribing_ = true;
}
}
}
}
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::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
{
if (impl_.getDistanceThreshold () != config.distance_threshold)
{
impl_.setDistanceThreshold (config.distance_threshold);
NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
}
}
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_);
}
}
// What we want to do here is to look at all the points that are *not* included in the indices list,
// (i.e. these are the outliers, the points not on the floor) and determine which are the closest to the camera
void cloudIndicesModelCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const PointIndicesConstPtr& indices, const pcl::ModelCoefficientsConstPtr& model)
{
boost::mutex::scoped_lock lock (callback_mutex);
NODELET_DEBUG_STREAM("Got cloud with timestamp " << cloud_msg->header.stamp << " + indices with timestamp "
<< indices->header.stamp << " + model with timestamp " << model->header.stamp);
double dt;
if (first)
{
first = false;
dt = 0;
}
else
{
dt = (cloud_msg->header.stamp - prev_cloud_time).toSec();
prev_cloud_time = cloud_msg->header.stamp;
}
if (model->values.size() > 0)
{
// Determine altitude:
kinect_z = reject_outliers(-fabs(model->values[3])) - imu_to_kinect_offset;
calcVelocity(kinect_z, dt, kinect_vz);
// Detect obstacles:
pcl::PointXYZ obstacle_location;
bool obstacle_found = detectObstacle(cloud_msg, indices, model, obstacle_location);
if (obstacle_found)
{
publishObstacleMarker(obstacle_location);
NODELET_DEBUG("Detected obstacle at: [%f, %f, %f]", obstacle_location.x, obstacle_location.y,
obstacle_location.z);
}
publishObstacle(obstacle_found, obstacle_location);
}
else
{
NODELET_WARN("No planar model found -- cannot determine altitude or obstacles");
}
updateMask();
if (not use_backup_estimator_alt)
{
publishOdom();
}
if (debug_throttle_rate > 0)
{
ros::Duration(1 / debug_throttle_rate).sleep();
}
}
void
pcl_ros::VoxelGrid::config_callback (pcl_ros::VoxelGridConfig &config, uint32_t level)
{
boost::mutex::scoped_lock lock (mutex_);
Eigen::Vector3f leaf_size = impl_.getLeafSize ();
if (leaf_size[0] != config.leaf_size)
{
leaf_size.setConstant (config.leaf_size);
NODELET_DEBUG ("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]);
impl_.setLeafSize (leaf_size[0], leaf_size[1], leaf_size[2]);
}
double filter_min, filter_max;
impl_.getFilterLimits (filter_min, filter_max);
if (filter_min != config.filter_limit_min)
{
filter_min = config.filter_limit_min;
NODELET_DEBUG ("[config_callback] Setting the minimum filtering value a point will be considered from to: %f.", filter_min);
}
if (filter_max != config.filter_limit_max)
{
filter_max = config.filter_limit_max;
NODELET_DEBUG ("[config_callback] Setting the maximum filtering value a point will be considered from to: %f.", filter_max);
}
impl_.setFilterLimits (filter_min, filter_max);
if (impl_.getFilterLimitsNegative () != config.filter_limit_negative)
{
impl_.setFilterLimitsNegative (config.filter_limit_negative);
NODELET_DEBUG ("[%s::config_callback] Setting the filter negative flag to: %s.", getName ().c_str (), config.filter_limit_negative ? "true" : "false");
}
if (impl_.getFilterFieldName () != config.filter_field_name)
{
impl_.setFilterFieldName (config.filter_field_name);
NODELET_DEBUG ("[config_callback] Setting the filter field name to: %s.", config.filter_field_name.c_str ());
}
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
if (tf_input_frame_ != config.input_frame)
{
tf_input_frame_ = config.input_frame;
NODELET_DEBUG ("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str ());
}
if (tf_output_frame_ != config.output_frame)
{
tf_output_frame_ = config.output_frame;
NODELET_DEBUG ("[config_callback] Setting the output TF frame to: %s.", tf_output_frame_.c_str ());
}
// ]---
}
void
jsk_pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
{
PointCloud2 output;
// Call the virtual method in the child
filter (input, indices, output);
PointCloud2::Ptr cloud_tf (new PointCloud2 (output)); // set the output by default
// Check whether the user has given a different output TF frame
if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
{
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
return;
}
cloud_tf.reset (new PointCloud2 (cloud_transformed));
}
if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
// no tf_output_frame given, transform the dataset to its original frame
{
NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
// Convert the cloud into the different frame
PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
{
NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
return;
}
cloud_tf.reset (new PointCloud2 (cloud_transformed));
}
// Publish a boost shared ptr
pub_output_.publish (cloud_tf);
}
void
pcl_ros::StatisticalOutlierRemoval::config_callback (pcl_ros::StatisticalOutlierRemovalConfig &config, uint32_t level)
{
boost::mutex::scoped_lock lock (mutex_);
if (impl_.getMeanK () != config.mean_k)
{
impl_.setMeanK (config.mean_k);
NODELET_DEBUG ("[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.", getName ().c_str (), config.mean_k);
}
if (impl_.getStddevMulThresh () != config.stddev)
{
impl_.setStddevMulThresh (config.stddev);
NODELET_DEBUG ("[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", getName ().c_str (), config.stddev);
}
if (impl_.getNegative () != config.negative)
{
impl_.setNegative (config.negative);
NODELET_DEBUG ("[%s::config_callback] Returning only inliers: %s.", getName ().c_str (), config.negative ? "false" : "true");
}
}
void
jsk_pcl_ros::Filter::onInit ()
{
// Call the super onInit ()
PCLNodelet::onInit ();
// Call the child's local init
bool has_service = false;
if (!child_init (*pnh_, has_service))
{
NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
return;
}
pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
// Enable the dynamic reconfigure service
if (!has_service)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
srv_->setCallback (f);
}
// If we're supposed to look for PointIndices (indices)
if (use_indices_)
{
// Subscribe to the input using a filter
sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
if (approximate_sync_)
{
sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
}
else
{
sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
}
}
else
// Subscribe in an old fashion to input only (no filters)
sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ()));
NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
}
void callback(const sensor_msgs::ImageConstPtr& imageLeft,
const sensor_msgs::ImageConstPtr& imageRight,
const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
const sensor_msgs::CameraInfoConstPtr& camInfoRight)
{
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();
if(imageLeftPub_.getNumSubscribers())
{
imageLeftPub_.publish(imageLeft);
}
if(imageRightPub_.getNumSubscribers())
{
imageRightPub_.publish(imageRight);
}
if(infoLeftPub_.getNumSubscribers())
{
infoLeftPub_.publish(camInfoLeft);
}
if(infoRightPub_.getNumSubscribers())
{
infoRightPub_.publish(camInfoRight);
}
}
void
pcl_ros::PieceExtraction::config_callback (PieceExtractionConfig &config, uint32_t level)
{
if (impl_.getClusterTolerance () != config.cluster_tolerance)
{
impl_.setClusterTolerance (config.cluster_tolerance);
NODELET_DEBUG ("[%s::config_callback] Setting new clustering tolerance to: %f.", getName ().c_str (), config.cluster_tolerance);
}
if (impl_.getMinClusterSize () != config.cluster_min_size)
{
impl_.setMinClusterSize (config.cluster_min_size);
NODELET_DEBUG ("[%s::config_callback] Setting the minimum cluster size to: %d.", getName ().c_str (), config.cluster_min_size);
}
if (impl_.getMaxClusterSize () != config.cluster_max_size)
{
impl_.setMaxClusterSize (config.cluster_max_size);
NODELET_DEBUG ("[%s::config_callback] Setting the maximum cluster size to: %d.", getName ().c_str (), config.cluster_max_size);
}
if (max_clusters_ != config.max_clusters)
{
max_clusters_ = config.max_clusters;
NODELET_DEBUG ("[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", getName ().c_str (), config.max_clusters);
}
}
void JointStateStaticFilter::filter(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_DEBUG("Pointcloud Callback");
vital_checker_->poke();
if (isStatic(msg->header.stamp)) {
ROS_DEBUG("static");
pub_.publish(msg);
}
else {
ROS_DEBUG("not static");
}
diagnostic_updater_->update();
}
