<commit_before>
/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_scenario.h"

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_backup.h"
#include "modules/planning/scenarios/side_pass/stage_detect_safety.h"
#include "modules/planning/scenarios/side_pass/stage_generate_path.h"
#include "modules/planning/scenarios/side_pass/stage_pass_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_stop_on_wait_point.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

apollo::common::util::Factory<
    ScenarioConfig::StageType, Stage,
    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
    SidePassScenario::s_stage_factory_;

// The clearance distance from intersection/destination.
// If ADC is too close, then do not enter SIDE_PASS.
constexpr double kClearDistance = 15.0;

void SidePassScenario::RegisterStages() {
  s_stage_factory_.Clear();
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_APPROACH_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageApproachObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_DETECT_SAFETY,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageDetectSafety(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_GENERATE_PATH,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageGeneratePath(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageStopOnWaitPoint(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_PASS_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StagePassObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_BACKUP,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageBackup(config);
      });
}

SidePassScenario::SidePassScenario(const ScenarioConfig& config,
                                   const ScenarioContext* scenario_context)
    : Scenario(config, scenario_context) {
  side_pass_context_.scenario_config_.CopyFrom(config.side_pass_config());

  // TODO(all): to be removed when SidePass obstacle decision impl is ready
  side_pass_context_.front_blocking_obstacle_id_ =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;
}

std::unique_ptr<Stage> SidePassScenario::CreateStage(
    const ScenarioConfig::StageConfig& stage_config) {
  if (s_stage_factory_.Empty()) {
    RegisterStages();
  }
  auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
                                                 stage_config);
  if (ptr == nullptr) {
    AERROR << "Failed to create stage for config: "
           << stage_config.DebugString();
    return nullptr;
  }
  ptr->SetContext(&side_pass_context_);
  return ptr;
}

bool SidePassScenario::IsTransferable(const Scenario& current_scenario,
                                      const Frame& frame) {
  // Sanity checks.
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  if (current_scenario.scenario_type() == ScenarioConfig::SIDE_PASS) {
    // Check if the blocking obstacle is still static.
    // If not, then switch to LANE_FOLLOW.
    const auto front_blocking_obstacle =
        const_cast<Frame&>(frame).Find(front_blocking_obstacle_id_);
    if (front_blocking_obstacle != nullptr &&
        !front_blocking_obstacle->IsStatic()) {
      AWARN << "Obstacle " << front_blocking_obstacle_id_
            << " starts to move. Change scenario from SIDE_PASS"
               " back to default scenario.";
      return false;
    }
    msg_ = "side pass obstacle: " + front_blocking_obstacle_id_;
    return (current_scenario.GetStatus() !=
            Scenario::ScenarioStatus::STATUS_DONE);
  } else if (current_scenario.scenario_type() != ScenarioConfig::LANE_FOLLOW) {
    // If in some other special scenario, then don't try to switch
    // to SIDE_PASS scenario.
    return false;
  } else {
    // If originally in LANE_FOLLOW, then decide whether we should
    // switch to SIDE_PASS scenario.
    bool is_side_pass = IsSidePassScenario(frame);
    if (is_side_pass) {
      msg_ = "side pass obstacle: " + front_blocking_obstacle_id_;
    }
    return is_side_pass;
  }
}

bool SidePassScenario::IsSidePassScenario(const Frame& frame) {
  return (IsFarFromDestination(frame) &&
          IsFarFromIntersection(frame) &&
          HasBlockingObstacle(frame));
}

bool SidePassScenario::IsFarFromDestination(const Frame& frame) {
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  const auto& reference_line_info = frame.reference_line_info().front();
  if (reference_line_info.SDistanceToDestination() < kClearDistance) {
    ADEBUG << "Too close to destination; don't SIDE_PASS";
    return false;
  }
  return true;
}

bool SidePassScenario::IsFarFromIntersection(const Frame& frame) {
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  const SLBoundary& adc_sl_boundary =
      frame.reference_line_info().front().AdcSlBoundary();
  const auto& first_encountered_overlaps =
      frame.reference_line_info().front().FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    ADEBUG << "AdcSL: " << adc_sl_boundary.ShortDebugString();
    ADEBUG << overlap.first << ", " << overlap.second.DebugString();
    if (overlap.first != ReferenceLineInfo::CLEAR_AREA &&
        overlap.first != ReferenceLineInfo::CROSSWALK &&
        // TODO(yifei) temporarily enable side pass at pnc junction without
        // stop sign and traffic light.
        // overlap.first != ReferenceLineInfo::PNC_JUNCTION &&
        overlap.first != ReferenceLineInfo::SIGNAL &&
        overlap.first != ReferenceLineInfo::STOP_SIGN) {
      continue;
    }

    auto distance = overlap.second.start_s - adc_sl_boundary.end_s();
    if (overlap.first == ReferenceLineInfo::SIGNAL) {
      if (distance < FLAGS_side_pass_min_signal_intersection_distance) {
        ADEBUG << "Too close to signal intersection ("
               << distance << "m); don't SIDE_PASS.";
        return false;
      }
    } else {
      if (distance < kClearDistance) {
        ADEBUG << "Too close to overlap_type[" << overlap.first
               << "] (" << distance << "m); don't SIDE_PASS";
        return false;
      }
    }
  }
  return true;
}

bool SidePassScenario::HasBlockingObstacle(const Frame& frame) {
  // Sanity checks.
  front_blocking_obstacle_id_ = "";
  if (frame.reference_line_info().size() > 1) {
    return false;
  }

  const auto& reference_line_info = frame.reference_line_info().front();
  const PathDecision& path_decision = reference_line_info.path_decision();
  double distance_to_closest_blocking_obstacle = -100.0;
  bool exists_a_blocking_obstacle = false;
  // A blocking obstacle is an obstacle blocks the road when it is not blocked
  // by other obstacles or traffic rules.
  // Loop through every obstacle to locate the closest blocking one, if there
  // exists such an obstacle.
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    const auto& side_pass_config = side_pass_context_.scenario_config_;
    if (IsBlockingObstacleToSidePass(
            frame, obstacle,
            side_pass_config.block_obstacle_min_speed(),
            side_pass_config.min_front_obstacle_distance(),
            side_pass_config.enable_obstacle_blocked_check())) {
      exists_a_blocking_obstacle = true;
      double distance_between_adc_and_obstacle =
          GetDistanceBetweenADCAndObstacle(frame, obstacle);
      if (distance_to_closest_blocking_obstacle < 0.0 ||
          distance_between_adc_and_obstacle <
              distance_to_closest_blocking_obstacle) {
        // Only record the closest front blocking obstacle.
        distance_to_closest_blocking_obstacle =
            distance_between_adc_and_obstacle;
        front_blocking_obstacle_id_ = obstacle->Id() + "_0";
        side_pass_context_.front_blocking_obstacle_id_ = obstacle->Id();
        // TODO(all): to be removed when SidePass obstacle decision impl is ready
        PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id =
            side_pass_context_.front_blocking_obstacle_id_;
      }
    }
  }
  if (exists_a_blocking_obstacle) {
    return true;
  } else {
    side_pass_context_.front_blocking_obstacle_id_ = "";
    // TODO(all): to be removed when SidePass obstacle decision impl is ready
    PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id =
        side_pass_context_.front_blocking_obstacle_id_;
    return false;
  }
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fix lint<commit_after>
/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_scenario.h"

#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/time/time.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_backup.h"
#include "modules/planning/scenarios/side_pass/stage_detect_safety.h"
#include "modules/planning/scenarios/side_pass/stage_generate_path.h"
#include "modules/planning/scenarios/side_pass/stage_pass_obstacle.h"
#include "modules/planning/scenarios/side_pass/stage_stop_on_wait_point.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

apollo::common::util::Factory<
    ScenarioConfig::StageType, Stage,
    Stage* (*)(const ScenarioConfig::StageConfig& stage_config)>
    SidePassScenario::s_stage_factory_;

// The clearance distance from intersection/destination.
// If ADC is too close, then do not enter SIDE_PASS.
constexpr double kClearDistance = 15.0;

