<commit_before>#include "plane_calibration/plane_calibration_nodelet.hpp"

#include <sstream>
#include <Eigen/Dense>
#include <ecl/geometry/angle.hpp>

#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <tf2_msgs/TFMessage.h>
#include <image_geometry/pinhole_camera_model.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>

#include "plane_calibration/image_msg_eigen_converter.hpp"

namespace plane_calibration
{

PlaneCalibrationNodelet::PlaneCalibrationNodelet() :
    transform_listener_buffer_(), transform_listener_(transform_listener_buffer_)
{
  debug_ = false;
  ground_plane_rotation_ = Eigen::AngleAxisd::Identity();

  last_valid_calibration_result_ = std::make_pair(0.0, 0.0);
}

void PlaneCalibrationNodelet::onInit()
{
  ROS_INFO("[PlaneCalibrationNodelet]: Initializing");

  reconfigure_server_ = std::shared_ptr<dynamic_reconfigure::Server<PlaneCalibrationConfig> >(
      new dynamic_reconfigure::Server<PlaneCalibrationConfig>());
  dynamic_reconfigure::Server<PlaneCalibrationConfig>::CallbackType reconfigure_cb = boost::bind(
      &PlaneCalibrationNodelet::reconfigureCB, this, _1, _2);
  reconfigure_server_->setCallback(reconfigure_cb);

  ros::NodeHandle node_handle = this->getPrivateNodeHandle();
  depth_visualizer_ = std::make_shared<DepthVisualizer>(node_handle);

  pub_candidate_points_ = node_handle.advertise<sensor_msgs::Image>("candidates", 1);
  pub_plane_points_ = node_handle.advertise<sensor_msgs::Image>("plane_points", 1);
  pub_transform_ = node_handle.advertise<tf2_msgs::TFMessage>("adjusted_tf", 1);

  sub_camera_info_ = node_handle.subscribe<sensor_msgs::CameraInfo>("camera_info", 1,
                                                                    &PlaneCalibrationNodelet::cameraInfoCB, this);
  sub_depth_image_ = node_handle.subscribe<sensor_msgs::Image>("input_depth_image", 1,
                                                               &PlaneCalibrationNodelet::depthImageCB, this);

  ROS_INFO("[PlaneCalibrationNodelet]: Initialization finished");
}

void PlaneCalibrationNodelet::reconfigureCB(PlaneCalibrationConfig &config, uint32_t level)
{
  if (debug_ != config.debug)
  {
    debug_ = config.debug;
    std::string debug_status = debug_ ? "enabled" : "disabled";
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Debug " << debug_status);
  }

  x_offset_ = config.x;
  y_offset_ = config.y;
  z_offset_ = config.z;

  px_offset_ = ecl::degrees_to_radians(config.px_degree);
  py_offset_ = ecl::degrees_to_radians(config.py_degree);
  pz_offset_ = ecl::degrees_to_radians(config.pz_degree);

  max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);
  iterations_ = config.iterations;

  input_filter_config_.max_nan_ratio = config.input_max_nan_ratio;
  input_filter_config_.max_zero_ratio = config.input_max_zero_ratio;
  input_filter_config_.min_data_ratio = config.input_min_data_ratio;
  input_filter_config_.max_error = config.input_max_noise;
  input_filter_config_.threshold_from_ground = config.input_threshold_from_ground;

  if (input_filter_)
  {
    input_filter_->updateConfig(input_filter_config_);
  }

  Eigen::Vector3d ground_plane_offset;
  Eigen::AngleAxisd rotation = ground_plane_rotation_;
  if (config.use_manual_ground_transform)
  {
    ground_plane_offset = Eigen::Vector3d(x_offset_, y_offset_, z_offset_);
    rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());
  }

  if (!calibration_parameters_)
  {
    calibration_parameters_ = std::make_shared<CalibrationParameters>();
  }

  calibration_parameters_->update(ground_plane_offset, ecl::degrees_to_radians(config.max_deviation_degrees), rotation);

  calibration_validation_config_.max_too_low_ratio = config.plane_max_too_low_ratio;
  calibration_validation_config_.max_mean = config.plane_max_mean;
  calibration_validation_config_.max_deviation = config.plane_max_deviation;

  if (calibration_validation_)
  {
    calibration_validation_->updateConfig(calibration_validation_config_);
  }
}

void PlaneCalibrationNodelet::cameraInfoCB(const sensor_msgs::CameraInfoConstPtr& camera_info_msg)
{
  image_geometry::PinholeCameraModel pinhole_camera_model;
  pinhole_camera_model.fromCameraInfo(camera_info_msg);

  depth_visualizer_->setCameraModel(pinhole_camera_model);

  if (!camera_model_)
  {
    camera_model_ = std::make_shared<CameraModel>();
  }

  camera_model_->update(pinhole_camera_model.cx(), pinhole_camera_model.cy(), pinhole_camera_model.fx(),
                        pinhole_camera_model.fy(), camera_info_msg->width, camera_info_msg->height);
}

void PlaneCalibrationNodelet::depthImageCB(const sensor_msgs::ImageConstPtr& depth_image_msg)
{
  bool wait_for_initialization = !camera_model_ || !calibration_parameters_;
  if (wait_for_initialization)
  {
    return;
  }

  if (!plane_calibration_)
  {
    plane_calibration_ = std::make_shared<PlaneCalibration>(*camera_model_, calibration_parameters_, depth_visualizer_);
  }

  if (!input_filter_)
  {
    input_filter_ = std::make_shared<InputFilter>(*camera_model_, calibration_parameters_, depth_visualizer_,
                                                  input_filter_config_);
  }

  if (!calibration_validation_)
  {
    calibration_validation_ = std::make_shared<CalibrationValidation>(*camera_model_, calibration_parameters_,
                                                                      calibration_validation_config_);
  }

  if (!plane_to_depth_converter_)
  {
    plane_to_depth_converter_ = std::make_shared<PlaneToDepthImage>(camera_model_->getParameters());
  }

  Eigen::MatrixXf depth_matrix;

  bool converted_successfully = ImageMsgEigenConverter::convert(depth_image_msg, depth_matrix);
  if (!converted_successfully)
  {
    ROS_ERROR_STREAM("[PlaneCalibrationNodelet]: Conversion from image msg to Eigen matrix failed");
    return;
  }

  getTransform();

  if (!transform_)
  {
    return;
  }

  runCalibration(depth_matrix);
  publishTransform();
}

