<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, <NAME>, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: <NAME> */

#include <occupancy_map_monitor/point_cloud_occupancy_map_updater.h>
#include <tf/tf.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <robot_self_filter/self_mask.h>

namespace occupancy_map_monitor
{


PointCloudOccupancyMapUpdater::PointCloudOccupancyMapUpdater(const boost::shared_ptr<tf::Transformer> &tf, const std::string &map_frame)
  : tf_(tf), map_frame_(map_frame)
{
}

PointCloudOccupancyMapUpdater::~PointCloudOccupancyMapUpdater(void)
{
  delete point_cloud_subscriber_;
  delete point_cloud_filter_;
}

bool PointCloudOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  if(!params.hasMember("point_cloud_topic"))
    return false;
  std::string point_cloud_topic = std::string (params["point_cloud_topic"]);

  if(!params.hasMember("max_range"))
    return false;
  double max_range = double (params["max_range"]);

  if(!params.hasMember("frame_subsample"))
    return false;
  size_t frame_subsample = int (params["frame_subsample"]);

  if(!params.hasMember("point_subsample"))
    return false;
  size_t point_subsample = int (params["point_subsample"]);

  std::vector<robot_self_filter::LinkInfo> links;
  if(params.hasMember("self_mask"))
  {
    ROS_INFO("Configuring self mask");
    if(!robot_self_filter::createLinksFromParams(params["self_mask"], links))
    {
      ROS_ERROR("Failed to load self mask");
      return false;
    }
  }

  return this->setParams(point_cloud_topic, max_range, frame_subsample, point_subsample, links);
}

bool PointCloudOccupancyMapUpdater::setParams(const std::string &point_cloud_topic, double max_range,  size_t frame_subsample,
                                              size_t point_subsample, const std::vector<robot_self_filter::LinkInfo> links)
{
  point_cloud_topic_ = point_cloud_topic;
  max_range_ = max_range;
  frame_subsample_ = frame_subsample;
  point_subsample_ = point_subsample;

  self_mask_.reset(new robot_self_filter::SelfMask(*tf_, links));
  return true;
}

void PointCloudOccupancyMapUpdater::initialize()
{
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 1024);
  point_cloud_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_, map_frame_, 1024);
  point_cloud_filter_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
  
  ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str());
}

void PointCloudOccupancyMapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  ROS_DEBUG("Got a point cloud message");
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    last_point_cloud_ = cloud_msg;
  }
  /* tell the monitor that we are ready to update the map */
  notifyUpdateReady();
}

void PointCloudOccupancyMapUpdater::process(const OccMapTreePtr &tree)
{
  ROS_DEBUG("Updating occupancy map with new cloud");
  sensor_msgs::PointCloud2::ConstPtr cloud;
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    cloud = last_point_cloud_;
    last_point_cloud_.reset();
  }
  
  if (cloud)
  {
    processCloud(tree, cloud);
    ROS_DEBUG("Done updating occupancy map");
  }
  else
    ROS_DEBUG("No point cloud to process");
}

void PointCloudOccupancyMapUpdater::processCloud(const OccMapTreePtr &tree, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  /* get transform for cloud into map frame */
  tf::StampedTransform map_H_sensor;
  try
  {
    tf_->lookupTransform(map_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, map_H_sensor);
  }
  catch (tf::TransformException& ex)
  {
    ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
  }

  /* convert cloud message to pcl cloud object */
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*cloud_msg, cloud);

  /* compute sensor origin in map frame */
  tf::Vector3 sensor_origin_tf = map_H_sensor.getOrigin();<commit_msg>robustness fixes<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, <NAME>, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the <NAME> nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: <NAME> */

#include <occupancy_map_monitor/point_cloud_occupancy_map_updater.h>
#include <tf/tf.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <robot_self_filter/self_mask.h>

namespace occupancy_map_monitor
{


PointCloudOccupancyMapUpdater::PointCloudOccupancyMapUpdater(const boost::shared_ptr<tf::Transformer> &tf, const std::string &map_frame)
  : tf_(tf), map_frame_(map_frame),
    point_cloud_subscriber_(NULL),
    point_cloud_filter_(NULL)
{
}

PointCloudOccupancyMapUpdater::~PointCloudOccupancyMapUpdater(void)
{
  delete point_cloud_subscriber_;
  delete point_cloud_filter_;
}

bool PointCloudOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  if(!params.hasMember("point_cloud_topic"))
    return false;
  std::string point_cloud_topic = std::string (params["point_cloud_topic"]);

  if(!params.hasMember("max_range"))
    return false;
  double max_range = double (params["max_range"]);

  if(!params.hasMember("frame_subsample"))
    return false;
  size_t frame_subsample = int (params["frame_subsample"]);

  if(!params.hasMember("point_subsample"))
    return false;
  size_t point_subsample = int (params["point_subsample"]);

  std::vector<robot_self_filter::LinkInfo> links;
  if(params.hasMember("self_mask"))
  {
    ROS_INFO("Configuring self mask");
    if(!robot_self_filter::createLinksFromParams(params["self_mask"], links))
    {
      ROS_ERROR("Failed to load self mask");
      return false;
    }
  }

  return this->setParams(point_cloud_topic, max_range, frame_subsample, point_subsample, links);
}

bool PointCloudOccupancyMapUpdater::setParams(const std::string &point_cloud_topic, double max_range,  size_t frame_subsample,
                                              size_t point_subsample, const std::vector<robot_self_filter::LinkInfo> links)
{
  point_cloud_topic_ = point_cloud_topic;
  max_range_ = max_range;
  frame_subsample_ = frame_subsample;
  point_subsample_ = point_subsample;
  if (tf_)
    self_mask_.reset(new robot_self_filter::SelfMask(*tf_, links));
  return true;
}

void PointCloudOccupancyMapUpdater::initialize()
{
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 1024);
  if (tf_)
  {
    point_cloud_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_, map_frame_, 1024);
    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str());
  }
  else
  {
    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
  }
}

void PointCloudOccupancyMapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  ROS_DEBUG("Got a point cloud message");
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    last_point_cloud_ = cloud_msg;
  }
  /* tell the monitor that we are ready to update the map */
  notifyUpdateReady();
}

void PointCloudOccupancyMapUpdater::process(const OccMapTreePtr &tree)
{
  ROS_DEBUG("Updating occupancy map with new cloud");
  sensor_msgs::PointCloud2::ConstPtr cloud;
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    cloud = last_point_cloud_;
    last_point_cloud_.reset();
  }
  
  if (cloud)
  {
    processCloud(tree, cloud);
    ROS_DEBUG("Done updating occupancy map");
  }
  else
    ROS_DEBUG("No point cloud to process");
}

void PointCloudOccupancyMapUpdater::processCloud(const OccMapTreePtr &tree, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  if (!tf_)
    return;
  
  /* get transform for cloud into map frame */
  tf::StampedTransform map_H_sensor;
  try
  {
    tf_->lookupTransform(map_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, map_H_sensor);
  }
  catch (tf::TransformException& ex)
  {
    ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
  }

  /* convert cloud message to pcl cloud object */
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*cloud_msg, cloud);

  /* compute sensor origin in map frame */
  tf::Vector3 sensor_origin_tf = map_H_sensor.getOrigin();
{'fraction_non_alphanumeric': 0.11924769907963186, 'fraction_numerical': 0.00312124849939976, 'mean_word_length': 5.164775530340404, 'pattern_counts': {'":': 0, '<': 48, '<?xml version=': 0, '>': 53, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5219493', 'n_tokens_mistral': 3878, 'n_tokens_neox': 3740, 'n_words': 982}