void SidePassScenario::RegisterStages() {
  s_stage_factory_.Clear();
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_APPROACH_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageApproachObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_DETECT_SAFETY,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageDetectSafety(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_GENERATE_PATH,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageGeneratePath(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageStopOnWaitPoint(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_PASS_OBSTACLE,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StagePassObstacle(config);
      });
  s_stage_factory_.Register(
      ScenarioConfig::SIDE_PASS_BACKUP,
      [](const ScenarioConfig::StageConfig& config) -> Stage* {
        return new StageBackup(config);
      });
}

SidePassScenario::SidePassScenario(const ScenarioConfig& config,
                                   const ScenarioContext* scenario_context)
    : Scenario(config, scenario_context) {
  side_pass_context_.scenario_config_.CopyFrom(config.side_pass_config());

  // TODO(all): to be removed when SidePass obstacle decision impl is ready
  side_pass_context_.front_blocking_obstacle_id_ =
      PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id;
}

std::unique_ptr<Stage> SidePassScenario::CreateStage(
    const ScenarioConfig::StageConfig& stage_config) {
  if (s_stage_factory_.Empty()) {
    RegisterStages();
  }
  auto ptr = s_stage_factory_.CreateObjectOrNull(stage_config.stage_type(),
                                                 stage_config);
  if (ptr == nullptr) {
    AERROR << "Failed to create stage for config: "
           << stage_config.DebugString();
    return nullptr;
  }
  ptr->SetContext(&side_pass_context_);
  return ptr;
}

bool SidePassScenario::IsTransferable(const Scenario& current_scenario,
                                      const Frame& frame) {
  // Sanity checks.
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  if (current_scenario.scenario_type() == ScenarioConfig::SIDE_PASS) {
    // Check if the blocking obstacle is still static.
    // If not, then switch to LANE_FOLLOW.
    const auto front_blocking_obstacle =
        const_cast<Frame&>(frame).Find(front_blocking_obstacle_id_);
    if (front_blocking_obstacle != nullptr &&
        !front_blocking_obstacle->IsStatic()) {
      AWARN << "Obstacle " << front_blocking_obstacle_id_
            << " starts to move. Change scenario from SIDE_PASS"
               " back to default scenario.";
      return false;
    }
    msg_ = "side pass obstacle: " + front_blocking_obstacle_id_;
    return (current_scenario.GetStatus() !=
            Scenario::ScenarioStatus::STATUS_DONE);
  } else if (current_scenario.scenario_type() != ScenarioConfig::LANE_FOLLOW) {
    // If in some other special scenario, then don't try to switch
    // to SIDE_PASS scenario.
    return false;
  } else {
    // If originally in LANE_FOLLOW, then decide whether we should
    // switch to SIDE_PASS scenario.
    bool is_side_pass = IsSidePassScenario(frame);
    if (is_side_pass) {
      msg_ = "side pass obstacle: " + front_blocking_obstacle_id_;
    }
    return is_side_pass;
  }
}

bool SidePassScenario::IsSidePassScenario(const Frame& frame) {
  return (IsFarFromDestination(frame) &&
          IsFarFromIntersection(frame) &&
          HasBlockingObstacle(frame));
}

bool SidePassScenario::IsFarFromDestination(const Frame& frame) {
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  const auto& reference_line_info = frame.reference_line_info().front();
  if (reference_line_info.SDistanceToDestination() < kClearDistance) {
    ADEBUG << "Too close to destination; don't SIDE_PASS";
    return false;
  }
  return true;
}

bool SidePassScenario::IsFarFromIntersection(const Frame& frame) {
  if (frame.reference_line_info().size() > 1) {
    return false;
  }
  const SLBoundary& adc_sl_boundary =
      frame.reference_line_info().front().AdcSlBoundary();
  const auto& first_encountered_overlaps =
      frame.reference_line_info().front().FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    ADEBUG << "AdcSL: " << adc_sl_boundary.ShortDebugString();
    ADEBUG << overlap.first << ", " << overlap.second.DebugString();
    if (overlap.first != ReferenceLineInfo::CLEAR_AREA &&
        overlap.first != ReferenceLineInfo::CROSSWALK &&
        // TODO(yifei) temporarily enable side pass at pnc junction without
        // stop sign and traffic light.
        // overlap.first != ReferenceLineInfo::PNC_JUNCTION &&
        overlap.first != ReferenceLineInfo::SIGNAL &&
        overlap.first != ReferenceLineInfo::STOP_SIGN) {
      continue;
    }

    auto distance = overlap.second.start_s - adc_sl_boundary.end_s();
    if (overlap.first == ReferenceLineInfo::SIGNAL) {
      if (distance < FLAGS_side_pass_min_signal_intersection_distance) {
        ADEBUG << "Too close to signal intersection ("
               << distance << "m); don't SIDE_PASS.";
        return false;
      }
    } else {
      if (distance < kClearDistance) {
        ADEBUG << "Too close to overlap_type[" << overlap.first
               << "] (" << distance << "m); don't SIDE_PASS";
        return false;
      }
    }
  }
  return true;
}

bool SidePassScenario::HasBlockingObstacle(const Frame& frame) {
  // Sanity checks.
  front_blocking_obstacle_id_ = "";
  if (frame.reference_line_info().size() > 1) {
    return false;
  }

  const auto& reference_line_info = frame.reference_line_info().front();
  const PathDecision& path_decision = reference_line_info.path_decision();
  double distance_to_closest_blocking_obstacle = -100.0;
  bool exists_a_blocking_obstacle = false;
  // A blocking obstacle is an obstacle blocks the road when it is not blocked
  // by other obstacles or traffic rules.
  // Loop through every obstacle to locate the closest blocking one, if there
  // exists such an obstacle.
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    const auto& side_pass_config = side_pass_context_.scenario_config_;
    if (IsBlockingObstacleToSidePass(
            frame, obstacle,
            side_pass_config.block_obstacle_min_speed(),
            side_pass_config.min_front_obstacle_distance(),
            side_pass_config.enable_obstacle_blocked_check())) {
      exists_a_blocking_obstacle = true;
      double distance_between_adc_and_obstacle =
          GetDistanceBetweenADCAndObstacle(frame, obstacle);
      if (distance_to_closest_blocking_obstacle < 0.0 ||
          distance_between_adc_and_obstacle <
              distance_to_closest_blocking_obstacle) {
        // Only record the closest front blocking obstacle.
        distance_to_closest_blocking_obstacle =
            distance_between_adc_and_obstacle;
        front_blocking_obstacle_id_ = obstacle->Id() + "_0";
        side_pass_context_.front_blocking_obstacle_id_ = obstacle->Id();
        // TODO(all): to be removed
        //            when SidePass obstacle decision impl is ready
        PlanningContext::GetScenarioInfo()
            ->side_pass_front_blocking_obstacle_id =
                side_pass_context_.front_blocking_obstacle_id_;
      }
    }
  }
  if (exists_a_blocking_obstacle) {
    return true;
  } else {
    side_pass_context_.front_blocking_obstacle_id_ = "";
    // TODO(all): to be removed when SidePass obstacle decision impl is ready
    PlanningContext::GetScenarioInfo()->side_pass_front_blocking_obstacle_id =
        side_pass_context_.front_blocking_obstacle_id_;
    return false;
  }
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL
#define SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL

#include <sofa/component/linearsolver/MinResLinearSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/component/linearsolver/FullMatrix.h>
#include <sofa/component/linearsolver/SparseMatrix.h>
#include <sofa/component/linearsolver/CompressedRowSparseMatrix.h>
#include <sofa/simulation/common/MechanicalVisitor.h>
#include <sofa/helper/system/thread/CTime.h>
#include <sofa/helper/AdvancedTimer.h>

#include <sofa/core/ObjectFactory.h>
#include <iostream>