void PlaneCalibrationNodelet::getTransform()
{
  geometry_msgs::TransformStamped transformStamped;
  try
  {
    transformStamped = transform_listener_buffer_.lookupTransform("base_footprint", "sensor_3d_short_range_depth_optical_frame",
                                                                  ros::Time(0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }

  if (!transform_)
  {
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Got transform to ground, will do calibration from now");
  }

  transform_ = std::make_shared<geometry_msgs::TransformStamped>(transformStamped);
}

void PlaneCalibrationNodelet::runCalibration(Eigen::MatrixXf depth_matrix)
{
  if (debug_)
  {
    static tf2_ros::TransformBroadcaster transform_broadcaster;
    geometry_msgs::TransformStamped transformStamped;

    transformStamped.header.stamp = ros::Time::now();
    std::string frame_id = "camera_depth_optical_frame";
    transformStamped.header.frame_id = frame_id;

    CalibrationParameters::Parameters parameters = calibration_parameters_->getParameters();

    transformStamped.child_frame_id = "uncalibrated_ground";
    tf::transformEigenToMsg(parameters.getTransform(), transformStamped.transform);
    transform_broadcaster.sendTransform(transformStamped);

    depth_visualizer_->publishCloud("debug/uncalibrated_ground", parameters.getTransform(),
                                    camera_model_->getParameters(), frame_id);
  }

  input_filter_->filter(depth_matrix, debug_);

  bool input_data_not_usable = !input_filter_->dataIsUsable(depth_matrix, debug_);
  if (input_data_not_usable)
  {
    if (debug_)
    {
      ROS_WARN_STREAM("[PlaneCalibrationNodelet]: Input data not usable, not going to calibrate");
    }
    return;
  }

  if (last_valid_calibration_result_plane_.size() != 0)
  {
    bool parameters_updated = calibration_parameters_->parametersUpdated();
    bool last_calibration_is_still_good = calibration_validation_->groundPlaneFitsData(
        last_valid_calibration_result_plane_, depth_matrix);
    if (!parameters_updated && last_calibration_is_still_good)
    {
      if (debug_)
      {
        ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Last calibration data still works, not going to calibrate");
      }
      return;
    }
  }

  CalibrationParameters::Parameters parameters = calibration_parameters_->getParameters();
  std::pair<double, double> calibration_result = plane_calibration_->calibrate(depth_matrix, iterations_);

  if (debug_)
  {
    ROS_INFO_STREAM(
        "[PlaneCalibrationNodelet]: Calibration result angles: " << calibration_result.first << ", " << calibration_result.second);

    Eigen::AngleAxisd rotation;
    rotation = parameters.rotation_ * Eigen::AngleAxisd(calibration_result.first, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(calibration_result.second, Eigen::Vector3d::UnitY());

    Eigen::Affine3d transform = Eigen::Translation3d(parameters.ground_plane_offset_) * rotation;

    std::string frame_id = "camera_depth_optical_frame";
    depth_visualizer_->publishCloud("debug/calibration_result", transform, camera_model_->getParameters(), frame_id);
  }

  //  std::cout << "offset angles: " << ecl::radians_to_degrees(one_shot_result.first) << ", "
  //      << ecl::radians_to_degrees(one_shot_result.second) << std::endl;
  //  std::cout << "original angles: " << ecl::radians_to_degrees(px_offset_.load()) << ", "
  //      << ecl::radians_to_degrees(py_offset_.load()) << std::endl;

  bool valid_calibration_angles = calibration_validation_->angleOffsetValid(calibration_result);
  if (!valid_calibration_angles)
  {
    if (debug_)
    {
      ROS_WARN_STREAM(
          "[PlaneCalibrationNodelet]: Calibration turned out bad, too big angles ( > " << parameters.max_deviation_ << "):");
      ROS_WARN_STREAM(
          "[PlaneCalibrationNodelet]: x angle: " << calibration_result.first << ", y angle: " << calibration_result.second);
    }
    //keep old / last one
    return;
  }

  Eigen::AngleAxisd rotation;
  rotation = parameters.rotation_ * Eigen::AngleAxisd(calibration_result.first, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(calibration_result.second, Eigen::Vector3d::UnitY());

  Eigen::Affine3d transform = Eigen::Translation3d(parameters.ground_plane_offset_) * rotation;
  Eigen::MatrixXf new_ground_plane = plane_to_depth_converter_->convert(transform);

  bool good_calibration = calibration_validation_->groundPlaneFitsData(new_ground_plane, depth_matrix);
  if (!good_calibration)
  {
    if (debug_)
    {
      ROS_WARN_STREAM("[PlaneCalibrationNodelet]: Calibration turned out bad: Data does not fit good enough");
    }
    //keep old / last one
    return;
  }

  std::stringstream angle_change_string;
  angle_change_string << "px: " << last_valid_calibration_result_.first << " -> " << calibration_result.first;
  angle_change_string << ", ";
  angle_change_string << "py: " << last_valid_calibration_result_.second << " -> " << calibration_result.second;

  ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Updated the calibration angles: " << angle_change_string.str());

  last_valid_calibration_result_ = calibration_result;
  last_valid_calibration_result_plane_ = new_ground_plane;
  last_valid_calibration_transformation_ = transform;
}

void PlaneCalibrationNodelet::publishTransform()
{
  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  std::string frame_id = "camera_depth_optical_frame";
  transformStamped.header.frame_id = frame_id;

  transformStamped.child_frame_id = "calibrated_plane";
  tf::transformEigenToMsg(last_valid_calibration_transformation_, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  if (debug_)
  {
    depth_visualizer_->publishCloud("debug/calibrated_plane", last_valid_calibration_transformation_,
                                    camera_model_->getParameters(), frame_id);
  }
}

} /* end namespace */

PLUGINLIB_EXPORT_CLASS(plane_calibration::PlaneCalibrationNodelet, nodelet::Nodelet)
<commit_msg>Nodelet: Log degrees instead of radians<commit_after>#include "plane_calibration/plane_calibration_nodelet.hpp"

#include <sstream>
#include <Eigen/Dense>
#include <ecl/geometry/angle.hpp>

#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>
#include <tf2_msgs/TFMessage.h>
#include <image_geometry/pinhole_camera_model.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>

#include "plane_calibration/image_msg_eigen_converter.hpp"

namespace plane_calibration
{

PlaneCalibrationNodelet::PlaneCalibrationNodelet() :
    transform_listener_buffer_(), transform_listener_(transform_listener_buffer_)
{
  debug_ = false;
  ground_plane_rotation_ = Eigen::AngleAxisd::Identity();

  last_valid_calibration_result_ = std::make_pair(0.0, 0.0);
}

void PlaneCalibrationNodelet::onInit()
{
  ROS_INFO("[PlaneCalibrationNodelet]: Initializing");

  reconfigure_server_ = std::shared_ptr<dynamic_reconfigure::Server<PlaneCalibrationConfig> >(
      new dynamic_reconfigure::Server<PlaneCalibrationConfig>());
  dynamic_reconfigure::Server<PlaneCalibrationConfig>::CallbackType reconfigure_cb = boost::bind(
      &PlaneCalibrationNodelet::reconfigureCB, this, _1, _2);
  reconfigure_server_->setCallback(reconfigure_cb);

  ros::NodeHandle node_handle = this->getPrivateNodeHandle();
  depth_visualizer_ = std::make_shared<DepthVisualizer>(node_handle);

  pub_candidate_points_ = node_handle.advertise<sensor_msgs::Image>("candidates", 1);
  pub_plane_points_ = node_handle.advertise<sensor_msgs::Image>("plane_points", 1);
  pub_transform_ = node_handle.advertise<tf2_msgs::TFMessage>("adjusted_tf", 1);

  sub_camera_info_ = node_handle.subscribe<sensor_msgs::CameraInfo>("camera_info", 1,
                                                                    &PlaneCalibrationNodelet::cameraInfoCB, this);
  sub_depth_image_ = node_handle.subscribe<sensor_msgs::Image>("input_depth_image", 1,
                                                               &PlaneCalibrationNodelet::depthImageCB, this);

  ROS_INFO("[PlaneCalibrationNodelet]: Initialization finished");
}

void PlaneCalibrationNodelet::reconfigureCB(PlaneCalibrationConfig &config, uint32_t level)
{
  if (debug_ != config.debug)
  {
    debug_ = config.debug;
    std::string debug_status = debug_ ? "enabled" : "disabled";
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Debug " << debug_status);
  }

  x_offset_ = config.x;
  y_offset_ = config.y;
  z_offset_ = config.z;

  px_offset_ = ecl::degrees_to_radians(config.px_degree);
  py_offset_ = ecl::degrees_to_radians(config.py_degree);
  pz_offset_ = ecl::degrees_to_radians(config.pz_degree);

  max_deviation_ = ecl::degrees_to_radians(config.max_deviation_degrees);
  iterations_ = config.iterations;

  input_filter_config_.max_nan_ratio = config.input_max_nan_ratio;
  input_filter_config_.max_zero_ratio = config.input_max_zero_ratio;
  input_filter_config_.min_data_ratio = config.input_min_data_ratio;
  input_filter_config_.max_error = config.input_max_noise;
  input_filter_config_.threshold_from_ground = config.input_threshold_from_ground;

  if (input_filter_)
  {
    input_filter_->updateConfig(input_filter_config_);
  }

  Eigen::Vector3d ground_plane_offset;
  Eigen::AngleAxisd rotation = ground_plane_rotation_;
  if (config.use_manual_ground_transform)
  {
    ground_plane_offset = Eigen::Vector3d(x_offset_, y_offset_, z_offset_);
    rotation = Eigen::AngleAxisd(px_offset_, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(py_offset_, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(pz_offset_, Eigen::Vector3d::UnitZ());
  }

  if (!calibration_parameters_)
  {
    calibration_parameters_ = std::make_shared<CalibrationParameters>();
  }

  calibration_parameters_->update(ground_plane_offset, ecl::degrees_to_radians(config.max_deviation_degrees), rotation);

  calibration_validation_config_.max_too_low_ratio = config.plane_max_too_low_ratio;
  calibration_validation_config_.max_mean = config.plane_max_mean;
  calibration_validation_config_.max_deviation = config.plane_max_deviation;

  if (calibration_validation_)
  {
    calibration_validation_->updateConfig(calibration_validation_config_);
  }
}

void PlaneCalibrationNodelet::cameraInfoCB(const sensor_msgs::CameraInfoConstPtr& camera_info_msg)
{
  image_geometry::PinholeCameraModel pinhole_camera_model;
  pinhole_camera_model.fromCameraInfo(camera_info_msg);

  depth_visualizer_->setCameraModel(pinhole_camera_model);

  if (!camera_model_)
  {
    camera_model_ = std::make_shared<CameraModel>();
  }

  camera_model_->update(pinhole_camera_model.cx(), pinhole_camera_model.cy(), pinhole_camera_model.fx(),
                        pinhole_camera_model.fy(), camera_info_msg->width, camera_info_msg->height);
}

void PlaneCalibrationNodelet::depthImageCB(const sensor_msgs::ImageConstPtr& depth_image_msg)
{
  bool wait_for_initialization = !camera_model_ || !calibration_parameters_;
  if (wait_for_initialization)
  {
    return;
  }

  if (!plane_calibration_)
  {
    plane_calibration_ = std::make_shared<PlaneCalibration>(*camera_model_, calibration_parameters_, depth_visualizer_);
  }

  if (!input_filter_)
  {
    input_filter_ = std::make_shared<InputFilter>(*camera_model_, calibration_parameters_, depth_visualizer_,
                                                  input_filter_config_);
  }

  if (!calibration_validation_)
  {
    calibration_validation_ = std::make_shared<CalibrationValidation>(*camera_model_, calibration_parameters_,
                                                                      calibration_validation_config_);
  }

  if (!plane_to_depth_converter_)
  {
    plane_to_depth_converter_ = std::make_shared<PlaneToDepthImage>(camera_model_->getParameters());
  }

  Eigen::MatrixXf depth_matrix;

  bool converted_successfully = ImageMsgEigenConverter::convert(depth_image_msg, depth_matrix);
  if (!converted_successfully)
  {
    ROS_ERROR_STREAM("[PlaneCalibrationNodelet]: Conversion from image msg to Eigen matrix failed");
    return;
  }

  getTransform();

  if (!transform_)
  {
    return;
  }

  runCalibration(depth_matrix);
  publishTransform();
}

void PlaneCalibrationNodelet::getTransform()
{
  geometry_msgs::TransformStamped transformStamped;
  try
  {
    transformStamped = transform_listener_buffer_.lookupTransform("base_footprint",
                                                                  "sensor_3d_short_range_depth_optical_frame",
                                                                  ros::Time(0));
  }
  catch (tf2::TransformException &ex)
  {
    ROS_WARN("%s", ex.what());
    return;
  }

  if (!transform_)
  {
    ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Got transform to ground, will do calibration from now");
  }

  transform_ = std::make_shared<geometry_msgs::TransformStamped>(transformStamped);
}

void PlaneCalibrationNodelet::runCalibration(Eigen::MatrixXf depth_matrix)
{
  if (debug_)
  {
    static tf2_ros::TransformBroadcaster transform_broadcaster;
    geometry_msgs::TransformStamped transformStamped;

    transformStamped.header.stamp = ros::Time::now();
    std::string frame_id = "camera_depth_optical_frame";
    transformStamped.header.frame_id = frame_id;

    CalibrationParameters::Parameters parameters = calibration_parameters_->getParameters();

    transformStamped.child_frame_id = "uncalibrated_ground";
    tf::transformEigenToMsg(parameters.getTransform(), transformStamped.transform);
    transform_broadcaster.sendTransform(transformStamped);

    depth_visualizer_->publishCloud("debug/uncalibrated_ground", parameters.getTransform(),
                                    camera_model_->getParameters(), frame_id);
  }

  input_filter_->filter(depth_matrix, debug_);

  bool input_data_not_usable = !input_filter_->dataIsUsable(depth_matrix, debug_);
  if (input_data_not_usable)
  {
    if (debug_)
    {
      ROS_WARN_STREAM("[PlaneCalibrationNodelet]: Input data not usable, not going to calibrate");
    }
    return;
  }

  if (last_valid_calibration_result_plane_.size() != 0)
  {
    bool parameters_updated = calibration_parameters_->parametersUpdated();
    bool last_calibration_is_still_good = calibration_validation_->groundPlaneFitsData(
        last_valid_calibration_result_plane_, depth_matrix);
    if (!parameters_updated && last_calibration_is_still_good)
    {
      if (debug_)
      {
        ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Last calibration data still works, not going to calibrate");
      }
      return;
    }
  }

  CalibrationParameters::Parameters parameters = calibration_parameters_->getParameters();
  std::pair<double, double> calibration_result = plane_calibration_->calibrate(depth_matrix, iterations_);

  if (debug_)
  {
    ROS_INFO_STREAM(
        "[PlaneCalibrationNodelet]: Calibration result angles [degree]: " << ecl::radians_to_degrees(calibration_result.first) << ", " << calibration_result.second);

    Eigen::AngleAxisd rotation;
    rotation = parameters.rotation_ * Eigen::AngleAxisd(calibration_result.first, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(calibration_result.second, Eigen::Vector3d::UnitY());

    Eigen::Affine3d transform = Eigen::Translation3d(parameters.ground_plane_offset_) * rotation;

    std::string frame_id = "camera_depth_optical_frame";
    depth_visualizer_->publishCloud("debug/calibration_result", transform, camera_model_->getParameters(), frame_id);
  }

  //  std::cout << "offset angles: " << ecl::radians_to_degrees(one_shot_result.first) << ", "
  //      << ecl::radians_to_degrees(one_shot_result.second) << std::endl;
  //  std::cout << "original angles: " << ecl::radians_to_degrees(px_offset_.load()) << ", "
  //      << ecl::radians_to_degrees(py_offset_.load()) << std::endl;

  bool valid_calibration_angles = calibration_validation_->angleOffsetValid(calibration_result);
  if (!valid_calibration_angles)
  {
    if (debug_)
    {
      ROS_WARN_STREAM(
          "[PlaneCalibrationNodelet]: Calibration turned out bad, too big angles ( > " << ecl::radians_to_degrees(parameters.max_deviation_) << " [degree]):");
      ROS_WARN_STREAM(
          "[PlaneCalibrationNodelet]: x,y angles [degree]: " << ecl::radians_to_degrees(calibration_result.first) << ", " << ecl::radians_to_degrees(calibration_result.second));
    }
    //keep old / last one
    return;
  }

  Eigen::AngleAxisd rotation;
  rotation = parameters.rotation_ * Eigen::AngleAxisd(calibration_result.first, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(calibration_result.second, Eigen::Vector3d::UnitY());

  Eigen::Affine3d transform = Eigen::Translation3d(parameters.ground_plane_offset_) * rotation;
  Eigen::MatrixXf new_ground_plane = plane_to_depth_converter_->convert(transform);

  bool good_calibration = calibration_validation_->groundPlaneFitsData(new_ground_plane, depth_matrix);
  if (!good_calibration)
  {
    if (debug_)
    {
      ROS_WARN_STREAM("[PlaneCalibrationNodelet]: Calibration turned out bad: Data does not fit good enough");
    }
    //keep old / last one
    return;
  }

  std::stringstream angle_change_string;
  angle_change_string << "px [degree]: " << ecl::radians_to_degrees(last_valid_calibration_result_.first) << " -> "
      << ecl::radians_to_degrees(calibration_result.first);
  angle_change_string << ", ";
  angle_change_string << "py [degree]: " << ecl::radians_to_degrees(last_valid_calibration_result_.second) << " -> "
      << ecl::radians_to_degrees(calibration_result.second);

  ROS_INFO_STREAM("[PlaneCalibrationNodelet]: Updated the calibration angles: " << angle_change_string.str());

  last_valid_calibration_result_ = calibration_result;
  last_valid_calibration_result_plane_ = new_ground_plane;
  last_valid_calibration_transformation_ = transform;
}

void PlaneCalibrationNodelet::publishTransform()
{
  static tf2_ros::TransformBroadcaster transform_broadcaster;
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  std::string frame_id = "camera_depth_optical_frame";
  transformStamped.header.frame_id = frame_id;

  transformStamped.child_frame_id = "calibrated_plane";
  tf::transformEigenToMsg(last_valid_calibration_transformation_, transformStamped.transform);
  transform_broadcaster.sendTransform(transformStamped);

  if (debug_)
  {
    depth_visualizer_->publishCloud("debug/calibrated_plane", last_valid_calibration_transformation_,
                                    camera_model_->getParameters(), frame_id);
  }
}

} /* end namespace */

PLUGINLIB_EXPORT_CLASS(plane_calibration::PlaneCalibrationNodelet, nodelet::Nodelet)
<|endoftext|>


<commit_before>//===- CodeEmitterGen.cpp - Code Emitter Generator ------------------------===//
//
// FIXME: Document.
//
//===----------------------------------------------------------------------===//

#include "CodeEmitterGen.h"
#include "Record.h"
#include "Support/Statistic.h"

bool CodeEmitterGen::run(std::ostream &o) {
  std::vector<Record*> Insts;

  const std::map<std::string, Record*> &Defs = Records.getDefs();

  Records.getAllDerivedDefinitions("Instruction", Insts);

  std::string Namespace = "V9::";
  std::string ClassName = "SparcV9CodeEmitter::";

  //const std::string &Namespace = Inst->getValue("Namespace")->getName();
  o << "unsigned " << ClassName
    << "getBinaryCodeForInstr(MachineInstr &MI) {\n"
    << "  unsigned Value = 0;\n"
    << "  DEBUG(std::cerr << MI);\n"
    << "  switch (MI.getOpcode()) {\n";
  for (std::vector<Record*>::iterator I = Insts.begin(), E = Insts.end();
       I != E; ++I)
  {
    Record *R = *I;
    o << "    case " << Namespace << R->getName() << ": {\n"
      << "      DEBUG(std::cerr << \"Emitting " << R->getName() << "\\n\");\n";

    const RecordVal *InstVal = R->getValue("Inst");
    if (!InstVal) {
      std::cerr << "No 'Inst' record found in target description file!\n";
      return true;
    }

    Init *InitVal = InstVal->getValue();
    assert(dynamic_cast<BitsInit*>(InitVal) &&
           "Can only handle undefined bits<> types!");
    BitsInit *BI = (BitsInit*)InitVal;

    unsigned Value = 0;
    const std::vector<RecordVal> &Vals = R->getValues();

    DEBUG(o << "      // prefilling: ");
    // Start by filling in fixed values...
    for (unsigned i = 0, e = BI->getNumBits(); i != e; ++i) {
      if (BitInit *B = dynamic_cast<BitInit*>(BI->getBit(e-i-1))) {
        Value |= B->getValue() << (e-i-1);
        DEBUG(o << B->getValue());
      } else {
        DEBUG(o << "0");
      }
    }
    DEBUG(o << "\n");

    DEBUG(o << "      // " << *InstVal << "\n");
    o << "      Value = " << Value << "U;\n\n";

    // Loop over all of the fields in the instruction adding in any
    // contributions to this value (due to bit references).
    //
    unsigned op = 0;
    std::map<const std::string,unsigned> OpOrder;
    std::map<const std::string,bool> OpContinuous;
    for (unsigned i = 0, e = Vals.size(); i != e; ++i) {
      if (Vals[i].getName() != "Inst" &&
          !Vals[i].getValue()->isComplete() &&
          /* ignore annul and predict bits since no one sets them yet */
          Vals[i].getName() != "annul" &&
          Vals[i].getName() != "predict")
      {
        o << "      // op" << op << ": " << Vals[i].getName() << "\n"
          << "      int64_t op" << op
          <<" = getMachineOpValue(MI, MI.getOperand("<<op<<"));\n";
        //<< "      MachineOperand &op" << op <<" = MI.getOperand("<<op<<");\n";
        OpOrder[Vals[i].getName()] = op++;

        // Is the operand continuous? If so, we can just mask and OR it in
        // instead of doing it bit-by-bit, saving a lot in runtime cost.
        const BitsInit *InstInit = BI;
        int beginBitInVar = -1, endBitInVar = -1,
          beginBitInInst = -1, endBitInInst = -1;
        bool continuous = true;

        for (int bit = InstInit->getNumBits()-1; bit >= 0; --bit) {
          if (VarBitInit *VBI =
              dynamic_cast<VarBitInit*>(InstInit->getBit(bit))) {
            TypedInit *TI = VBI->getVariable();
            if (VarInit *VI = dynamic_cast<VarInit*>(TI)) {
              // only process the current variable
              if (VI->getName() != Vals[i].getName())
                continue;

              if (beginBitInVar == -1)
                beginBitInVar = VBI->getBitNum();

              if (endBitInVar == -1)
                endBitInVar = VBI->getBitNum();
              else {
                if (endBitInVar == (int)VBI->getBitNum() + 1)
                  endBitInVar = VBI->getBitNum();
                else {
                  continuous = false;
                  break;
                }
              }

              if (beginBitInInst == -1)
                beginBitInInst = bit;
              if (endBitInInst == -1)
                endBitInInst = bit;
              else {
                if (endBitInInst == bit + 1)
                  endBitInInst = bit;
                else {
                  continuous = false;
                  break;
                }
              }

              // maintain same distance between bits in field and bits in
              // instruction. if the relative distances stay the same
              // throughout,
              if ((beginBitInVar - (int)VBI->getBitNum()) !=
                  (beginBitInInst - bit))
              {
                continuous = false;
                break;
              }
            }
          }
        }

        DEBUG(o << "      // Var: begin = " << beginBitInVar
                << ", end = " << endBitInVar
                << "; Inst: begin = " << beginBitInInst
                << ", end = " << endBitInInst << "\n");

        if (continuous) {
          DEBUG(o << "      // continuous: op" << OpOrder[Vals[i].getName()]
                  << "\n");

          // Mask off the right bits
          // Low mask (ie. shift, if necessary)
          if (endBitInVar != 0) {
            o << "      op" << OpOrder[Vals[i].getName()]
              << " >>= " << endBitInVar << ";\n";
            beginBitInVar -= endBitInVar;
            endBitInVar = 0;
          }

          // High mask
          o << "      op" << OpOrder[Vals[i].getName()]
            << " &= (1<<" << beginBitInVar+1 << ") - 1;\n";

          // Shift the value to the correct place (according to place in instr)
          if (endBitInInst != 0)
            o << "      op" << OpOrder[Vals[i].getName()]
              << " <<= " << endBitInInst << ";\n";

          // Just OR in the result
          o << "      Value |= op" << OpOrder[Vals[i].getName()] << ";\n";
        }

        // otherwise, will be taken care of in the loop below using this value:
        OpContinuous[Vals[i].getName()] = continuous;
      }
    }

    for (unsigned f = 0, e = Vals.size(); f != e; ++f) {
      if (Vals[f].getPrefix()) {
        BitsInit *FieldInitializer = (BitsInit*)Vals[f].getValue();

        // Scan through the field looking for bit initializers of the current
        // variable...
        for (int i = FieldInitializer->getNumBits()-1; i >= 0; --i) {
          if (BitInit *BI=dynamic_cast<BitInit*>(FieldInitializer->getBit(i)))
          {
            DEBUG(o << "      // bit init: f: " << f << ", i: " << i << "\n");
          } else if (UnsetInit *UI =
                     dynamic_cast<UnsetInit*>(FieldInitializer->getBit(i))) {
            DEBUG(o << "      // unset init: f: " << f << ", i: " << i << "\n");
          } else if (VarBitInit *VBI =
                     dynamic_cast<VarBitInit*>(FieldInitializer->getBit(i))) {
            TypedInit *TI = VBI->getVariable();
            if (VarInit *VI = dynamic_cast<VarInit*>(TI)) {
              // If the bits of the field are laid out consecutively in the
              // instruction, then instead of separately ORing in bits, just
              // mask and shift the entire field for efficiency.
              if (OpContinuous[VI->getName()]) {
                // already taken care of in the loop above, thus there is no
                // need to individually OR in the bits

                // for debugging, output the regular version anyway, commented
                DEBUG(o << "      // Value |= getValueBit(op"
                        << OpOrder[VI->getName()] << ", " << VBI->getBitNum()
                        << ")" << " << " << i << ";\n");
              } else {
                o << "      Value |= getValueBit(op" << OpOrder[VI->getName()]
                  << ", " << VBI->getBitNum()
                  << ")" << " << " << i << ";\n";
              }
            } else if (FieldInit *FI = dynamic_cast<FieldInit*>(TI)) {
              // FIXME: implement this!
              o << "FIELD INIT not implemented yet!\n";
            } else {
              o << "Error: UNIMPLEMENTED\n";
            }
          }
        }
      } else {
        // ignore annul and predict bits since no one sets them yet
        if (Vals[f].getName() == "annul" || Vals[f].getName() == "predict") {
          o << "      // found " << Vals[f].getName() << "\n";
        }
      }
    }

    o << "      break;\n"
      << "    }\n";
  }

  o << "  default:\n"
    << "    DEBUG(std::cerr << \"Not supported instr: \" << MI << \"\\n\");\n"
    << "    abort();\n"
    << "  }\n"
    << "  return Value;\n"
    << "}\n";
  return false;
}
<commit_msg>Minor cleanups<commit_after>//===- CodeEmitterGen.cpp - Code Emitter Generator ------------------------===//
//
// FIXME: Document.
//
//===----------------------------------------------------------------------===//

#include "CodeEmitterGen.h"
#include "Record.h"
#include "Support/Statistic.h"

bool CodeEmitterGen::run(std::ostream &o) {
  std::vector<Record*> Insts;
  if (Records.getAllDerivedDefinitions("Instruction", Insts)) return true;

  std::string Namespace = "V9::";
  std::string ClassName = "SparcV9CodeEmitter::";

  //const std::string &Namespace = Inst->getValue("Namespace")->getName();
  o << "unsigned " << ClassName
    << "getBinaryCodeForInstr(MachineInstr &MI) {\n"
    << "  unsigned Value = 0;\n"
    << "  DEBUG(std::cerr << MI);\n"
    << "  switch (MI.getOpcode()) {\n";
  for (std::vector<Record*>::iterator I = Insts.begin(), E = Insts.end();
       I != E; ++I) {
    Record *R = *I;
    o << "    case " << Namespace << R->getName() << ": {\n"
      << "      DEBUG(std::cerr << \"Emitting " << R->getName() << "\\n\");\n";

    const RecordVal *InstVal = R->getValue("Inst");
    if (!InstVal) {
      std::cerr << "No 'Inst' record found in target description file!\n";
      return true;
    }

    Init *InitVal = InstVal->getValue();
    assert(dynamic_cast<BitsInit*>(InitVal) &&
           "Can only handle undefined bits<> types!");
    BitsInit *BI = (BitsInit*)InitVal;

    unsigned Value = 0;
    const std::vector<RecordVal> &Vals = R->getValues();

    DEBUG(o << "      // prefilling: ");
    // Start by filling in fixed values...
    for (unsigned i = 0, e = BI->getNumBits(); i != e; ++i) {
      if (BitInit *B = dynamic_cast<BitInit*>(BI->getBit(e-i-1))) {
        Value |= B->getValue() << (e-i-1);
        DEBUG(o << B->getValue());
      } else {
        DEBUG(o << "0");
      }
    }
    DEBUG(o << "\n");

    DEBUG(o << "      // " << *InstVal << "\n");
    o << "      Value = " << Value << "U;\n\n";

    // Loop over all of the fields in the instruction adding in any
    // contributions to this value (due to bit references).
    //
    unsigned op = 0;
    std::map<const std::string,unsigned> OpOrder;
    std::map<const std::string,bool> OpContinuous;
    for (unsigned i = 0, e = Vals.size(); i != e; ++i) {
      if (Vals[i].getName() != "Inst" &&
          !Vals[i].getValue()->isComplete() &&
          /* ignore annul and predict bits since no one sets them yet */
          Vals[i].getName() != "annul" &&
          Vals[i].getName() != "predict")
      {
        o << "      // op" << op << ": " << Vals[i].getName() << "\n"
          << "      int64_t op" << op
          <<" = getMachineOpValue(MI, MI.getOperand("<<op<<"));\n";
        //<< "      MachineOperand &op" << op <<" = MI.getOperand("<<op<<");\n";
        OpOrder[Vals[i].getName()] = op++;

        // Is the operand continuous? If so, we can just mask and OR it in
        // instead of doing it bit-by-bit, saving a lot in runtime cost.
        const BitsInit *InstInit = BI;
        int beginBitInVar = -1, endBitInVar = -1,
          beginBitInInst = -1, endBitInInst = -1;
        bool continuous = true;

        for (int bit = InstInit->getNumBits()-1; bit >= 0; --bit) {
          if (VarBitInit *VBI =
              dynamic_cast<VarBitInit*>(InstInit->getBit(bit))) {
            TypedInit *TI = VBI->getVariable();
            if (VarInit *VI = dynamic_cast<VarInit*>(TI)) {
              // only process the current variable
              if (VI->getName() != Vals[i].getName())
                continue;

              if (beginBitInVar == -1)
                beginBitInVar = VBI->getBitNum();

              if (endBitInVar == -1)
                endBitInVar = VBI->getBitNum();
              else {
                if (endBitInVar == (int)VBI->getBitNum() + 1)
                  endBitInVar = VBI->getBitNum();
                else {
                  continuous = false;
                  break;
                }
              }

              if (beginBitInInst == -1)
                beginBitInInst = bit;
              if (endBitInInst == -1)
                endBitInInst = bit;
              else {
                if (endBitInInst == bit + 1)
                  endBitInInst = bit;
                else {
                  continuous = false;
                  break;
                }
              }

              // maintain same distance between bits in field and bits in
              // instruction. if the relative distances stay the same
              // throughout,
              if ((beginBitInVar - (int)VBI->getBitNum()) !=
                  (beginBitInInst - bit))
              {
                continuous = false;
                break;
              }
            }
          }
        }

        DEBUG(o << "      // Var: begin = " << beginBitInVar
                << ", end = " << endBitInVar
                << "; Inst: begin = " << beginBitInInst
                << ", end = " << endBitInInst << "\n");

        if (continuous) {
          DEBUG(o << "      // continuous: op" << OpOrder[Vals[i].getName()]
                  << "\n");

          // Mask off the right bits
          // Low mask (ie. shift, if necessary)
          if (endBitInVar != 0) {
            o << "      op" << OpOrder[Vals[i].getName()]
              << " >>= " << endBitInVar << ";\n";
            beginBitInVar -= endBitInVar;
            endBitInVar = 0;
          }

          // High mask
          o << "      op" << OpOrder[Vals[i].getName()]
            << " &= (1<<" << beginBitInVar+1 << ") - 1;\n";

          // Shift the value to the correct place (according to place in instr)
          if (endBitInInst != 0)
            o << "      op" << OpOrder[Vals[i].getName()]
              << " <<= " << endBitInInst << ";\n";

          // Just OR in the result
          o << "      Value |= op" << OpOrder[Vals[i].getName()] << ";\n";
        }

        // otherwise, will be taken care of in the loop below using this value:
        OpContinuous[Vals[i].getName()] = continuous;
      }
    }

    for (unsigned f = 0, e = Vals.size(); f != e; ++f) {
      if (Vals[f].getPrefix()) {
        BitsInit *FieldInitializer = (BitsInit*)Vals[f].getValue();

        // Scan through the field looking for bit initializers of the current
        // variable...
        for (int i = FieldInitializer->getNumBits()-1; i >= 0; --i) {
          if (BitInit *BI=dynamic_cast<BitInit*>(FieldInitializer->getBit(i)))
          {
            DEBUG(o << "      // bit init: f: " << f << ", i: " << i << "\n");
          } else if (UnsetInit *UI =
                     dynamic_cast<UnsetInit*>(FieldInitializer->getBit(i))) {
            DEBUG(o << "      // unset init: f: " << f << ", i: " << i << "\n");
          } else if (VarBitInit *VBI =
                     dynamic_cast<VarBitInit*>(FieldInitializer->getBit(i))) {
            TypedInit *TI = VBI->getVariable();
            if (VarInit *VI = dynamic_cast<VarInit*>(TI)) {
              // If the bits of the field are laid out consecutively in the
              // instruction, then instead of separately ORing in bits, just
              // mask and shift the entire field for efficiency.
              if (OpContinuous[VI->getName()]) {
                // already taken care of in the loop above, thus there is no
                // need to individually OR in the bits

                // for debugging, output the regular version anyway, commented
                DEBUG(o << "      // Value |= getValueBit(op"
                        << OpOrder[VI->getName()] << ", " << VBI->getBitNum()
                        << ")" << " << " << i << ";\n");
              } else {
                o << "      Value |= getValueBit(op" << OpOrder[VI->getName()]
                  << ", " << VBI->getBitNum()
                  << ")" << " << " << i << ";\n";
              }
            } else if (FieldInit *FI = dynamic_cast<FieldInit*>(TI)) {
              // FIXME: implement this!
              o << "FIELD INIT not implemented yet!\n";
            } else {
              o << "Error: UNIMPLEMENTED\n";
            }
          }
        }
      } else {
        // ignore annul and predict bits since no one sets them yet
        if (Vals[f].getName() == "annul" || Vals[f].getName() == "predict") {
          o << "      // found " << Vals[f].getName() << "\n";
        }
      }
    }

    o << "      break;\n"
      << "    }\n";
  }

  o << "  default:\n"
    << "    DEBUG(std::cerr << \"Not supported instr: \" << MI << \"\\n\");\n"
    << "    abort();\n"
    << "  }\n"
    << "  return Value;\n"
    << "}\n";
  return false;
}
<|endoftext|>


<commit_before>/*
 * Copyright 2015 Aldebaran
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef BASE_PUBLISHER_HPP
#define BASE_PUBLISHER_HPP

#include <algorithm>
#include <iostream>
#include <string>

#include <alvalue/alvalue.h>
#include <qi/session.hpp>

#include <alrosbridge/tools.hpp>

namespace alros
{
namespace publisher
{

// CRTP
template<class T>
class BasePublisher
{

public:
  BasePublisher( const std::string& name, const std::string& topic, float frequency, qi::SessionPtr session ):
    name_( name ),
    topic_( topic ),
    frequency_( frequency ),
    is_initialized_( false ),
    robot_( UNIDENTIFIED ),
    session_(session)
  {}

  virtual ~BasePublisher() {};

  inline std::string name() const
  {
    return name_;
  }

  inline std::string topic() const
  {
    return topic_;
  }

  inline float frequency() const
  {
    return frequency_;
  }

  inline bool isInitialized() const
  {
    return is_initialized_;
  }

  /** Function that returns the type of a robot
   */
  inline Robot robot() const
  {
    if (robot_ != UNIDENTIFIED)
      return robot_;

    qi::AnyObject p_memory = session_->service("ALMemory");
    std::string robot = p_memory.call<AL::ALValue>("getData", "RobotConfig/Body/Type" );
    std::transform(robot.begin(), robot.end(), robot.begin(), ::tolower);

    if (std::string(robot) == "nao")
      robot_ = NAO;
    if (std::string(robot) == "pepper   ")
      robot_ = PEPPER;
  }

  virtual bool isSubscribed() const = 0;

protected:
  std::string name_, topic_;

  bool is_initialized_;

  /** Frequency at which the publisher should publish. This is informative */
  float frequency_;
  /** The type of the robot */
  mutable Robot robot_;

  /** Pointer to a session from which we can create proxies */
  qi::SessionPtr session_;
}; // class

} //publisher
} // alros

#endif
<commit_msg>quickfix: remove whitespaces in string compare<commit_after>/*
 * Copyright 2015 Aldebaran
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef BASE_PUBLISHER_HPP
#define BASE_PUBLISHER_HPP

#include <algorithm>
#include <iostream>
#include <string>

#include <alvalue/alvalue.h>
#include <qi/session.hpp>

#include <alrosbridge/tools.hpp>

namespace alros
{
namespace publisher
{

// CRTP
template<class T>
class BasePublisher
{

public:
  BasePublisher( const std::string& name, const std::string& topic, float frequency, qi::SessionPtr session ):
    name_( name ),
    topic_( topic ),
    frequency_( frequency ),
    is_initialized_( false ),
    robot_( UNIDENTIFIED ),
    session_(session)
  {}

  virtual ~BasePublisher() {};

  inline std::string name() const
  {
    return name_;
  }

  inline std::string topic() const
  {
    return topic_;
  }

  inline float frequency() const
  {
    return frequency_;
  }

  inline bool isInitialized() const
  {
    return is_initialized_;
  }

  /** Function that returns the type of a robot
   */
  inline Robot robot() const
  {
    if (robot_ != UNIDENTIFIED)
      return robot_;

    qi::AnyObject p_memory = session_->service("ALMemory");
    std::string robot = p_memory.call<AL::ALValue>("getData", "RobotConfig/Body/Type" );
    std::transform(robot.begin(), robot.end(), robot.begin(), ::tolower);

    if (std::string(robot) == "nao")
      robot_ = NAO;
    if (std::string(robot) == "pepper")
      robot_ = PEPPER;
  }

  virtual bool isSubscribed() const = 0;

protected:
  std::string name_, topic_;

  bool is_initialized_;

  /** Frequency at which the publisher should publish. This is informative */
  float frequency_;
  /** The type of the robot */
  mutable Robot robot_;

  /** Pointer to a session from which we can create proxies */
  qi::SessionPtr session_;
}; // class

} //publisher
} // alros

#endif
<|endoftext|>


<commit_before>#include <SimpleITKTestHarness.h>

#include <sitkImageFileReader.h>
#include <sitkImageFileWriter.h>
#include <sitkImageHashFilter.h>

#include <itkIntTypes.h>

#include "itkImage.h"
#include "itkVectorImage.h"

using  itk::simple::InstantiatedPixelIDTypeList;

class Image : public ::testing::Test {
public:
  virtual void SetUp() {
    itk::ImageBase<3>::IndexType index;
    itk::ImageBase<3>::SizeType size;
    itk::ImageBase<3>::RegionType region;
    // Create an image
    for ( int i = 0; i < 3; i++ ) {
      index[i] = 0;
      size[i] = 64+i;
    }
    region.SetSize ( size );
    region.SetIndex ( index );
    itk::Image<short,3>::Pointer im = itk::Image<short,3>::New();
    im->SetRegions ( region );
    im->Allocate();
    im->FillBuffer ( 100 );
    itkShortImage = im;
    shortImage = new itk::simple::Image( im.GetPointer() );

    itk::Image<float,3>::Pointer fim = itk::Image<float,3>::New();
    fim->SetRegions ( region );
    fim->Allocate();
    im->FillBuffer ( 0.0 );
    itkFloatImage = fim;
    floatImage = new itk::simple::Image( fim.GetPointer() );

    itkFloatVectorImage = FloatVectorImageType::New();
    floatVectorImage = new itk::simple::Image( itkFloatVectorImage.GetPointer() );

    itkFloatVector2DImage = FloatVector2DImageType::New();
    floatVector2DImage = new itk::simple::Image( itkFloatVector2DImage );

  }

  itk::simple::Image::Pointer image;
  itk::ImageBase<3>::Pointer itkShortImage;
  itk::ImageBase<3>::IndexType index;
  itk::ImageBase<3>::SizeType size;
  itk::ImageBase<3>::RegionType region;

  typedef itk::Image<short,3> ShortImageType;
  itk::simple::Image::Pointer shortImage;

  typedef itk::Image<float,3> FloatImageType;
  itk::simple::Image::Pointer floatImage;
  FloatImageType::Pointer itkFloatImage;

  typedef itk::VectorImage<float,3> FloatVectorImageType;
  itk::simple::Image::Pointer floatVectorImage;
  FloatVectorImageType::Pointer itkFloatVectorImage;

  typedef itk::VectorImage<float,2> FloatVector2DImageType;
  itk::simple::Image::Pointer floatVector2DImage;
  FloatVector2DImageType::Pointer itkFloatVector2DImage;

  itk::simple::Image::Pointer differentSizedImage;
  ShortImageType::Pointer itkDifferentSizedImage;
};

TEST_F(Image,Create) {
  ASSERT_TRUE ( shortImage->GetImageBase().IsNotNull() );
  EXPECT_EQ ( shortImage->GetWidth(), itkShortImage->GetLargestPossibleRegion().GetSize()[0] ) << " Checking image width";
  EXPECT_EQ ( shortImage->GetHeight(), itkShortImage->GetLargestPossibleRegion().GetSize()[1] ) << " Checking image height";
  EXPECT_EQ ( shortImage->GetDepth(), itkShortImage->GetLargestPossibleRegion().GetSize()[2] ) << " Checking image depth";
}

TEST_F(Image,ImageDataType) {

  // this test checks that the DataType of the images are correct
  int result;

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result);

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<float> >::Result;
  EXPECT_EQ( floatImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVector2DImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result);

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::BasicPixelID<float> >::Result;
  EXPECT_EQ( floatImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVector2DImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList,  itk::simple::ImageTypeToPixelID<ShortImageType>::PixelIDType >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList,  itk::simple::ImageTypeToPixelID<FloatVectorImageType>::PixelIDType >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = itk::simple::ImageTypeToPixelIDValue<ShortImageType>::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result );

  result = itk::simple::ImageTypeToPixelIDValue<FloatVectorImageType>::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

}

TEST_F(Image,Constructors) {
  itk::simple::Image::Pointer image;
  itk::simple::ImageHashFilter hasher;
  int result;

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkUInt8 );
  EXPECT_EQ ( "08183e1b0c50fd2cf6f070b58e218443fb7d5317", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt8";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<unsigned char> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "8-bit unsigned integer" );
  EXPECT_EQ ( image->GetDimension(), 3u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkInt16 );
  EXPECT_EQ ( "645b71695b94923c868e16b943d8acf8f6788617", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt16";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "16-bit signed integer" );
  EXPECT_EQ ( image->GetDimension(), 3u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );

  image = new itk::simple::Image ( 64, 65, itk::simple::sitkUInt16 );
  EXPECT_EQ ( "e3c464cc1b73df3f48bacf238a80f88b5ab0d3e6", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt16";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<unsigned short> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "16-bit unsigned integer" );
  EXPECT_EQ ( image->GetDimension(), 2u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 0u, image->GetDepth() );

  // currently we don't have a good interface to check the values of
  // these images, let just construct these types need todo better
  // testing!

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt8 );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt16 );
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt32 );

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkVectorUInt8 );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkVectorUInt16 );

}

TEST_F(Image,Hash) {
  itk::simple::ImageHashFilter hasher;
  EXPECT_EQ ( "645b71695b94923c868e16b943d8acf8f6788617", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( shortImage ) ) << " SHA1 hash value";
  EXPECT_EQ ( "d4ca27c766665f7422027ab1b977b2ef", hasher.SetHashFunction ( itk::simple::ImageHashFilter::MD5 ).Execute ( shortImage ) ) << " MD5 hash value";

  EXPECT_EQ ( "3b6bfcb1922bf8b29b171062ad722c82f8aa3f50", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( floatImage ) ) << " SHA1 hash value";
  EXPECT_EQ ( "e5eba8af943d7911220c9f2fb9b5b9c8", hasher.SetHashFunction ( itk::simple::ImageHashFilter::MD5 ).Execute ( floatImage ) ) << " MD5 hash value";
}
<commit_msg>BUG: updated ImageTests to actually release the data<commit_after>#include <SimpleITKTestHarness.h>

#include <sitkImageFileReader.h>
#include <sitkImageFileWriter.h>
#include <sitkImageHashFilter.h>

#include <itkIntTypes.h>

#include "itkImage.h"
#include "itkVectorImage.h"

using  itk::simple::InstantiatedPixelIDTypeList;

class Image : public ::testing::Test {
public:
  virtual void SetUp() {
    itk::ImageBase<3>::IndexType index;
    itk::ImageBase<3>::SizeType size;
    itk::ImageBase<3>::RegionType region;
    // Create an image
    for ( int i = 0; i < 3; i++ ) {
      index[i] = 0;
      size[i] = 64+i;
    }
    region.SetSize ( size );
    region.SetIndex ( index );
    itk::Image<short,3>::Pointer im = itk::Image<short,3>::New();
    im->SetRegions ( region );
    im->Allocate();
    im->FillBuffer ( 100 );
    itkShortImage = im;
    shortImage = new itk::simple::Image( im.GetPointer() );
    shortImage->UnRegister();

    itk::Image<float,3>::Pointer fim = itk::Image<float,3>::New();
    fim->SetRegions ( region );
    fim->Allocate();
    im->FillBuffer ( 0.0 );
    itkFloatImage = fim;
    floatImage = new itk::simple::Image( fim.GetPointer() );
    floatImage->UnRegister();

    itkFloatVectorImage = FloatVectorImageType::New();
    floatVectorImage = new itk::simple::Image( itkFloatVectorImage.GetPointer() );
    floatVectorImage->UnRegister();

    itkFloatVector2DImage = FloatVector2DImageType::New();
    floatVector2DImage = new itk::simple::Image( itkFloatVector2DImage );
    floatVector2DImage->UnRegister();

  }