namespace sofa
{

namespace component
{

namespace linearsolver
{
using core::VecId;
using namespace sofa::defaulttype;
using namespace sofa::core::behavior;
using namespace sofa::simulation;

/// Linear system solver using the conjugate gradient iterative algorithm
template<class TMatrix, class TVector>
MinResLinearSolver<TMatrix,TVector>::MinResLinearSolver()
    : f_maxIter( initData(&f_maxIter,(unsigned)25,"iterations","maximum number of iterations of the Conjugate Gradient solution") )
    , f_tolerance( initData(&f_tolerance,1e-5,"tolerance","desired precision of the Conjugate Gradient Solution (ratio of current residual norm over initial residual norm)") )
    , f_verbose( initData(&f_verbose,false,"verbose","Dump system state at each iteration") )
    , f_graph( initData(&f_graph,"graph","Graph of residuals at each iteration") )
{
    f_graph.setWidget("graph");
//    f_graph.setReadOnly(true);
}

template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::resetSystem()
{
    f_graph.beginEdit()->clear();
    f_graph.endEdit();

    Inherit::resetSystem();
}

template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::setSystemMBKMatrix(const sofa::core::MechanicalParams* mparams)
{
    Inherit::setSystemMBKMatrix(mparams);
}

/// Solve Ax=b
/// code issued (and modified) from tminres (https://code.google.com/p/tminres/)
// - Umberto Villa, Emory University - uvilla@emory.edu
// - Michael Saunders, Stanford University
// - Santiago Akle, Stanford University
template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::solve(Matrix& A, Vector& x, Vector& b)
{
    const double& tol = f_tolerance.getValue();
    const unsigned& max_iter = f_maxIter.getValue();

    std::map < std::string, sofa::helper::vector<double> >& graph = *f_graph.beginEdit();
    sofa::helper::vector<double>& graph_error = graph[(this->isMultiGroup()) ? this->currentNode->getName()+std::string("-Error") : std::string("Error")];
    graph_error.clear();
    graph_error.push_back(1);

    double eps(std::numeric_limits<double>::epsilon());
//    int istop(0);
    unsigned itn(0);
    double Anorm(0.0), Acond(0.0)/*, Arnorm(0.0)*/;
    double rnorm(0.0), ynorm(0.0);
    bool done(false);

    // Step 1
    /*
     * Set up y and v for the first Lanczos vector v1.
     * y = beta1 P' v1, where P = C^(-1).
     * v is really P'v1
     */

    const core::ExecParams* params = core::ExecParams::defaultInstance();
    typename Inherit::TempVectorContainer vtmp(this, params, A, x, b);
    Vector* r1 =  vtmp.createTempVector();
    Vector* r2 =  vtmp.createTempVector();
    Vector& y  = *vtmp.createTempVector();
    Vector* w  =  vtmp.createTempVector();
    Vector* w2 =  vtmp.createTempVector();
    Vector& v  = *vtmp.createTempVector();

    *r1 = A * x;
//    r1 = b - r1;
    *r1 *= -1;
    *r1 += b;

    double beta1(0.0);
    beta1 = r1->dot( *r1 );

    // Test for an indefined preconditioner
    // If b = 0 exactly stop with x = x0.

    if(beta1 < 0.0)
    {
        done = true;
    }
    else
    {
        if(beta1 == 0.0)
        {
            done = true;
        }
        else
        {
            beta1 = sqrt(beta1); // Normalize y to get v1 later

            y = *r1;
        }
    }

    // TODO: port symmetry checks for A

    // STEP 2
    /* Initialize other quantities */
    double oldb(0.0), beta(beta1), dbar(0.0), epsln(0.0), oldeps(0.0);
    double qrnorm(beta1), phi(0.0), phibar(beta1), rhs1(beta1);
    double rhs2(0.0), tnorm2(0.0), ynorm2(0.0);
    double cs(-1.0), sn(0.0);
    double gmax(0.0), gmin(std::numeric_limits<double>::max());
    double alpha(0.0), gamma(0.0);
    double delta(0.0), gbar(0.0);
    double z(0.0);

    if( !done )
    {
        /* Main Iteration */
        for(itn = 0; itn < max_iter; ++itn)
        {
            // STEP 3
            /*
            -----------------------------------------------------------------
            Obtain quantities for the next Lanczos vector vk+1, k = 1, 2,...
            The general iteration is similar to the case k = 1 with v0 = 0:

            p1      = Operator * v1  -  beta1 * v0,
            alpha1  = v1'p1,
            q2      = p2  -  alpha1 * v1,
            beta2^2 = q2'q2,
            v2      = (1/beta2) q2.

            Again, y = betak P vk,  where  P = C**(-1).
            .... more description needed.
            -----------------------------------------------------------------
             */

            *r2 = y;

            double s(1./beta); //Normalize previous vector (in y)
            v  = y;
            v *= s;         // v = vk if P = I

            y = A * v;
            if(itn) y.peq( *r1, -beta/oldb );

            alpha = v.dot( y );	// alphak
            y.peq( *r2, -alpha/beta ); // y += -a/b * r2

            std::swap( r1, r2 ); // save a copy by swaping pointers

            oldb = beta; //oldb = betak
            beta = y.dot( y );

            if(beta < 0)
            {
                break;
            }

            beta = sqrt(beta);
            tnorm2 += alpha*alpha + oldb*oldb + beta*beta;

            if(itn == 0)	//Initialize a few things
            {
                if(beta/beta1 < 10.0*eps)
                    done = true;
            }

            // Apply previous rotation Q_{k-1} to get
            // [delta_k epsln_{k+1}] = [cs sn]  [dbar_k 0]
            // [gbar_k   dbar_{k+1}]   [sn -cs] [alpha_k beta_{k+1}].
            oldeps = epsln;
            delta  = cs*dbar + sn*alpha;
            gbar   = sn*dbar - cs*alpha;
            epsln  =           sn*beta;
            dbar   =         - cs*beta;
            double root(sqrt(gbar*gbar + dbar*dbar));
            //Arnorm = phibar * root; // ||Ar_{k-1}||

            // Compute next plane rotation Q_k
            gamma = sqrt(gbar*gbar + beta*beta); // gamma_k
            gamma = std::max(gamma, eps);
            cs = gbar/gamma;                     // c_k
            sn = beta/gamma;                     // s_k
            phi = cs*phibar;                     // phi_k
            phibar = sn*phibar;                  // phibar_{k+1}

            // Update x
            double denom(1./gamma);

            std::swap( w, w2 );

            *w *= -oldeps;
            w->peq( *w2, -delta );
        //            add(denom, v, w, w);
            *w *= denom;
            w->peq( v, denom );
        //            add(x, phi, w, x);
            x.peq( *w, phi );

            // go round again
            gmax    = std::max(gmax, gamma);
            gmin    = std::min(gmin, gamma);
            z       = rhs1/gamma;
            rhs1    = rhs2 - delta*z;
            rhs2    =      - epsln*z;

            // Estimate various norms

            Anorm = sqrt(tnorm2);
            ynorm2 = x.dot( x );
            ynorm = sqrt(ynorm2);
            double epsa(Anorm*eps);
            double epsx(epsa*ynorm);
        //            double epsr(Anorm*ynorm*tol);
            double diag(gbar);
            if(0 == diag)
                diag = epsa;

            qrnorm = phibar;
            rnorm  = qrnorm;
            double test1(0.0), test2(0.0);
            test1  = rnorm / (Anorm*ynorm); // ||r||/(||A|| ||x||)
            test2  = root / Anorm;         // ||A r_{k-1}|| / (||A|| ||r_{k-1}||)

            graph_error.push_back(test1);

            // Estimate cond(A)
            /*
             In this version we look at the diagonals of  R  in the
             factorization of the lower Hessenberg matrix,  Q * H = R,
             where H is the tridiagonal matrix from Lanczos with one
             extra row, beta(k+1) e_k^T.
             */
            Acond = gmax/gmin;

            if( done ) break;

            //See if any of the stopping criteria is satisfied
            double t1(1.0+test1), t2(1.0+test2); //This test work if tol < eps
            if( t2 <= 1. ||
                t1 <= 1.||
                itn >= max_iter-1||
                Acond >= .1/eps||
                epsx >= beta1  ||
                test2 <= tol   ||
                test1 <= tol  ) break;
        }
    }

    vtmp.deleteTempVector(r1);
    vtmp.deleteTempVector(r2);
    vtmp.deleteTempVector(&y);
    vtmp.deleteTempVector(w);
    vtmp.deleteTempVector(w2);
    vtmp.deleteTempVector(&v);
}

} // namespace linearsolver

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL
<commit_msg>r9224/sofa : MINRES optimization<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL
#define SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL

#include <sofa/component/linearsolver/MinResLinearSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/component/linearsolver/FullMatrix.h>
#include <sofa/component/linearsolver/SparseMatrix.h>
#include <sofa/component/linearsolver/CompressedRowSparseMatrix.h>
#include <sofa/simulation/common/MechanicalVisitor.h>
#include <sofa/helper/system/thread/CTime.h>
#include <sofa/helper/AdvancedTimer.h>

#include <sofa/core/ObjectFactory.h>
#include <iostream>

namespace sofa
{

namespace component
{

namespace linearsolver
{
using core::VecId;
using namespace sofa::defaulttype;
using namespace sofa::core::behavior;
using namespace sofa::simulation;

/// Linear system solver using the conjugate gradient iterative algorithm
template<class TMatrix, class TVector>
MinResLinearSolver<TMatrix,TVector>::MinResLinearSolver()
    : f_maxIter( initData(&f_maxIter,(unsigned)25,"iterations","maximum number of iterations of the Conjugate Gradient solution") )
    , f_tolerance( initData(&f_tolerance,1e-5,"tolerance","desired precision of the Conjugate Gradient Solution (ratio of current residual norm over initial residual norm)") )
    , f_verbose( initData(&f_verbose,false,"verbose","Dump system state at each iteration") )
    , f_graph( initData(&f_graph,"graph","Graph of residuals at each iteration") )
{
    f_graph.setWidget("graph");
//    f_graph.setReadOnly(true);
}

template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::resetSystem()
{
    f_graph.beginEdit()->clear();
    f_graph.endEdit();

    Inherit::resetSystem();
}

template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::setSystemMBKMatrix(const sofa::core::MechanicalParams* mparams)
{
    Inherit::setSystemMBKMatrix(mparams);
}

/// Solve Ax=b
/// code issued (and modified) from tminres (https://code.google.com/p/tminres/)
// - Umberto Villa, Emory University - uvilla@emory.edu
// - Michael Saunders, Stanford University
// - Santiago Akle, Stanford University
template<class TMatrix, class TVector>
void MinResLinearSolver<TMatrix,TVector>::solve(Matrix& A, Vector& x, Vector& b)
{
    const double& tol = f_tolerance.getValue();
    const unsigned& max_iter = f_maxIter.getValue();

    std::map < std::string, sofa::helper::vector<double> >& graph = *f_graph.beginEdit();
    sofa::helper::vector<double>& graph_error = graph[(this->isMultiGroup()) ? this->currentNode->getName()+std::string("-Error") : std::string("Error")];
    graph_error.clear();
    graph_error.push_back(1);

    double eps(std::numeric_limits<double>::epsilon());
//    int istop(0);
    unsigned itn(0);
    double Anorm(0.0), Acond(0.0)/*, Arnorm(0.0)*/;
    double rnorm(0.0), ynorm(0.0);
    bool done(false);

    // Step 1
    /*
     * Set up y and v for the first Lanczos vector v1.
     * y = beta1 P' v1, where P = C^(-1).
     * v is really P'v1
     */

    const core::ExecParams* params = core::ExecParams::defaultInstance();
    typename Inherit::TempVectorContainer vtmp(this, params, A, x, b);
    Vector* r1 =  vtmp.createTempVector();
    Vector* r2 =  vtmp.createTempVector();
    Vector& y  = *vtmp.createTempVector();
    Vector* w  =  vtmp.createTempVector();
    Vector* w2 =  vtmp.createTempVector();
    Vector& v  = *vtmp.createTempVector();

    *r1 = A * x;
//    r1 = b - r1;
    *r1 *= -1;
    *r1 += b;

    double beta1(0.0);
    beta1 = r1->dot( *r1 );

    // Test for an indefined preconditioner
    // If b = 0 exactly stop with x = x0.

    if(beta1 < 0.0)
    {
        done = true;
    }
    else
    {
        if(beta1 == 0.0)
        {
            done = true;
        }
        else
        {
            beta1 = sqrt(beta1); // Normalize y to get v1 later

            y = *r1;
        }
    }

    // TODO: port symmetry checks for A

    // STEP 2
    /* Initialize other quantities */
    double oldb(0.0), beta(beta1), dbar(0.0), epsln(0.0), oldeps(0.0);
    double qrnorm(beta1), phi(0.0), phibar(beta1), rhs1(beta1);
    double rhs2(0.0), tnorm2(0.0), ynorm2(0.0);
    double cs(-1.0), sn(0.0);
    double gmax(0.0), gmin(std::numeric_limits<double>::max());
    double alpha(0.0), gamma(0.0);
    double delta(0.0), gbar(0.0);
    double z(0.0);

    if( !done )
    {
        /* Main Iteration */
        for(itn = 0; itn < max_iter; ++itn)
        {
            // STEP 3
            /*
            -----------------------------------------------------------------
            Obtain quantities for the next Lanczos vector vk+1, k = 1, 2,...
            The general iteration is similar to the case k = 1 with v0 = 0:

            p1      = Operator * v1  -  beta1 * v0,
            alpha1  = v1'p1,
            q2      = p2  -  alpha1 * v1,
            beta2^2 = q2'q2,
            v2      = (1/beta2) q2.

            Again, y = betak P vk,  where  P = C**(-1).
            .... more description needed.
            -----------------------------------------------------------------
             */

            *r2 = y;

            double s(1./beta); //Normalize previous vector (in y)
            v  = y;
            v *= s;         // v = vk if P = I

            y = A * v;
            if(itn) y.peq( *r1, -beta/oldb );

            alpha = v.dot( y );	// alphak
            y.peq( *r2, -alpha/beta ); // y += -a/b * r2

            std::swap( r1, r2 ); // save a copy by swaping pointers

            oldb = beta; //oldb = betak
            beta = y.dot( y );

            if(beta < 0)
            {
                break;
            }

            beta = sqrt(beta);
            tnorm2 += alpha*alpha + oldb*oldb + beta*beta;

            if(itn == 0)	//Initialize a few things
            {
                if(beta/beta1 < 10.0*eps)
                    done = true;
            }

            // Apply previous rotation Q_{k-1} to get
            // [delta_k epsln_{k+1}] = [cs sn]  [dbar_k 0]
            // [gbar_k   dbar_{k+1}]   [sn -cs] [alpha_k beta_{k+1}].
            oldeps = epsln;
            delta  = cs*dbar + sn*alpha;
            gbar   = sn*dbar - cs*alpha;
            epsln  =           sn*beta;
            dbar   =         - cs*beta;
            double root(sqrt(gbar*gbar + dbar*dbar));
            //Arnorm = phibar * root; // ||Ar_{k-1}||

            // Compute next plane rotation Q_k
            gamma = sqrt(gbar*gbar + beta*beta); // gamma_k
            gamma = std::max(gamma, eps);

            double denom(1./gamma);

            cs = gbar*denom;                     // c_k
            sn = beta*denom;                     // s_k
            phi = cs*phibar;                     // phi_k
            phibar = sn*phibar;                  // phibar_{k+1}

            // Update x

            std::swap( w, w2 );

            *w *= -oldeps;
            w->peq( *w2, -delta );
        //            add(denom, v, w, w);
            *w *= denom;
            w->peq( v, denom );
        //            add(x, phi, w, x);
            x.peq( *w, phi );

            // go round again
            gmax    = std::max(gmax, gamma);
            gmin    = std::min(gmin, gamma);
            z       = rhs1*denom;
            rhs1    = rhs2 - delta*z;
            rhs2    =      - epsln*z;

            // Estimate various norms

            Anorm = sqrt(tnorm2);
            ynorm2 = x.dot( x );
            ynorm = sqrt(ynorm2);
            double epsa(Anorm*eps);
            double epsx(epsa*ynorm);
        //            double epsr(Anorm*ynorm*tol);
            double diag(gbar);
            if(0 == diag)
                diag = epsa;

            qrnorm = phibar;
            rnorm  = qrnorm;
            double test1(0.0), test2(0.0);
            test1  = rnorm / (Anorm*ynorm); // ||r||/(||A|| ||x||)
            test2  = root / Anorm;         // ||A r_{k-1}|| / (||A|| ||r_{k-1}||)

            graph_error.push_back(test1);

            // Estimate cond(A)
            /*
             In this version we look at the diagonals of  R  in the
             factorization of the lower Hessenberg matrix,  Q * H = R,
             where H is the tridiagonal matrix from Lanczos with one
             extra row, beta(k+1) e_k^T.
             */
            Acond = gmax/gmin;

            if( done ) break;

            //See if any of the stopping criteria is satisfied
            double t1(1.0+test1), t2(1.0+test2); //This test work if tol < eps
            if( t2 <= 1. ||
                t1 <= 1.||
                itn >= max_iter-1||
                Acond >= .1/eps||
                epsx >= beta1  ||
                test2 <= tol   ||
                test1 <= tol  ) break;
        }
    }

    vtmp.deleteTempVector(r1);
    vtmp.deleteTempVector(r2);
    vtmp.deleteTempVector(&y);
    vtmp.deleteTempVector(w);
    vtmp.deleteTempVector(w2);
    vtmp.deleteTempVector(&v);
}

} // namespace linearsolver

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_LINEARSOLVER_MinResLinearSolver_INL
<|endoftext|>


<commit_before>/*
 * System.cpp
 *
 *  Created on: Apr 29, 2014
 *      Author: Pimenta
 */

// this
#include "System.hpp"

// standard
#include <cstdio>

// lib
#include <SDL_net.h>
#include <SDL_stdinc.h>

// local
#include "Globals.hpp"
#include "Lockable.hpp"
#include "SystemDetectFailure.hpp"
#include "SystemListen.hpp"
#include "SystemSpeak.hpp"
#include "Define.hpp"

Thread<System::run>* System::thread = nullptr;
bool System::running = false;

bool System::start() {
  if (running)
    return false;
  running = true;
  thread = new Thread<run>;
  thread->start();
  return true;
}

bool System::stop() {
  if (!running)
    return false;
  running = false;
  Globals::get<bool>("systemOn").value() = false;
  thread->join();
  return true;
}

bool System::isRunning() {
  return running;
}

static void printAddr(Uint32 host) {
  printf("%d", ((Uint8*)&host)[0]);
  for (int i = 1; i < 4; i++)
    printf(".%d", ((Uint8*)&host)[i]);
  printf("\n");
}

void System::run() {
  // init
  Globals::init();

  // resolve ip
  {
    UDPsocket sock = SDLNet_UDP_Open(0);
    UDPsocket socklist = SDLNet_UDP_Open(FD8_UDP_PORT_RESOLVE_IP);
    UDPpacket* pack = SDLNet_AllocPacket(1);
    IPaddress ip;
    SDLNet_ResolveHost(&ip, "255.255.255.255", FD8_UDP_PORT_RESOLVE_IP);
    pack->address.host = ip.host;
    pack->address.port = ip.port;
    pack->len = 1;
    *((Uint8*)pack->data) = (Uint8)0xFF;
    SDLNet_UDP_Send(sock, -1, pack);
    SDLNet_UDP_Recv(socklist, pack);
    printAddr(pack->address.host);
    SDLNet_FreePacket(pack);
  }

  // all system threads
  Thread<SystemSpeak> speak;
  Thread<SystemListen> listen;
  Thread<SystemDetectFailure> detectFailure;

  // start all threads
  speak.start();
  listen.start();
  detectFailure.start();

  // join all threads
  speak.join();
  listen.join();
  detectFailure.join();

  // close
  Globals::close();
}
<commit_msg>Resolving peer ip<commit_after>/*
 * System.cpp
 *
 *  Created on: Apr 29, 2014
 *      Author: Pimenta
 */

// this
#include "System.hpp"

// standard
#include <cstdio>

// lib
#include <SDL_net.h>
#include <SDL_stdinc.h>

// local
#include "Globals.hpp"
#include "Lockable.hpp"
#include "SystemDetectFailure.hpp"
#include "SystemListen.hpp"
#include "SystemSpeak.hpp"
#include "Define.hpp"

Thread<System::run>* System::thread = nullptr;
bool System::running = false;

bool System::start() {
  if (running)
    return false;
  running = true;
  thread = new Thread<run>;
  thread->start();
  return true;
}

bool System::stop() {
  if (!running)
    return false;
  running = false;
  Globals::get<bool>("systemOn").value() = false;
  thread->join();
  return true;
}

bool System::isRunning() {
  return running;
}

static void printAddr(Uint32 host) {
  printf("%d", ((Uint8*)&host)[0]);
  for (int i = 1; i < 4; i++)
    printf(".%d", ((Uint8*)&host)[i]);
  printf("\n");
}

void System::run() {
  // init
  Globals::init();

  // resolve ip
  {
    UDPsocket sock = SDLNet_UDP_Open(0);
    UDPsocket socklist = SDLNet_UDP_Open(FD8_UDP_PORT_RESOLVE_IP);
    UDPpacket* pack = SDLNet_AllocPacket(1);
    IPaddress ip;
    SDLNet_ResolveHost(&ip, "255.255.255.255", FD8_UDP_PORT_RESOLVE_IP);
    pack->address.host = ip.host;
    pack->address.port = ip.port;
    pack->len = 1;
    *((Uint8*)pack->data) = (Uint8)0xFF;
    SDLNet_UDP_Send(sock, -1, pack);
    /*while (true) {
      SDLNet_UDP_Recv(socklist, pack);
      char port[6];
      sprintf(port, "%d", pack->address.port);

    }*/
    while (SDLNet_UDP_Recv(socklist, pack) != 1);
    printf("local address: ");
    printAddr(pack->address.host);
    SDLNet_FreePacket(pack);
    SDLNet_UDP_Close(sock);
    SDLNet_UDP_Close(socklist);
  }

  // all system threads
  Thread<SystemSpeak> speak;
  Thread<SystemListen> listen;
  Thread<SystemDetectFailure> detectFailure;

  // start all threads
  speak.start();
  listen.start();
  detectFailure.start();

  // join all threads
  speak.join();
  listen.join();
  detectFailure.join();

  // close
  Globals::close();
}
<|endoftext|>


<commit_before>//
//  Starcounter Lisp
//
//  Vector.cpp
//  Planned Implementation of a persistent bit-partitioned vector trie.
//  Pioneered by Phil Bagwell: http://lampwww.epfl.ch/papers/idealhashtrees.pdf
//  Needs unit tests badly.
//
//  Made popular in Clojure as their persistent vector.
//  https://www.youtube.com/watch?v=pNhBQJN44YQ
//  http://www.infoq.com/presentations/Value-Identity-State-Rich-Hickey
//
//  Created by Joachim Wester on 22/04/16.
//  Copyright © 2016 Starcounter AB. All rights reserved.
//

#include "Vector.hpp"

<commit_msg>Update Vector.cpp<commit_after>//
//  Starcounter Lisp
//
//  Vector.cpp
//  Planned Implementation of a persistent bit-partitioned vector trie.
//  Needs unit tests badly.
//
//  Made popular in Clojure as their persistent vector.
//  https://www.youtube.com/watch?v=pNhBQJN44YQ
//  http://www.infoq.com/presentations/Value-Identity-State-Rich-Hickey
//
//  Created by Joachim Wester on 22/04/16.
//  Copyright © 2016 Starcounter AB. All rights reserved.
//

#include "Vector.hpp"

<|endoftext|>


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

/* Author: Ioan Sucan */

#include <move_group_interface/move_group.h>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/join.hpp>
#include <ros/ros.h>

void printHelp(void)
{
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group_cmdline", ros::init_options::AnonymousName);

  if (argc < 2)
  {
    std::cerr << "Usage: " << argv[0] << " <group_name>" << std::endl;
    return 1;
  }

  ros::AsyncSpinner spinner(1);
  spinner.start();

  move_group_interface::MoveGroup group(argv[1]);
  std::cout << std::endl;
  std::cout << "Waiting for commands. Type help to get a list of known commands." << std::endl << std::endl;

  while (ros::ok())
  {
    std::cout << "> ";
    std::string cmd;
    std::getline(std::cin, cmd);
    if (!std::cin.good())
      break;
    boost::trim(cmd);
    if (cmd == "help")
      printHelp();
    else
      if (cmd == "quit" || cmd == "exit" || cmd == "q")
        break;
      else
        if (cmd.substr(0, 6) == "record")
        {
          std::string name = cmd.substr(6);
          boost::trim(name);
          group.rememberJointValues(name);
          std::cout << "Remembered current joint values under the name '" << name << "'." << std::endl;
        }
        else
          if (cmd.substr(0, 2) == "go")
          {
            std::string name = cmd.substr(2);
            boost::trim(name);
            if (group.setNamedTarget(name))
            {
              if (group.move())
                std::cout << "Moved to '" << name << "'" << std::endl;
              else
                std::cout << "Failed while moving to '" << name << "'" << std::endl;
            }
            else
              std::cout << "'" << name << "' is unknown" << std::endl;
          }
          else
            if (cmd.substr(0, 3) == "set")
            {
              std::string line = cmd.substr(3);
              boost::trim(line);
              std::stringstream s(line);
              std::string name; s >> name;
              std::vector<double> jv;
              std::string jvs;
              while (s.good() && !s.eof())
              {
                double v; s >> v;
                jvs = jvs + " " + boost::lexical_cast<std::string>(v);
                jv.push_back(v);
              }
              group.rememberJointValues(name, jv);
              std::cout << "Remembered specified joint values [" << jvs << " ] under the name '" << name << "'." << std::endl;
            }
            else
              if (cmd.substr(0, 3) == "del")
              {
                std::string name = cmd.substr(3);
                boost::trim(name);
                group.forgetJointValues(name);
              }
              else
                if (cmd == "show")
                {
                  const std::map<std::string, std::vector<double> >& jv = group.getRememberedJointValues();
                  for (std::map<std::string, std::vector<double> >::const_iterator it = jv.begin() ; it != jv.end() ; ++it)
                  {
                    std::cout << it->first << " = [";
                    std::string v;
                    for (std::size_t i = 0; i < it->second.size() ; ++i)
                      v = v + " " + boost::lexical_cast<std::string>(it->second[i]);
                    std::cout << v << " ]" << std::endl;
                  }
                }
                else
                {
                  const std::map<std::string, std::vector<double> >& jv = group.getRememberedJointValues();
                  std::map<std::string, std::vector<double> >::const_iterator it = jv.find(cmd);
                  if (it != jv.end())
                  {
                    std::string v;
                    for (std::size_t i = 0; i < it->second.size() ; ++i)
                      v = v + " " + boost::lexical_cast<std::string>(it->second[i]);
                    std::cout << v << std::endl;
                  }
                  else
                    std::cerr << "Unknown command: '" << cmd << "'" << std::endl;
                }
  }
  std::cout << "\nBye bye!" << std::endl;
  return 0;
}
<commit_msg>add random state command<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <move_group_interface/move_group.h>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/join.hpp>
#include <ros/ros.h>

void printHelp(void)
{
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group_cmdline", ros::init_options::AnonymousName);

  if (argc < 2)
  {
    std::cerr << "Usage: " << argv[0] << " <group_name>" << std::endl;
    return 1;
  }

  ros::AsyncSpinner spinner(1);
  spinner.start();

  move_group_interface::MoveGroup group(argv[1]);
  std::cout << std::endl;
  std::cout << "Waiting for commands. Type help to get a list of known commands." << std::endl << std::endl;

  while (ros::ok())
  {
    std::cout << "> ";
    std::string cmd;
    std::getline(std::cin, cmd);
    if (!std::cin.good())
      break;
    boost::trim(cmd);
    if (cmd == "help")
      printHelp();
    else
      if (cmd == "quit" || cmd == "exit" || cmd == "q")
        break;
      else
        if (cmd.substr(0, 6) == "record")
        {
          std::string name = cmd.substr(6);
          boost::trim(name);
          group.rememberJointValues(name);
          std::cout << "Remembered current joint values under the name '" << name << "'." << std::endl;
        }
        else
          if (cmd.substr(0, 2) == "go")
          {
            std::string name = cmd.substr(2);
            boost::trim(name);
            if (group.setNamedTarget(name))
            {
              if (group.move())
                std::cout << "Moved to '" << name << "'" << std::endl;
              else
                std::cout << "Failed while moving to '" << name << "'" << std::endl;
            }
            else
              std::cout << "'" << name << "' is unknown" << std::endl;
          }
          else
            if (cmd.substr(0, 3) == "set")
            {
              std::string line = cmd.substr(3);
              boost::trim(line);
              std::stringstream s(line);
              std::string name; s >> name;
              std::vector<double> jv;
              std::string jvs;
              while (s.good() && !s.eof())
              {
                double v; s >> v;
                jvs = jvs + " " + boost::lexical_cast<std::string>(v);
                jv.push_back(v);
              }
              group.rememberJointValues(name, jv);
              std::cout << "Remembered specified joint values [" << jvs << " ] under the name '" << name << "'." << std::endl;
            }
            else
              if (cmd.substr(0, 4) == "rand")
	      {
		group.setRandomTarget();
		if (group.move())
		  std::cout << "Moved to random target" << std::endl;
		else
		  std::cout << "Failed while moving to random target." << std::endl;
	      }
	      else
	      if (cmd.substr(0, 3) == "del")
	      {
                std::string name = cmd.substr(3);
                boost::trim(name);
                group.forgetJointValues(name);
              }
              else
                if (cmd == "show")
                {
                  const std::map<std::string, std::vector<double> >& jv = group.getRememberedJointValues();
                  for (std::map<std::string, std::vector<double> >::const_iterator it = jv.begin() ; it != jv.end() ; ++it)
                  {
                    std::cout << it->first << " = [";
                    std::string v;
                    for (std::size_t i = 0; i < it->second.size() ; ++i)
                      v = v + " " + boost::lexical_cast<std::string>(it->second[i]);
                    std::cout << v << " ]" << std::endl;
                  }
                }
                else
                {
                  const std::map<std::string, std::vector<double> >& jv = group.getRememberedJointValues();
                  std::map<std::string, std::vector<double> >::const_iterator it = jv.find(cmd);
                  if (it != jv.end())
                  {
                    std::string v;
                    for (std::size_t i = 0; i < it->second.size() ; ++i)
                      v = v + " " + boost::lexical_cast<std::string>(it->second[i]);
                    std::cout << v << std::endl;
                  }
                  else
                    std::cerr << "Unknown command: '" << cmd << "'" << std::endl;
                }
  }
  std::cout << "\nBye bye!" << std::endl;
  return 0;
}
<|endoftext|>


<commit_before>/**
*  Copyright (C) 2015 3D Repo Ltd
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU Affero General Public License as
*  published by the Free Software Foundation, either version 3 of the
*  License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU Affero General Public License for more details.
*
*  You should have received a copy of the GNU Affero General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cstdlib>

#include <gtest/gtest.h>

#include <repo/core/model/bson/repo_node_mesh.h>
#include <repo/core/model/bson/repo_node_texture.h>
#include <repo/core/model/bson/repo_node_transformation.h>
#include <repo/core/model/bson/repo_bson_factory.h>
#include <repo/core/model/collection/repo_scene.h>

#include "../../../../repo_test_utils.h"

using namespace repo::core::model;

const static RepoScene::GraphType defaultG = RepoScene::GraphType::DEFAULT;

static RepoBSON makeRandomNode(
	const std::string &name = "")
{
	return RepoBSONFactory::appendDefaults("", 0U, generateUUID(), name);
}

static RepoBSON makeRandomNode(
	const repoUUID &parent,
	const std::string &name = "")
{
	return RepoBSONFactory::appendDefaults("", 0U, generateUUID(), name, { parent });
}

TEST(RepoSceneTest, Constructor)
{
	//Just to make sure it doesn't throw exceptiosn with empty
	RepoScene scene;
	RepoNodeSet empty;
	std::vector<std::string> files;
	RepoScene scene2(files, empty, empty, empty, empty, empty, empty);
}

TEST(RepoSceneTest, FilterNodesByType)
{
	std::vector<RepoNode*> nodes;
	TextureNode texNode;
	MeshNode meshNode;
	TransformationNode transNode;
	//Check empty vector doesn't crash
	EXPECT_EQ(0, RepoScene::filterNodesByType(nodes, NodeType::MAP).size());

	int nMeshes = rand() % 10 + 1;
	int nTrans = rand() % 10 + 1;
	int nTexts = rand() % 10 + 1;
	for (int i = 0; i < nMeshes; ++i)
		nodes.push_back(&meshNode);

	for (int i = 0; i < nTrans; ++i)
		nodes.push_back(&transNode);

	for (int i = 0; i < nTexts; ++i)
		nodes.push_back(&texNode);

	auto meshNodes = RepoScene::filterNodesByType(nodes, NodeType::MESH);
	EXPECT_EQ(nMeshes, meshNodes.size());
	for (const auto &m : meshNodes)
		EXPECT_EQ(m, &meshNode);

	auto transNodes = RepoScene::filterNodesByType(nodes, NodeType::TRANSFORMATION);
	EXPECT_EQ(nTrans, transNodes.size());
	for (const auto &m : transNodes)
		EXPECT_EQ(m, &transNode);

	auto textNodes = RepoScene::filterNodesByType(nodes, NodeType::TEXTURE);
	EXPECT_EQ(nTexts, textNodes.size());
	for (const auto &m : textNodes)
		EXPECT_EQ(m, &texNode);
}

TEST(RepoSceneTest, StatusCheck)
{
	RepoScene emptyScene;
	EXPECT_TRUE(emptyScene.isOK());
	EXPECT_FALSE(emptyScene.isMissingTexture());

	RepoNodeSet empty;
	std::vector<std::string> files;
	RepoScene scene2(files, empty, empty, empty, empty, empty, empty);

	EXPECT_TRUE(scene2.isOK());
	EXPECT_FALSE(scene2.isMissingTexture());

	emptyScene.setMissingTexture();
	EXPECT_FALSE(emptyScene.isOK());
	EXPECT_TRUE(emptyScene.isMissingTexture());
}

TEST(RepoSceneTest, AddMetadata)
{
	RepoNodeSet transNodes;
	RepoNodeSet meshNodes;
	RepoNodeSet metaNodes;
	RepoNodeSet empty;

	auto root = TransformationNode(makeRandomNode(getRandomString(rand() % 10 + 1)));

	auto t1 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t2 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t3 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));

	auto m1 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m2 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m3 = MeshNode(makeRandomNode(t2.getSharedID()));

	auto mm1 = MetadataNode(makeRandomNode(t1.getName()));
	auto mm2 = MetadataNode(makeRandomNode(t2.getName()));
	auto mm3 = MetadataNode(makeRandomNode(t3.getName()));

	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	metaNodes.insert(new MetadataNode(mm1));
	metaNodes.insert(new MetadataNode(mm2));
	metaNodes.insert(new MetadataNode(mm3));

	RepoScene scene,
		scene2(std::vector<std::string>(), empty, meshNodes, empty, empty, empty, transNodes);
	scene.addMetadata(metaNodes, true);
	EXPECT_EQ(0, scene.getAllMetadata(defaultG).size());
	scene2.addMetadata(metaNodes, true, false); //no propagation check
	auto scene2Meta = scene2.getAllMetadata(defaultG);
	EXPECT_EQ(metaNodes.size(), scene2Meta.size());
	for (const auto &s2meta : scene2Meta)
	{
		auto parents = s2meta->getParentIDs();
		EXPECT_EQ(1, parents.size());
		for (const auto &parent : parents)
		{
			auto node = scene2.getNodeBySharedID(RepoScene::GraphType::DEFAULT, parent);
			EXPECT_EQ(NodeType::TRANSFORMATION, node->getTypeAsEnum());
			EXPECT_EQ(s2meta->getName(), node->getName());
		}
	}

	transNodes.clear();
	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.clear();
	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	metaNodes.clear();
	metaNodes.insert(new MetadataNode(mm1));
	metaNodes.insert(new MetadataNode(mm2));
	metaNodes.insert(new MetadataNode(mm3));

	RepoScene scene3(std::vector<std::string>(), empty, meshNodes, empty, empty, empty, transNodes);

	scene3.addMetadata(metaNodes, true, true); //propagation check

	auto scene3Meta = scene3.getAllMetadata(defaultG);
	EXPECT_EQ(metaNodes.size(), scene2Meta.size());
	for (const auto &s3meta : scene3Meta)
	{
		auto parents = s3meta->getParentIDs();
		EXPECT_TRUE(parents.size() > 0);
		for (const auto &parent : parents)
		{
			auto node = scene3.getNodeBySharedID(RepoScene::GraphType::DEFAULT, parent);

			if (NodeType::TRANSFORMATION == node->getTypeAsEnum())
				EXPECT_EQ(s3meta->getName(), node->getName());
			else
			{
				EXPECT_EQ(NodeType::MESH, node->getTypeAsEnum());
			}
		}
	}
}

TEST(RepoSceneTest, AddAndClearStashGraph)
{
	RepoScene scene;
	RepoNodeSet empty;
	auto stashGraph = RepoScene::GraphType::OPTIMIZED;

	EXPECT_FALSE(scene.hasRoot(stashGraph));
	scene.addStashGraph(empty, empty, empty, empty, empty);
	EXPECT_FALSE(scene.hasRoot(stashGraph));

	RepoNodeSet transNodes;
	RepoNodeSet meshNodes;
	RepoNodeSet metaNodes;

	auto root = TransformationNode(makeRandomNode(getRandomString(rand() % 10 + 1)));

	auto t1 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t2 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t3 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));

	auto m1 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m2 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m3 = MeshNode(makeRandomNode(t2.getSharedID()));

	auto rootNode = new TransformationNode(root);
	transNodes.insert(rootNode);
	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	scene.addStashGraph(empty, meshNodes, empty, empty, transNodes);
	EXPECT_EQ(0, scene.getAllCameras(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTextures(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMaterials(stashGraph).size());
	EXPECT_EQ(meshNodes.size(), scene.getAllMeshes(stashGraph).size());
	EXPECT_EQ(transNodes.size(), scene.getAllTransformations(stashGraph).size());
	EXPECT_TRUE(scene.hasRoot(stashGraph));
	EXPECT_FALSE(scene.hasRoot(RepoScene::GraphType::DEFAULT));
	EXPECT_EQ(rootNode, scene.getRoot(stashGraph));

	scene.clearStash();

	EXPECT_EQ(0, scene.getAllCameras(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTextures(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMaterials(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMeshes(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTransformations(stashGraph).size());
	EXPECT_FALSE(scene.hasRoot(stashGraph));
	EXPECT_FALSE(scene.hasRoot(RepoScene::GraphType::DEFAULT));
	EXPECT_EQ(nullptr, scene.getRoot(stashGraph));
}<commit_msg>#13 missing unit test file from last commit<commit_after>/**
*  Copyright (C) 2015 3D Repo Ltd
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU Affero General Public License as
*  published by the Free Software Foundation, either version 3 of the
*  License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU Affero General Public License for more details.
*
*  You should have received a copy of the GNU Affero General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cstdlib>

#include <gtest/gtest.h>

#include <repo/core/model/bson/repo_node_mesh.h>
#include <repo/core/model/bson/repo_node_texture.h>
#include <repo/core/model/bson/repo_node_transformation.h>
#include <repo/core/model/bson/repo_bson_factory.h>
#include <repo/core/model/collection/repo_scene.h>

#include "../../../../repo_test_utils.h"
#include "../../../../repo_test_database_info.h"

using namespace repo::core::model;

const static RepoScene::GraphType defaultG = RepoScene::GraphType::DEFAULT;

static RepoBSON makeRandomNode(
	const std::string &name = "")
{
	return RepoBSONFactory::appendDefaults("", 0U, generateUUID(), name);
}

static RepoBSON makeRandomNode(
	const repoUUID &parent,
	const std::string &name = "")
{
	return RepoBSONFactory::appendDefaults("", 0U, generateUUID(), name, { parent });
}

TEST(RepoSceneTest, Constructor)
{
	//Just to make sure it doesn't throw exceptiosn with empty
	RepoScene scene;
	RepoNodeSet empty;
	std::vector<std::string> files;
	RepoScene scene2(files, empty, empty, empty, empty, empty, empty);
}

TEST(RepoSceneTest, FilterNodesByType)
{
	std::vector<RepoNode*> nodes;
	TextureNode texNode;
	MeshNode meshNode;
	TransformationNode transNode;
	//Check empty vector doesn't crash
	EXPECT_EQ(0, RepoScene::filterNodesByType(nodes, NodeType::MAP).size());

	int nMeshes = rand() % 10 + 1;
	int nTrans = rand() % 10 + 1;
	int nTexts = rand() % 10 + 1;
	for (int i = 0; i < nMeshes; ++i)
		nodes.push_back(&meshNode);

	for (int i = 0; i < nTrans; ++i)
		nodes.push_back(&transNode);

	for (int i = 0; i < nTexts; ++i)
		nodes.push_back(&texNode);

	auto meshNodes = RepoScene::filterNodesByType(nodes, NodeType::MESH);
	EXPECT_EQ(nMeshes, meshNodes.size());
	for (const auto &m : meshNodes)
		EXPECT_EQ(m, &meshNode);

	auto transNodes = RepoScene::filterNodesByType(nodes, NodeType::TRANSFORMATION);
	EXPECT_EQ(nTrans, transNodes.size());
	for (const auto &m : transNodes)
		EXPECT_EQ(m, &transNode);

	auto textNodes = RepoScene::filterNodesByType(nodes, NodeType::TEXTURE);
	EXPECT_EQ(nTexts, textNodes.size());
	for (const auto &m : textNodes)
		EXPECT_EQ(m, &texNode);
}

TEST(RepoSceneTest, StatusCheck)
{
	RepoScene emptyScene;
	EXPECT_TRUE(emptyScene.isOK());
	EXPECT_FALSE(emptyScene.isMissingTexture());

	RepoNodeSet empty;
	std::vector<std::string> files;
	RepoScene scene2(files, empty, empty, empty, empty, empty, empty);

	EXPECT_TRUE(scene2.isOK());
	EXPECT_FALSE(scene2.isMissingTexture());

	emptyScene.setMissingTexture();
	EXPECT_FALSE(emptyScene.isOK());
	EXPECT_TRUE(emptyScene.isMissingTexture());
}

TEST(RepoSceneTest, AddMetadata)
{
	RepoNodeSet transNodes;
	RepoNodeSet meshNodes;
	RepoNodeSet metaNodes;
	RepoNodeSet empty;

	auto root = TransformationNode(makeRandomNode(getRandomString(rand() % 10 + 1)));

	auto t1 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t2 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t3 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));

	auto m1 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m2 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m3 = MeshNode(makeRandomNode(t2.getSharedID()));

	auto mm1 = MetadataNode(makeRandomNode(t1.getName()));
	auto mm2 = MetadataNode(makeRandomNode(t2.getName()));
	auto mm3 = MetadataNode(makeRandomNode(t3.getName()));

	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	metaNodes.insert(new MetadataNode(mm1));
	metaNodes.insert(new MetadataNode(mm2));
	metaNodes.insert(new MetadataNode(mm3));

	RepoScene scene,
		scene2(std::vector<std::string>(), empty, meshNodes, empty, empty, empty, transNodes);
	scene.addMetadata(metaNodes, true);
	EXPECT_EQ(0, scene.getAllMetadata(defaultG).size());
	scene2.addMetadata(metaNodes, true, false); //no propagation check
	auto scene2Meta = scene2.getAllMetadata(defaultG);
	EXPECT_EQ(metaNodes.size(), scene2Meta.size());
	for (const auto &s2meta : scene2Meta)
	{
		auto parents = s2meta->getParentIDs();
		EXPECT_EQ(1, parents.size());
		for (const auto &parent : parents)
		{
			auto node = scene2.getNodeBySharedID(RepoScene::GraphType::DEFAULT, parent);
			EXPECT_EQ(NodeType::TRANSFORMATION, node->getTypeAsEnum());
			EXPECT_EQ(s2meta->getName(), node->getName());
		}
	}

	transNodes.clear();
	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.clear();
	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	metaNodes.clear();
	metaNodes.insert(new MetadataNode(mm1));
	metaNodes.insert(new MetadataNode(mm2));
	metaNodes.insert(new MetadataNode(mm3));

	RepoScene scene3(std::vector<std::string>(), empty, meshNodes, empty, empty, empty, transNodes);

	scene3.addMetadata(metaNodes, true, true); //propagation check

	auto scene3Meta = scene3.getAllMetadata(defaultG);
	EXPECT_EQ(metaNodes.size(), scene2Meta.size());
	for (const auto &s3meta : scene3Meta)
	{
		auto parents = s3meta->getParentIDs();
		EXPECT_TRUE(parents.size() > 0);
		for (const auto &parent : parents)
		{
			auto node = scene3.getNodeBySharedID(RepoScene::GraphType::DEFAULT, parent);

			if (NodeType::TRANSFORMATION == node->getTypeAsEnum())
				EXPECT_EQ(s3meta->getName(), node->getName());
			else
			{
				EXPECT_EQ(NodeType::MESH, node->getTypeAsEnum());
			}
		}
	}
}

TEST(RepoSceneTest, AddAndClearStashGraph)
{
	RepoScene scene;
	RepoNodeSet empty;
	auto stashGraph = RepoScene::GraphType::OPTIMIZED;

	EXPECT_FALSE(scene.hasRoot(stashGraph));
	scene.addStashGraph(empty, empty, empty, empty, empty);
	EXPECT_FALSE(scene.hasRoot(stashGraph));

	RepoNodeSet transNodes;
	RepoNodeSet meshNodes;
	RepoNodeSet metaNodes;

	auto root = TransformationNode(makeRandomNode(getRandomString(rand() % 10 + 1)));

	auto t1 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t2 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));
	auto t3 = TransformationNode(makeRandomNode(root.getSharedID(), getRandomString(rand() % 10 + 1)));

	auto m1 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m2 = MeshNode(makeRandomNode(t1.getSharedID()));
	auto m3 = MeshNode(makeRandomNode(t2.getSharedID()));

	auto rootNode = new TransformationNode(root);
	transNodes.insert(rootNode);
	transNodes.insert(new TransformationNode(t1));
	transNodes.insert(new TransformationNode(t2));
	transNodes.insert(new TransformationNode(t3));

	meshNodes.insert(new MeshNode(m1));
	meshNodes.insert(new MeshNode(m2));
	meshNodes.insert(new MeshNode(m3));

	scene.addStashGraph(empty, meshNodes, empty, empty, transNodes);
	EXPECT_EQ(0, scene.getAllCameras(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTextures(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMaterials(stashGraph).size());
	EXPECT_EQ(meshNodes.size(), scene.getAllMeshes(stashGraph).size());
	EXPECT_EQ(transNodes.size(), scene.getAllTransformations(stashGraph).size());
	EXPECT_TRUE(scene.hasRoot(stashGraph));
	EXPECT_FALSE(scene.hasRoot(RepoScene::GraphType::DEFAULT));
	EXPECT_EQ(rootNode, scene.getRoot(stashGraph));

	scene.clearStash();

	EXPECT_EQ(0, scene.getAllCameras(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTextures(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMaterials(stashGraph).size());
	EXPECT_EQ(0, scene.getAllMeshes(stashGraph).size());
	EXPECT_EQ(0, scene.getAllTransformations(stashGraph).size());
	EXPECT_FALSE(scene.hasRoot(stashGraph));
	EXPECT_FALSE(scene.hasRoot(RepoScene::GraphType::DEFAULT));
	EXPECT_EQ(nullptr, scene.getRoot(stashGraph));
}

TEST(RepoSceneTest, CommitScene)
{
	RepoScene scene;
	std::string errMsg;
	std::string commitUser = "me";

	//Commiting an empty scene should fail (fails on empty project/database name)
	EXPECT_FALSE(scene.commit(getHandler(), errMsg, commitUser));
	EXPECT_FALSE(errMsg.empty());

	scene.setDatabaseAndProjectName("sceneCommit", "test1");
	EXPECT_FALSE(scene.commit(getHandler(), errMsg, commitUser));
	EXPECT_FALSE(errMsg.empty());
}<|endoftext|>


<commit_before>#include <iostream>
#include <map>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <cstdlib>
#include <cstdio>
#include <unistd.h>
#include <ext/stdio_filebuf.h>
#include <cstring>
#include <signal.h>

using namespace std;

int iabs(int a) { return a < 0 ? -a : a; }

void exit_verifier(int result_code, int exit_code) {
    cout << endl << result_code;
    exit(exit_code);
}

int main(int argc, char* argv[]) {
    ifstream instance(argv[1]);
    ifstream solver_output(argv[2]);
    string line;

    map<int, int> variables;
    bool SAT_answer = false;
    bool UNSAT_answer = false;
    while (getline(solver_output, line)) {
        istringstream lss(line);
        string timestamp, prefix;
        lss >> timestamp >> prefix; // read away timestamp
        if (prefix == "s") {
            string answer;
            lss >> answer;
            if (answer == "UNKNOWN") {
                cout << "Solver reported unknown." << endl;
                exit_verifier(0, 0);
            }
            else if (answer == "SATISFIABLE") {
                cout << "Solver reported satisfiable. Checking." << endl;
                SAT_answer = true;
            }
            else if (answer == "UNSATISFIABLE") {
                cout << "Solver reported unsatisfiable. Checking." << endl;
                UNSAT_answer = true;
                break; // Proof should follow.
            }
        }
        else if (prefix == "v") {
            // read solution
            int v;
            while (lss >> v && v != 0) {
                variables[iabs(v)] = v;
            }
        }
    }
    if (SAT_answer) {
        while (getline(instance, line)) {
            if (line.substr(0, 1) == "c" || line.substr(0, 1) == "p") continue;
            istringstream lss(line);
            int v;
            bool sat_clause = false;
            int num_vars = 0;
            while (lss >> v && v != 0) {
                num_vars++;
                if (variables[iabs(v)] == v) {
                    sat_clause = true;
                    break;
                }
            }
            if (!sat_clause && num_vars > 0) {
                cout << "Clause " << line << " not satisfied" << endl;
                cout << "Wrong solution." << endl;
                exit_verifier(-1, 0);
            }
        }
        cout << "Solution verified." << endl;
        exit_verifier(11, 0);
    } else if (UNSAT_answer) {
        // Verify the solution that should be given in DRUP or TRACECHECK format following the solution line.

        // First line is used to detect the proof format
        string first_line;
        getline(solver_output, first_line);
        first_line = first_line.substr(first_line.find(" ")); // skip timestamp
        istringstream lss(first_line);
        bool trace = false;
        if (first_line.find("0 0") != string::npos) { // "0 0" means TRACE proof format
            trace = true;
            cout << "Detected TRACE proof format." << endl;
        } else { // DRUP format
            cout << "Detected RUP proof format." << endl;
            trace = false;
        }

        // Prepare communication pipes with checker programs
        int outfd[2];
        int infd[2];
        pipe(outfd);
        pipe(infd);

        char* checker_cmd = new char[4096];
        if (trace) {
            strcpy(checker_cmd, "./stc");
        } else {
            strcpy(checker_cmd, "./drup-check");
        }
        cout << "Calling " << checker_cmd << " " << argv[1] << endl;
        char* argvc[] = {checker_cmd, argv[1], 0 };

        if (!fork()) {
            // This is the child, connect pipes and launch checker program
            close(STDOUT_FILENO); // Close default stdout
            close(STDIN_FILENO); // Close default stdin

            dup2(outfd[0], STDIN_FILENO); // Connect read end of parent-child pipe with stdin
            dup2(infd[1], STDOUT_FILENO); // Connect write end of parent-child pipe with stdout

            // Close now unneeded pipe FDs
            close(outfd[0]);
            close(outfd[1]);
            close(infd[0]);
            close(infd[1]);

            execv(argvc[0], argvc);
        } else {
            // This is the parent (verifier), write the proof to the checker's stdin and read result from stdout.
            close(outfd[0]); // Don't need the read end of the pipe to the child
            close(infd[1]); // Don't need the write end of the pipe from the child

            //signal(SIGPIPE, SIG_IGN);

            write(outfd[1], first_line.c_str(), first_line.length());
            while (getline(solver_output, line)) {
                line = line.substr(line.find('\t')) + "\n"; // skip timestamp, add newline
                write(outfd[1], line.c_str(), line.length());
            }
            close(outfd[1]); // This should signal the checker the end of the proof

            __gnu_cxx::stdio_filebuf<char> filebuf(infd[0], std::ios::in);
            istream checker_output(&filebuf);
            string line;
            cout << "Reading output from checker." << endl;
            getline(checker_output, line);
            cout << "Checker output: " << line << endl;
            if (line.find("s VERIFIED") != string::npos) {
                close(outfd[1]);
                close(infd[0]);
                exit_verifier(10, 0);
            }

        }
    }
    cout << "Could not find a valid solution" << endl;
    exit_verifier(0, 0);
}
<commit_msg>bugfix<commit_after>#include <iostream>
#include <map>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <cstdlib>
#include <cstdio>
#include <unistd.h>
#include <ext/stdio_filebuf.h>
#include <cstring>
#include <signal.h>

using namespace std;

int iabs(int a) { return a < 0 ? -a : a; }

void exit_verifier(int result_code, int exit_code) {
    cout << endl << result_code;
    exit(exit_code);
}

int main(int argc, char* argv[]) {
    ifstream instance(argv[1]);
    ifstream solver_output(argv[2]);
    string line;

    map<int, int> variables;
    bool SAT_answer = false;
    bool UNSAT_answer = false;
    while (getline(solver_output, line)) {
        istringstream lss(line);
        string timestamp, prefix;
        lss >> timestamp >> prefix; // read away timestamp
        if (prefix == "s") {
            string answer;
            lss >> answer;
            if (answer == "UNKNOWN") {
                cout << "Solver reported unknown." << endl;
                exit_verifier(0, 0);
            }
            else if (answer == "SATISFIABLE") {
                cout << "Solver reported satisfiable. Checking." << endl;
                SAT_answer = true;
            }
            else if (answer == "UNSATISFIABLE") {
                cout << "Solver reported unsatisfiable. Checking." << endl;
                UNSAT_answer = true;
                break; // Proof should follow.
            }
        }
        else if (prefix == "v") {
            // read solution
            int v;
            while (lss >> v && v != 0) {
                variables[iabs(v)] = v;
            }
        }
    }
    if (SAT_answer) {
        while (getline(instance, line)) {
            if (line.substr(0, 1) == "c" || line.substr(0, 1) == "p") continue;
            istringstream lss(line);
            int v;
            bool sat_clause = false;
            int num_vars = 0;
            while (lss >> v && v != 0) {
                num_vars++;
                if (variables[iabs(v)] == v) {
                    sat_clause = true;
                    break;
                }
            }
            if (!sat_clause && num_vars > 0) {
                cout << "Clause " << line << " not satisfied" << endl;
                cout << "Wrong solution." << endl;
                exit_verifier(-1, 0);
            }
        }
        cout << "Solution verified." << endl;
        exit_verifier(11, 0);
    } else if (UNSAT_answer) {
        // Verify the solution that should be given in DRUP or TRACECHECK format following the solution line.

        // First line is used to detect the proof format
        string first_line;
        getline(solver_output, first_line);
        first_line = first_line.substr(first_line.find('\t')); // skip timestamp
        istringstream lss(first_line);
        bool trace = false;
        if (first_line.find("0 0") != string::npos) { // "0 0" means TRACE proof format
            trace = true;
            cout << "Detected TRACE proof format." << endl;
        } else { // DRUP format
            cout << "Detected RUP proof format." << endl;
            trace = false;
        }

        // Prepare communication pipes with checker programs
        int outfd[2];
        int infd[2];
        pipe(outfd);
        pipe(infd);

        char* checker_cmd = new char[4096];
        if (trace) {
            strcpy(checker_cmd, "./stc");
        } else {
            strcpy(checker_cmd, "./drup-check");
        }
        cout << "Calling " << checker_cmd << " " << argv[1] << endl;
        char* argvc[] = {checker_cmd, argv[1], 0 };

        if (!fork()) {
            // This is the child, connect pipes and launch checker program
            close(STDOUT_FILENO); // Close default stdout
            close(STDIN_FILENO); // Close default stdin

            dup2(outfd[0], STDIN_FILENO); // Connect read end of parent-child pipe with stdin
            dup2(infd[1], STDOUT_FILENO); // Connect write end of parent-child pipe with stdout

            // Close now unneeded pipe FDs
            close(outfd[0]);
            close(outfd[1]);
            close(infd[0]);
            close(infd[1]);

            execv(argvc[0], argvc);
        } else {
            // This is the parent (verifier), write the proof to the checker's stdin and read result from stdout.
            close(outfd[0]); // Don't need the read end of the pipe to the child
            close(infd[1]); // Don't need the write end of the pipe from the child

            //signal(SIGPIPE, SIG_IGN);

            write(outfd[1], first_line.c_str(), first_line.length());
            while (getline(solver_output, line)) {
                line = line.substr(line.find('\t')) + "\n"; // skip timestamp, add newline
                write(outfd[1], line.c_str(), line.length());
            }
            close(outfd[1]); // This should signal the checker the end of the proof

            __gnu_cxx::stdio_filebuf<char> filebuf(infd[0], std::ios::in);
            istream checker_output(&filebuf);
            string line;
            cout << "Reading output from checker." << endl;
            getline(checker_output, line);
            cout << "Checker output: " << line << endl;
            if (line.find("s VERIFIED") != string::npos) {
                close(outfd[1]);
                close(infd[0]);
                exit_verifier(10, 0);
            }

        }
    }
    cout << "Could not find a valid solution" << endl;
    exit_verifier(0, 0);
}
<|endoftext|>


<commit_before>#ifndef THREAD_POOL_H
#define THREAD_POOL_H

#include <vector>
#include <queue>
#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <future>
#include <functional>
#include <stdexcept>
#include <atomic>

class ThreadPool {
public:
    ThreadPool(size_t);
    template<class F, class... Args>
    auto enqueue(F&& f, Args&&... args)
            -> std::future<typename std::result_of<F(Args...)>::type>;
    ~ThreadPool();
private:
    // need to keep track of threads so we can join them
    std::vector< std::thread > workers;
    // the task queue
    std::queue< std::function<void()> > tasks;

    // synchronization
    std::mutex queue_mutex;
    std::condition_variable condition;
    std::atomic<bool> stop;
};

// the constructor just launches some amount of workers
inline ThreadPool::ThreadPool(size_t threads)
        :   stop(false)
{
    for(size_t i = 0;i<threads;++i)
        workers.emplace_back(
                [this]
                {
                    for(;;)
                    {
                        std::function<void()> task;

                        {
                            std::unique_lock<std::mutex> lock(this->queue_mutex);
                            this->condition.wait(lock,
                                                 [this]{ return this->stop || !this->tasks.empty(); });
                            if(this->stop && this->tasks.empty())
                                return;
                            task = std::move(this->tasks.front());
                            this->tasks.pop();
                        }

                        task();
                    }
                }
        );
}

// add new work item to the pool
template<class F, class... Args>
auto ThreadPool::enqueue(F&& f, Args&&... args)
-> std::future<typename std::result_of<F(Args...)>::type>
{
    using return_type = typename std::result_of<F(Args...)>::type;

    auto task = std::make_shared< std::packaged_task<return_type()> >(
            std::bind(std::forward<F>(f), std::forward<Args>(args)...)
    );

    std::future<return_type> res = task->get_future();
    {
        std::unique_lock<std::mutex> lock(queue_mutex);

        // don't allow enqueueing after stopping the pool
        if(stop)
            throw std::runtime_error("enqueue on stopped ThreadPool");

        tasks.emplace([task](){ (*task)(); });
    }
    condition.notify_one();
    return res;
}

// the destructor joins all threads
inline ThreadPool::~ThreadPool()
{
    {
        std::unique_lock<std::mutex> lock(queue_mutex);
        stop = true;
    }
    condition.notify_all();
    for(std::thread &worker: workers)
        worker.join();
}

#endif<commit_msg>add namespace<commit_after>#ifndef THREAD_POOL_H
#define THREAD_POOL_H

#include <vector>
#include <queue>
#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <future>
#include <functional>
#include <stdexcept>
#include <atomic>
namespace saberUtils
{
    class ThreadPool {
    public:
        ThreadPool(size_t);
        template<class F, class... Args>
        auto enqueue(F&& f, Args&&... args)
                -> std::future<typename std::result_of<F(Args...)>::type>;
        ~ThreadPool();
    private:
        // need to keep track of threads so we can join them
        std::vector< std::thread > workers;
        // the task queue
        std::queue< std::function<void()> > tasks;

        // synchronization
        std::mutex queue_mutex;
        std::condition_variable condition;
        std::atomic<bool> stop;
    };

// the constructor just launches some amount of workers
    inline ThreadPool::ThreadPool(size_t threads)
            :   stop(false)
    {
        for(size_t i = 0;i<threads;++i)
            workers.emplace_back(
                    [this]
                    {
                        for(;;)
                        {
                            std::function<void()> task;

                            {
                                std::unique_lock<std::mutex> lock(this->queue_mutex);
                                this->condition.wait(lock,
                                                     [this]{ return this->stop || !this->tasks.empty(); });
                                if(this->stop && this->tasks.empty())
                                    return;
                                task = std::move(this->tasks.front());
                                this->tasks.pop();
                            }

                            task();
                        }
                    }
            );
    }

// add new work item to the pool
    template<class F, class... Args>
    auto ThreadPool::enqueue(F&& f, Args&&... args)
    -> std::future<typename std::result_of<F(Args...)>::type>
    {
        using return_type = typename std::result_of<F(Args...)>::type;

        auto task = std::make_shared< std::packaged_task<return_type()> >(
                std::bind(std::forward<F>(f), std::forward<Args>(args)...)
        );

        std::future<return_type> res = task->get_future();
        {
            std::unique_lock<std::mutex> lock(queue_mutex);

            // don't allow enqueueing after stopping the pool
            if(stop)
                throw std::runtime_error("enqueue on stopped ThreadPool");

            tasks.emplace([task](){ (*task)(); });
        }
        condition.notify_one();
        return res;
    }

// the destructor joins all threads
    inline ThreadPool::~ThreadPool()
    {
        {
            std::unique_lock<std::mutex> lock(queue_mutex);
            stop = true;
        }
        condition.notify_all();
        for(std::thread &worker: workers)
            worker.join();
    }
}

#endif<|endoftext|>