  itk::simple::Image::Pointer image;
  itk::ImageBase<3>::Pointer itkShortImage;
  itk::ImageBase<3>::IndexType index;
  itk::ImageBase<3>::SizeType size;
  itk::ImageBase<3>::RegionType region;

  typedef itk::Image<short,3> ShortImageType;
  itk::simple::Image::Pointer shortImage;

  typedef itk::Image<float,3> FloatImageType;
  itk::simple::Image::Pointer floatImage;
  FloatImageType::Pointer itkFloatImage;

  typedef itk::VectorImage<float,3> FloatVectorImageType;
  itk::simple::Image::Pointer floatVectorImage;
  FloatVectorImageType::Pointer itkFloatVectorImage;

  typedef itk::VectorImage<float,2> FloatVector2DImageType;
  itk::simple::Image::Pointer floatVector2DImage;
  FloatVector2DImageType::Pointer itkFloatVector2DImage;

  itk::simple::Image::Pointer differentSizedImage;
  ShortImageType::Pointer itkDifferentSizedImage;
};

TEST_F(Image,Create) {
  ASSERT_TRUE ( shortImage->GetImageBase().IsNotNull() );
  EXPECT_EQ ( shortImage->GetWidth(), itkShortImage->GetLargestPossibleRegion().GetSize()[0] ) << " Checking image width";
  EXPECT_EQ ( shortImage->GetHeight(), itkShortImage->GetLargestPossibleRegion().GetSize()[1] ) << " Checking image height";
  EXPECT_EQ ( shortImage->GetDepth(), itkShortImage->GetLargestPossibleRegion().GetSize()[2] ) << " Checking image depth";
}

TEST_F(Image,ImageDataType) {

  // this test checks that the DataType of the images are correct
  int result;

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result);

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<float> >::Result;
  EXPECT_EQ( floatImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVector2DImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result);

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::BasicPixelID<float> >::Result;
  EXPECT_EQ( floatImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = itk::simple::PixelIDToPixelIDValue< itk::simple::VectorPixelID<float> >::Result;
  EXPECT_EQ( floatVector2DImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList,  itk::simple::ImageTypeToPixelID<ShortImageType>::PixelIDType >::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result );

  result = typelist::IndexOf< InstantiatedPixelIDTypeList,  itk::simple::ImageTypeToPixelID<FloatVectorImageType>::PixelIDType >::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

  result = itk::simple::ImageTypeToPixelIDValue<ShortImageType>::Result;
  EXPECT_EQ( shortImage->GetPixelIDValue(), result );

  result = itk::simple::ImageTypeToPixelIDValue<FloatVectorImageType>::Result;
  EXPECT_EQ( floatVectorImage->GetPixelIDValue(), result );

}

TEST_F(Image,Constructors) {
  itk::simple::Image::Pointer image;
  itk::simple::ImageHashFilter hasher;
  int result;

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkUInt8 );
  image->UnRegister();
  EXPECT_EQ ( "08183e1b0c50fd2cf6f070b58e218443fb7d5317", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt8";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<unsigned char> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "8-bit unsigned integer" );
  EXPECT_EQ ( image->GetDimension(), 3u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkInt16 );
  image->UnRegister();
  EXPECT_EQ ( "645b71695b94923c868e16b943d8acf8f6788617", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt16";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<short> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "16-bit signed integer" );
  EXPECT_EQ ( image->GetDimension(), 3u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );

  image = new itk::simple::Image ( 64, 65, itk::simple::sitkUInt16 );
  image->UnRegister();
  EXPECT_EQ ( "e3c464cc1b73df3f48bacf238a80f88b5ab0d3e6", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( image ) ) << " SHA1 hash value sitkUInt16";
  result = typelist::IndexOf< InstantiatedPixelIDTypeList, itk::simple::BasicPixelID<unsigned short> >::Result;
  EXPECT_EQ ( image->GetPixelIDValue(), result );
  EXPECT_EQ ( image->GetPixelIDTypeAsString(), "16-bit unsigned integer" );
  EXPECT_EQ ( image->GetDimension(), 2u );
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 0u, image->GetDepth() );

  // currently we don't have a good interface to check the values of
  // these images, let just construct these types need todo better
  // testing!

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt8 );
  image->UnRegister();
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt16 );
  image->UnRegister();
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkLabelUInt32 );
  image->UnRegister();

  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkVectorUInt8 );
  image->UnRegister();
  EXPECT_EQ ( 64u, image->GetWidth() );
  EXPECT_EQ ( 65u, image->GetHeight() );
  EXPECT_EQ ( 66u, image->GetDepth() );
  image = new itk::simple::Image ( 64, 65, 66, itk::simple::sitkVectorUInt16 );
  image->UnRegister();

}

TEST_F(Image,Hash) {
  itk::simple::ImageHashFilter hasher;
  EXPECT_EQ ( "645b71695b94923c868e16b943d8acf8f6788617", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( shortImage ) ) << " SHA1 hash value";
  EXPECT_EQ ( "d4ca27c766665f7422027ab1b977b2ef", hasher.SetHashFunction ( itk::simple::ImageHashFilter::MD5 ).Execute ( shortImage ) ) << " MD5 hash value";

  EXPECT_EQ ( "3b6bfcb1922bf8b29b171062ad722c82f8aa3f50", hasher.SetHashFunction ( itk::simple::ImageHashFilter::SHA1 ).Execute ( floatImage ) ) << " SHA1 hash value";
  EXPECT_EQ ( "e5eba8af943d7911220c9f2fb9b5b9c8", hasher.SetHashFunction ( itk::simple::ImageHashFilter::MD5 ).Execute ( floatImage ) ) << " MD5 hash value";
}
<|endoftext|>


<commit_before>// Copyright 2017 The Ray Authors.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//  http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <functional>
#include <iostream>

#include "ray/raylet/mock_gcs_client.h"

namespace ray {

ray::Status ObjectTable::GetObjectClientIDs(const ray::ObjectID &object_id,
                                            const ClientIDsCallback &success,
                                            const FailCallback &fail) {
  RAY_LOG(DEBUG) << "GetObjectClientIDs " << object_id;
  if (client_lookup.count(object_id) > 0) {
    if (!client_lookup[object_id].empty()) {
      std::vector<ClientID> v;
      for (auto client_id : client_lookup[object_id]) {
        v.push_back(client_id);
      }
      success(std::move(v));
      return Status::OK();
    } else {
      fail(Status::KeyError("ObjectID has no clients."));
      return Status::OK();
    }
  } else {
    fail(Status::KeyError("ObjectID doesn't exist."));
    return Status::OK();
  }
}

ray::Status ObjectTable::Add(const ObjectID &object_id, const ClientID &client_id,
                             const DoneCallback &done_callback) {
  if (client_lookup.count(object_id) == 0) {
    RAY_LOG(DEBUG) << "Add ObjectID set " << object_id;
    client_lookup[object_id] = std::unordered_set<ClientID>();
  } else if (client_lookup[object_id].count(client_id) != 0) {
    return ray::Status::KeyError("ClientID already exists.");
  }
  RAY_LOG(DEBUG) << "Insert ClientID " << client_id;
  client_lookup[object_id].insert(client_id);
  done_callback();
  return ray::Status::OK();
}

ray::Status ObjectTable::Remove(const ObjectID &object_id, const ClientID &client_id,
                                const DoneCallback &done_callback) {
  if (client_lookup.count(object_id) == 0) {
    return ray::Status::KeyError("ObjectID doesn't exist.");
  } else if (client_lookup[object_id].count(client_id) == 0) {
    return ray::Status::KeyError("ClientID doesn't exist.");
  }
  client_lookup[object_id].erase(client_id);
  done_callback();
  return ray::Status::OK();
}

ray::Status ClientTable::GetClientIds(ClientIDsCallback callback) {
  std::vector<ClientID> keys;
  keys.reserve(info_lookup.size());
  for (auto kv : info_lookup) {
    keys.push_back(kv.first);
  }
  callback(keys);
  return Status::OK();
}

void ClientTable::GetClientInformationSet(const std::vector<ClientID> &client_ids,
                                          ManyInfoCallback callback,
                                          FailCallback failcb) {
  std::vector<ClientInformation> info_vec;
  for (const auto &client_id : client_ids) {
    if (info_lookup.count(client_id) != 0) {
      info_vec.push_back(info_lookup.at(client_id));
    }
  }
  if (info_vec.empty()) {
    failcb(Status::KeyError("ClientID not found."));
  } else {
    callback(info_vec);
  }
}

void ClientTable::GetClientInformation(const ClientID &client_id,
                                       SingleInfoCallback callback, FailCallback failcb) {
  if (info_lookup.count(client_id) == 0) {
    failcb(ray::Status::KeyError("CleintID not found."));
  } else {
    callback(info_lookup.at(client_id));
  }
}

ray::Status ClientTable::Add(const ClientID &client_id, const std::string &ip,
                             uint16_t port, DoneCallback done_callback) {
  if (info_lookup.count(client_id) != 0) {
    return ray::Status::KeyError("ClientID already exists.");
  }
  info_lookup.emplace(client_id, ClientInformation(client_id, ip, port));
  done_callback();
  return ray::Status::OK();
}

ray::Status ClientTable::Remove(const ClientID &client_id, DoneCallback done_callback) {
  if (info_lookup.count(client_id) == 0) {
    return ray::Status::KeyError("ClientID doesn't exist.");
  }
  info_lookup.erase(client_id);
  done_callback();
  return ray::Status::OK();
}

ClientID GcsClient::Register(const std::string &ip, uint16_t port) {
  ClientID client_id = ClientID::FromRandom();
  // TODO: handle client registration failure.
  ray::Status status = client_table().Add(std::move(client_id), ip, port, []() {});
  return client_id;
}

ClientTable &GcsClient::client_table() { return *client_table_; }

}  // namespace ray
<commit_msg>Fix a bad usage of std::move (#8364)<commit_after>// Copyright 2017 The Ray Authors.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//  http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <functional>
#include <iostream>

#include "ray/raylet/mock_gcs_client.h"

namespace ray {

ray::Status ObjectTable::GetObjectClientIDs(const ray::ObjectID &object_id,
                                            const ClientIDsCallback &success,
                                            const FailCallback &fail) {
  RAY_LOG(DEBUG) << "GetObjectClientIDs " << object_id;
  if (client_lookup.count(object_id) > 0) {
    if (!client_lookup[object_id].empty()) {
      std::vector<ClientID> v;
      for (auto client_id : client_lookup[object_id]) {
        v.push_back(client_id);
      }
      success(std::move(v));
      return Status::OK();
    } else {
      fail(Status::KeyError("ObjectID has no clients."));
      return Status::OK();
    }
  } else {
    fail(Status::KeyError("ObjectID doesn't exist."));
    return Status::OK();
  }
}

ray::Status ObjectTable::Add(const ObjectID &object_id, const ClientID &client_id,
                             const DoneCallback &done_callback) {
  if (client_lookup.count(object_id) == 0) {
    RAY_LOG(DEBUG) << "Add ObjectID set " << object_id;
    client_lookup[object_id] = std::unordered_set<ClientID>();
  } else if (client_lookup[object_id].count(client_id) != 0) {
    return ray::Status::KeyError("ClientID already exists.");
  }
  RAY_LOG(DEBUG) << "Insert ClientID " << client_id;
  client_lookup[object_id].insert(client_id);
  done_callback();
  return ray::Status::OK();
}

ray::Status ObjectTable::Remove(const ObjectID &object_id, const ClientID &client_id,
                                const DoneCallback &done_callback) {
  if (client_lookup.count(object_id) == 0) {
    return ray::Status::KeyError("ObjectID doesn't exist.");
  } else if (client_lookup[object_id].count(client_id) == 0) {
    return ray::Status::KeyError("ClientID doesn't exist.");
  }
  client_lookup[object_id].erase(client_id);
  done_callback();
  return ray::Status::OK();
}

ray::Status ClientTable::GetClientIds(ClientIDsCallback callback) {
  std::vector<ClientID> keys;
  keys.reserve(info_lookup.size());
  for (auto kv : info_lookup) {
    keys.push_back(kv.first);
  }
  callback(keys);
  return Status::OK();
}

void ClientTable::GetClientInformationSet(const std::vector<ClientID> &client_ids,
                                          ManyInfoCallback callback,
                                          FailCallback failcb) {
  std::vector<ClientInformation> info_vec;
  for (const auto &client_id : client_ids) {
    if (info_lookup.count(client_id) != 0) {
      info_vec.push_back(info_lookup.at(client_id));
    }
  }
  if (info_vec.empty()) {
    failcb(Status::KeyError("ClientID not found."));
  } else {
    callback(info_vec);
  }
}

void ClientTable::GetClientInformation(const ClientID &client_id,
                                       SingleInfoCallback callback, FailCallback failcb) {
  if (info_lookup.count(client_id) == 0) {
    failcb(ray::Status::KeyError("CleintID not found."));
  } else {
    callback(info_lookup.at(client_id));
  }
}

ray::Status ClientTable::Add(const ClientID &client_id, const std::string &ip,
                             uint16_t port, DoneCallback done_callback) {
  if (info_lookup.count(client_id) != 0) {
    return ray::Status::KeyError("ClientID already exists.");
  }
  info_lookup.emplace(client_id, ClientInformation(client_id, ip, port));
  done_callback();
  return ray::Status::OK();
}

ray::Status ClientTable::Remove(const ClientID &client_id, DoneCallback done_callback) {
  if (info_lookup.count(client_id) == 0) {
    return ray::Status::KeyError("ClientID doesn't exist.");
  }
  info_lookup.erase(client_id);
  done_callback();
  return ray::Status::OK();
}

ClientID GcsClient::Register(const std::string &ip, uint16_t port) {
  ClientID client_id = ClientID::FromRandom();
  // TODO: handle client registration failure.
  ray::Status status = client_table().Add(client_id, ip, port, []() {});
  return client_id;
}

ClientTable &GcsClient::client_table() { return *client_table_; }

}  // namespace ray
<|endoftext|>


<commit_before>#ifndef __BTREE_RGET_HPP__
#define __BTREE_RGET_HPP__

#include "utils.hpp"
#include "buffer_cache/buffer_cache.hpp"
#include "buffer_cache/large_buf.hpp"
#include "btree/key_value_store.hpp"
#include "btree/slice.hpp"
#include "buffer_cache/transactor.hpp"
#include <boost/shared_ptr.hpp>

struct rget_value_provider_t : public auto_copying_data_provider_t {
    static rget_value_provider_t *create_provider(const btree_value *value, boost::shared_ptr<transactor_t> transactor);
    virtual ~rget_value_provider_t() { }
protected:
    rget_value_provider_t() { }
};

struct rget_small_value_provider_t : public rget_value_provider_t {
    typedef std::vector<byte> buffer_t;
    buffer_t value;
    boost::scoped_ptr<const_buffer_group_t> buffers;

    size_t get_size() const {
        return value.size();
    }

    const const_buffer_group_t *get_data_as_buffers() throw (data_provider_failed_exc_t) {
        rassert(!buffers.get());

        buffers.reset(new const_buffer_group_t());
        buffers->add_buffer(get_size(), value.data());
        return buffers.get();
    }
protected:
    rget_small_value_provider_t(const btree_value *value) : value() {
        rassert(!value->is_large());
        const byte *data = ptr_cast<byte>(value->value());
        this->value.assign(data, data + value->value_size());
    }

    friend class rget_value_provider_t;
};

struct rget_large_value_provider_t : public rget_value_provider_t {
    ~rget_large_value_provider_t();

    size_t get_size() const {
        // RSI
        return 0;
    }

    const const_buffer_group_t *get_data_as_buffers() throw (data_provider_failed_exc_t) {
        // RSI
        return NULL;
    }
private:
    rget_large_value_provider_t(const btree_value *value, boost::shared_ptr<transactor_t> transactor) {} // RSI

    boost::shared_ptr<transactor_t> transactor;
    large_buf_t *large_value;

    friend class rget_value_provider_t;
};

struct rget_key_value_pair_t {
    std::string key;
    mcflags_t flags;
    boost::shared_ptr<data_provider_t> value;
};

store_t::rget_result_t btree_rget(btree_key_value_store_t *store, store_key_t *start, store_key_t *end, bool left_open, bool right_open, uint64_t max_results);
store_t::rget_result_t btree_rget_slice(btree_slice_t *slice, store_key_t *start, store_key_t *end, bool left_open, bool right_open, uint64_t max_results);

#endif // __BTREE_RGET_HPP__

<commit_msg>Implement the large value data provider for rget<commit_after>#ifndef __BTREE_RGET_HPP__
#define __BTREE_RGET_HPP__

#include "utils.hpp"
#include "buffer_cache/buffer_cache.hpp"
#include "buffer_cache/large_buf.hpp"
#include "btree/key_value_store.hpp"
#include "btree/slice.hpp"
#include "buffer_cache/transactor.hpp"
#include "buffer_cache/co_functions.hpp"
#include <boost/shared_ptr.hpp>

struct rget_value_provider_t : public auto_copying_data_provider_t {
    static rget_value_provider_t *create_provider(const btree_value *value, boost::shared_ptr<transactor_t> transactor);
    virtual ~rget_value_provider_t() { }
protected:
    rget_value_provider_t() { }
};

struct rget_small_value_provider_t : public rget_value_provider_t {
    typedef std::vector<byte> buffer_t;
    buffer_t value;
    boost::scoped_ptr<const_buffer_group_t> buffers;

    size_t get_size() const {
        return value.size();
    }

    const const_buffer_group_t *get_data_as_buffers() throw (data_provider_failed_exc_t) {
        rassert(!buffers.get());

        buffers.reset(new const_buffer_group_t());
        buffers->add_buffer(get_size(), value.data());
        return buffers.get();
    }
protected:
    rget_small_value_provider_t(const btree_value *value) : value(), buffers() {
        rassert(!value->is_large());
        const byte *data = ptr_cast<byte>(value->value());
        this->value.assign(data, data + value->value_size());
    }

    friend class rget_value_provider_t;
};

struct rget_large_value_provider_t : public rget_value_provider_t {
    ~rget_large_value_provider_t();

    size_t get_size() const {
        return lb_ref.size;
    }

    const const_buffer_group_t *get_data_as_buffers() throw (data_provider_failed_exc_t) {
        rassert(!buffers.get());

        large_value = new large_buf_t(transactor->transaction());
        co_acquire_large_value(large_value, lb_ref, rwi_read);
        rassert(large_value->state == large_buf_t::loaded);
        rassert(large_value->get_root_ref().block_id == lb_ref.block_id);

        for (int i = 0; i < large_value->get_num_segments(); i++) {
            uint16_t size;
            const void *data = large_value->get_segment(i, &size);
            buffers->add_buffer(size, data);
        }
        return buffers.get();
    }
private:
    rget_large_value_provider_t(const btree_value *value, boost::shared_ptr<transactor_t> transactor)
        : transactor(transactor), buffers(), lb_ref(value->lb_ref()) {
    }

    boost::shared_ptr<transactor_t> transactor;
    boost::scoped_ptr<const_buffer_group_t> buffers;
    large_buf_ref lb_ref;
    large_buf_t *large_value;

    friend class rget_value_provider_t;
};

struct rget_key_value_pair_t {
    std::string key;
    mcflags_t flags;
    boost::shared_ptr<data_provider_t> value;
};

store_t::rget_result_t btree_rget(btree_key_value_store_t *store, store_key_t *start, store_key_t *end, bool left_open, bool right_open, uint64_t max_results);
store_t::rget_result_t btree_rget_slice(btree_slice_t *slice, store_key_t *start, store_key_t *end, bool left_open, bool right_open, uint64_t max_results);

#endif // __BTREE_RGET_HPP__

<|endoftext|>


<commit_before>#include "mini_stdint.h"

#define WEAK __attribute__((weak))

extern "C" {

extern long sysconf(int);

typedef struct {
    uint32_t flags;
    void * stack_base;
    size_t stack_size;
    size_t guard_size;
    int32_t sched_policy;
    int32_t sched_priority;
} pthread_attr_t;
typedef long pthread_t;
typedef struct {
    // 48 bytes is enough for a cond on 64-bit and 32-bit systems
    unsigned char _private[48];
} pthread_cond_t;
typedef long pthread_condattr_t;
typedef struct {
    // 40 bytes is enough for a mutex on 64-bit and 32-bit systems
    unsigned char _private[40];
} pthread_mutex_t;
typedef long pthread_mutexattr_t;
extern int pthread_create(pthread_t *thread, pthread_attr_t const * attr,
                          void *(*start_routine)(void *), void * arg);
extern int pthread_join(pthread_t thread, void **retval);
extern int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr);
extern int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex);
extern int pthread_cond_broadcast(pthread_cond_t *cond);
extern int pthread_cond_destroy(pthread_cond_t *cond);
extern int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr);
extern int pthread_mutex_lock(pthread_mutex_t *mutex);
extern int pthread_mutex_unlock(pthread_mutex_t *mutex);
extern int pthread_mutex_destroy(pthread_mutex_t *mutex);

extern char *getenv(const char *);
extern int atoi(const char *);

extern int halide_printf(void *user_context, const char *, ...);

#ifndef NULL
#define NULL 0
#endif

struct work {
    work *next_job;
    int (*f)(void *, int, uint8_t *);
    void *user_context;
    int next, max;
    uint8_t *closure;
    int active_workers;
    int exit_status;
    bool running() { return next < max || active_workers > 0; }
};

// The work queue and thread pool is weak, so one big work queue is shared by all halide functions
#define MAX_THREADS 64
WEAK struct {
    // all fields are protected by this mutex.
    pthread_mutex_t mutex;

    // Singly linked list for job stack
    work *jobs;

    // Broadcast whenever items are added to the queue or a job completes.
    pthread_cond_t state_change;
    // Keep track of threads so they can be joined at shutdown
    pthread_t threads[MAX_THREADS];

    // Global flag indicating
    bool shutdown;

    bool running() {
        return !shutdown;
    }

} halide_work_queue;

WEAK int halide_threads;
WEAK bool halide_thread_pool_initialized = false;

WEAK void halide_shutdown_thread_pool() {
    if (!halide_thread_pool_initialized) return;

    // Wake everyone up and tell them the party's over and it's time
    // to go home
    pthread_mutex_lock(&halide_work_queue.mutex);
    halide_work_queue.shutdown = true;
    pthread_cond_broadcast(&halide_work_queue.state_change);
    pthread_mutex_unlock(&halide_work_queue.mutex);

    // Wait until they leave
    for (int i = 0; i < halide_threads-1; i++) {
        //fprintf(stderr, "Waiting for thread %d to exit\n", i);
        void *retval;
        pthread_join(halide_work_queue.threads[i], &retval);
    }

    //fprintf(stderr, "All threads have quit. Destroying mutex and condition variable.\n");
    // Tidy up
    pthread_mutex_destroy(&halide_work_queue.mutex);
    // Reset it to zero in case we call another do_par_for
    pthread_mutex_t uninitialized_mutex = {0};
    halide_work_queue.mutex = uninitialized_mutex;
    pthread_cond_destroy(&halide_work_queue.state_change);
    halide_thread_pool_initialized = false;
}

typedef int (*halide_task)(void *user_context, int, uint8_t *);

WEAK int (*halide_custom_do_task)(void *user_context, halide_task, int, uint8_t *);

WEAK void halide_set_custom_do_task(int (*f)(void *, halide_task, int, uint8_t *)) {
    halide_custom_do_task = f;
}

WEAK int (*halide_custom_do_par_for)(void *, halide_task, int, int, uint8_t *);

WEAK void halide_set_custom_do_par_for(int (*f)(void *, halide_task, int, int, uint8_t *)) {
    halide_custom_do_par_for = f;
}

WEAK int halide_do_task(void *user_context, halide_task f, int idx,
                        uint8_t *closure) {
    if (halide_custom_do_task) {
        return (*halide_custom_do_task)(user_context, f, idx, closure);
    } else {
        return f(user_context, idx, closure);
    }
}

WEAK void *halide_worker_thread(void *void_arg) {
    work *owned_job = (work *)void_arg;

    // Grab the lock
    pthread_mutex_lock(&halide_work_queue.mutex);

    // If I'm a job owner, then I was the thread that called
    // do_par_for, and I should only stay in this function until my
    // job is complete. If I'm a lowly worker thread, I should stay in
    // this function as long as the work queue is running.
    while (owned_job != NULL ? owned_job->running()
           : halide_work_queue.running()) {

        if (halide_work_queue.jobs == NULL) {
            // There are no jobs pending, though some tasks may still
            // be in flight from the last job. Release the lock and
            // wait for something new to happen.
            pthread_cond_wait(&halide_work_queue.state_change, &halide_work_queue.mutex);
        } else {
            // There are jobs still to do. Grab the next one.
            work *job = halide_work_queue.jobs;

            // Claim a task from it.
            work myjob = *job;
            job->next++;

            // If there were no more tasks pending for this job, or if
            // it has failed, remove it from the stack.
            if (job->next == job->max) {
                halide_work_queue.jobs = job->next_job;
            }

            // Increment the active_worker count so that other threads
            // are aware that this job is still in progress even
            // though there are no outstanding tasks for it.
            job->active_workers++;

            // Release the lock and do the task.
            pthread_mutex_unlock(&halide_work_queue.mutex);
            int result = halide_do_task(myjob.user_context, myjob.f, myjob.next,
                                        myjob.closure);
            pthread_mutex_lock(&halide_work_queue.mutex);

            // If this task failed, set the exit status on the job.
            if (result) {
                job->exit_status = result;
            }

            // We are no longer active on this job
            job->active_workers--;

            // If the job is done and I'm not the owner of it, wake up
            // the owner.
            if (!job->running() && job != owned_job) {
                pthread_cond_broadcast(&halide_work_queue.state_change);
            }
        }
    }
    pthread_mutex_unlock(&halide_work_queue.mutex);
    return NULL;
}

extern int halide_host_cpu_count();

WEAK int halide_do_par_for(void *user_context, int (*f)(void *, int, uint8_t *),
                           int min, int size, uint8_t *closure) {
    if (halide_custom_do_par_for) {
        return (*halide_custom_do_par_for)(user_context, f, min, size, closure);
    }

    // Grab the lock. If it hasn't been initialized yet, then the
    // field will be zero-initialized because it's a static
    // global. pthreads helpfully interprets zero-valued mutex objects
    // as uninitialized and initializes them for you (see PTHREAD_MUTEX_INITIALIZER).
    pthread_mutex_lock(&halide_work_queue.mutex);

    if (!halide_thread_pool_initialized) {
        halide_work_queue.shutdown = false;
        pthread_cond_init(&halide_work_queue.state_change, NULL);
        halide_work_queue.jobs = NULL;

        char *threadStr = getenv("HL_NUMTHREADS");
        if (threadStr) {
            halide_threads = atoi(threadStr);
        } else {
            halide_threads = halide_host_cpu_count();
            // halide_printf(user_context, "HL_NUMTHREADS not defined. Defaulting to %d threads.\n", halide_threads);
        }
        if (halide_threads > MAX_THREADS) {
            halide_threads = MAX_THREADS;
        } else if (halide_threads < 1) {
            halide_threads = 1;
        }
        for (int i = 0; i < halide_threads-1; i++) {
            //fprintf(stderr, "Creating thread %d\n", i);
            pthread_create(halide_work_queue.threads + i, NULL, halide_worker_thread, NULL);
        }

        halide_thread_pool_initialized = true;
    }

    // Make the job.
    work job;
    job.f = f;               // The job should call this function. It takes an index and a closure.
    job.user_context = user_context;
    job.next = min;          // Start at this index.
    job.max  = min + size;   // Keep going until one less than this index.
    job.closure = closure;   // Use this closure.
    job.exit_status = 0;     // The job hasn't failed yet
    job.active_workers = 0;  // Nobody is working on this yet

    // Push the job onto the stack.
    job.next_job = halide_work_queue.jobs;
    halide_work_queue.jobs = &job;
    pthread_mutex_unlock(&halide_work_queue.mutex);

    // Wake up any idle worker threads.
    pthread_cond_broadcast(&halide_work_queue.state_change);

    // Do some work myself.
    halide_worker_thread((void *)(&job));

    // Return zero if the job succeeded, otherwise return the exit
    // status of one of the failing jobs (whichever one failed last).
    return job.exit_status;
}

}
<commit_msg>Comment fix.<commit_after>#include "mini_stdint.h"

#define WEAK __attribute__((weak))

extern "C" {

extern long sysconf(int);

typedef struct {
    uint32_t flags;
    void * stack_base;
    size_t stack_size;
    size_t guard_size;
    int32_t sched_policy;
    int32_t sched_priority;
} pthread_attr_t;
typedef long pthread_t;
typedef struct {
    // 48 bytes is enough for a cond on 64-bit and 32-bit systems
    unsigned char _private[48];
} pthread_cond_t;
typedef long pthread_condattr_t;
typedef struct {
    // 40 bytes is enough for a mutex on 64-bit and 32-bit systems
    unsigned char _private[40];
} pthread_mutex_t;
typedef long pthread_mutexattr_t;
extern int pthread_create(pthread_t *thread, pthread_attr_t const * attr,
                          void *(*start_routine)(void *), void * arg);
extern int pthread_join(pthread_t thread, void **retval);
extern int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr);
extern int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex);
extern int pthread_cond_broadcast(pthread_cond_t *cond);
extern int pthread_cond_destroy(pthread_cond_t *cond);
extern int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr);
extern int pthread_mutex_lock(pthread_mutex_t *mutex);
extern int pthread_mutex_unlock(pthread_mutex_t *mutex);
extern int pthread_mutex_destroy(pthread_mutex_t *mutex);

extern char *getenv(const char *);
extern int atoi(const char *);

extern int halide_printf(void *user_context, const char *, ...);

#ifndef NULL
#define NULL 0
#endif

struct work {
    work *next_job;
    int (*f)(void *, int, uint8_t *);
    void *user_context;
    int next, max;
    uint8_t *closure;
    int active_workers;
    int exit_status;
    bool running() { return next < max || active_workers > 0; }
};

// The work queue and thread pool is weak, so one big work queue is shared by all halide functions
#define MAX_THREADS 64
WEAK struct {
    // all fields are protected by this mutex.
    pthread_mutex_t mutex;

    // Singly linked list for job stack
    work *jobs;

    // Broadcast whenever items are added to the queue or a job completes.
    pthread_cond_t state_change;
    // Keep track of threads so they can be joined at shutdown
    pthread_t threads[MAX_THREADS];

    // Global flag indicating
    bool shutdown;

    bool running() {
        return !shutdown;
    }

} halide_work_queue;

WEAK int halide_threads;
WEAK bool halide_thread_pool_initialized = false;

WEAK void halide_shutdown_thread_pool() {
    if (!halide_thread_pool_initialized) return;

    // Wake everyone up and tell them the party's over and it's time
    // to go home
    pthread_mutex_lock(&halide_work_queue.mutex);
    halide_work_queue.shutdown = true;
    pthread_cond_broadcast(&halide_work_queue.state_change);
    pthread_mutex_unlock(&halide_work_queue.mutex);

    // Wait until they leave
    for (int i = 0; i < halide_threads-1; i++) {
        //fprintf(stderr, "Waiting for thread %d to exit\n", i);
        void *retval;
        pthread_join(halide_work_queue.threads[i], &retval);
    }

    //fprintf(stderr, "All threads have quit. Destroying mutex and condition variable.\n");
    // Tidy up
    pthread_mutex_destroy(&halide_work_queue.mutex);
    // Reset it to zero in case we call another do_par_for
    pthread_mutex_t uninitialized_mutex = {0};
    halide_work_queue.mutex = uninitialized_mutex;
    pthread_cond_destroy(&halide_work_queue.state_change);
    halide_thread_pool_initialized = false;
}

typedef int (*halide_task)(void *user_context, int, uint8_t *);

WEAK int (*halide_custom_do_task)(void *user_context, halide_task, int, uint8_t *);

WEAK void halide_set_custom_do_task(int (*f)(void *, halide_task, int, uint8_t *)) {
    halide_custom_do_task = f;
}

WEAK int (*halide_custom_do_par_for)(void *, halide_task, int, int, uint8_t *);

WEAK void halide_set_custom_do_par_for(int (*f)(void *, halide_task, int, int, uint8_t *)) {
    halide_custom_do_par_for = f;
}

WEAK int halide_do_task(void *user_context, halide_task f, int idx,
                        uint8_t *closure) {
    if (halide_custom_do_task) {
        return (*halide_custom_do_task)(user_context, f, idx, closure);
    } else {
        return f(user_context, idx, closure);
    }
}

WEAK void *halide_worker_thread(void *void_arg) {
    work *owned_job = (work *)void_arg;

    // Grab the lock
    pthread_mutex_lock(&halide_work_queue.mutex);

    // If I'm a job owner, then I was the thread that called
    // do_par_for, and I should only stay in this function until my
    // job is complete. If I'm a lowly worker thread, I should stay in
    // this function as long as the work queue is running.
    while (owned_job != NULL ? owned_job->running()
           : halide_work_queue.running()) {

        if (halide_work_queue.jobs == NULL) {
            // There are no jobs pending, though some tasks may still
            // be in flight from the last job. Release the lock and
            // wait for something new to happen.
            pthread_cond_wait(&halide_work_queue.state_change, &halide_work_queue.mutex);
        } else {
            // There are jobs still to do. Grab the next one.
            work *job = halide_work_queue.jobs;

            // Claim a task from it.
            work myjob = *job;
            job->next++;

            // If there were no more tasks pending for this job,
            // remove it from the stack.
            if (job->next == job->max) {
                halide_work_queue.jobs = job->next_job;
            }

            // Increment the active_worker count so that other threads
            // are aware that this job is still in progress even
            // though there are no outstanding tasks for it.
            job->active_workers++;

            // Release the lock and do the task.
            pthread_mutex_unlock(&halide_work_queue.mutex);
            int result = halide_do_task(myjob.user_context, myjob.f, myjob.next,
                                        myjob.closure);
            pthread_mutex_lock(&halide_work_queue.mutex);

            // If this task failed, set the exit status on the job.
            if (result) {
                job->exit_status = result;
            }

            // We are no longer active on this job
            job->active_workers--;

            // If the job is done and I'm not the owner of it, wake up
            // the owner.
            if (!job->running() && job != owned_job) {
                pthread_cond_broadcast(&halide_work_queue.state_change);
            }
        }
    }
    pthread_mutex_unlock(&halide_work_queue.mutex);
    return NULL;
}

extern int halide_host_cpu_count();

WEAK int halide_do_par_for(void *user_context, int (*f)(void *, int, uint8_t *),
                           int min, int size, uint8_t *closure) {
    if (halide_custom_do_par_for) {
        return (*halide_custom_do_par_for)(user_context, f, min, size, closure);
    }

    // Grab the lock. If it hasn't been initialized yet, then the
    // field will be zero-initialized because it's a static
    // global. pthreads helpfully interprets zero-valued mutex objects
    // as uninitialized and initializes them for you (see PTHREAD_MUTEX_INITIALIZER).
    pthread_mutex_lock(&halide_work_queue.mutex);

    if (!halide_thread_pool_initialized) {
        halide_work_queue.shutdown = false;
        pthread_cond_init(&halide_work_queue.state_change, NULL);
        halide_work_queue.jobs = NULL;

        char *threadStr = getenv("HL_NUMTHREADS");
        if (threadStr) {
            halide_threads = atoi(threadStr);
        } else {
            halide_threads = halide_host_cpu_count();
            // halide_printf(user_context, "HL_NUMTHREADS not defined. Defaulting to %d threads.\n", halide_threads);
        }
        if (halide_threads > MAX_THREADS) {
            halide_threads = MAX_THREADS;
        } else if (halide_threads < 1) {
            halide_threads = 1;
        }
        for (int i = 0; i < halide_threads-1; i++) {
            //fprintf(stderr, "Creating thread %d\n", i);
            pthread_create(halide_work_queue.threads + i, NULL, halide_worker_thread, NULL);
        }

        halide_thread_pool_initialized = true;
    }

    // Make the job.
    work job;
    job.f = f;               // The job should call this function. It takes an index and a closure.
    job.user_context = user_context;
    job.next = min;          // Start at this index.
    job.max  = min + size;   // Keep going until one less than this index.
    job.closure = closure;   // Use this closure.
    job.exit_status = 0;     // The job hasn't failed yet
    job.active_workers = 0;  // Nobody is working on this yet

    // Push the job onto the stack.
    job.next_job = halide_work_queue.jobs;
    halide_work_queue.jobs = &job;
    pthread_mutex_unlock(&halide_work_queue.mutex);

    // Wake up any idle worker threads.
    pthread_cond_broadcast(&halide_work_queue.state_change);

    // Do some work myself.
    halide_worker_thread((void *)(&job));

    // Return zero if the job succeeded, otherwise return the exit
    // status of one of the failing jobs (whichever one failed last).
    return job.exit_status;
}

}
<|endoftext|>