<commit_before>/******************************************************************************
 * Copyright 2019 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.
 *****************************************************************************/

#include "modules/prediction/scenario/prioritization/obstacles_prioritizer.h"

#include <algorithm>
#include <limits>
#include <string>

#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/common/prediction_map.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/adc_trajectory/adc_trajectory_container.h"
#include "modules/prediction/container/obstacles/obstacle_clusters.h"
#include "modules/prediction/container/pose/pose_container.h"

namespace apollo {
namespace prediction {

using apollo::perception::PerceptionObstacle;
using common::adapter::AdapterConfig;
using common::math::Box2d;
using common::math::Vec2d;

namespace {

bool IsLaneSequenceInReferenceLine(const LaneSequence& lane_sequence,
    const ADCTrajectoryContainer* ego_trajectory_container) {
  for (const auto& lane_segment : lane_sequence.lane_segment()) {
    std::string lane_id = lane_segment.lane_id();
    if (ego_trajectory_container->IsLaneIdInReferenceLine(lane_id)) {
      return true;
    }
  }
  return false;
}

int NearestFrontObstacleIdOnLaneSequence(const LaneSequence& lane_sequence) {
  int nearest_front_obstacle_id = -std::numeric_limits<int>::infinity();
  double smallest_relative_s = std::numeric_limits<double>::infinity();
  for (const auto& nearby_obs : lane_sequence.nearby_obstacle()) {
    if (nearby_obs.s() < 0.0) {
      continue;
    }
    if (nearby_obs.s() < smallest_relative_s) {
      smallest_relative_s = nearby_obs.s();
      nearest_front_obstacle_id = nearby_obs.id();
    }
  }
  return nearest_front_obstacle_id;
}

int NearestBackwardObstacleIdOnLaneSequence(const LaneSequence& lane_sequence) {
  int nearest_backward_obstacle_id = -std::numeric_limits<int>::infinity();
  double smallest_relative_s = std::numeric_limits<double>::infinity();
  for (const auto& nearby_obs : lane_sequence.nearby_obstacle()) {
    if (nearby_obs.s() > 0.0) {
      continue;
    }
    if (-nearby_obs.s() < smallest_relative_s) {
      smallest_relative_s = -nearby_obs.s();
      nearest_backward_obstacle_id = nearby_obs.id();
    }
  }
  return nearest_backward_obstacle_id;
}

}  // namespace

void ObstaclesPrioritizer::PrioritizeObstacles(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> scenario_features) {
  AssignIgnoreLevel(environment_features, scenario_features);
}

void ObstaclesPrioritizer::AssignIgnoreLevel(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> ptr_scenario_features) {
  auto obstacles_container =
      ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);

  if (obstacles_container == nullptr) {
    AERROR << "Obstacles container pointer is a null pointer.";
    return;
  }

  auto pose_container =
      ContainerManager::Instance()->GetContainer<PoseContainer>(
          AdapterConfig::LOCALIZATION);
  if (pose_container == nullptr) {
    AERROR << "Pose container pointer is a null pointer.";
    return;
  }

  const PerceptionObstacle* pose_obstacle_ptr =
      pose_container->ToPerceptionObstacle();
  if (pose_obstacle_ptr == nullptr) {
    AERROR << "Pose obstacle pointer is a null pointer.";
    return;
  }

  double pose_theta = pose_obstacle_ptr->theta();
  double pose_x = pose_obstacle_ptr->position().x();
  double pose_y = pose_obstacle_ptr->position().y();

  ADEBUG << "Get pose (" << pose_x << ", " << pose_y << ", " << pose_theta
         << ")";

  // Build rectangular scan_area
  Box2d scan_box({pose_x + FLAGS_scan_length / 2.0 * std::cos(pose_theta),
                  pose_y + FLAGS_scan_length / 2.0 * std::sin(pose_theta)},
                 pose_theta, FLAGS_scan_length, FLAGS_scan_width);

  const auto& obstacle_ids =
      obstacles_container->curr_frame_predictable_obstacle_ids();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    double obstacle_x = latest_feature_ptr->position().x();
    double obstacle_y = latest_feature_ptr->position().y();
    Vec2d ego_to_obstacle_vec(obstacle_x - pose_x, obstacle_y - pose_y);
    Vec2d ego_vec = Vec2d::CreateUnitVec2d(pose_theta);
    double s = ego_to_obstacle_vec.InnerProd(ego_vec);

    double pedestrian_like_nearby_lane_radius =
        FLAGS_pedestrian_nearby_lane_search_radius;
    bool is_near_lane = PredictionMap::HasNearbyLane(
        obstacle_x, obstacle_y, pedestrian_like_nearby_lane_radius);

    // Decide if we need consider this obstacle
    bool is_in_scan_area = scan_box.IsPointIn({obstacle_x, obstacle_y});
    bool is_on_lane = obstacle_ptr->IsOnLane();
    bool is_pedestrian_like_in_front_near_lanes =
        s > FLAGS_back_dist_ignore_ped &&
        (latest_feature_ptr->type() == PerceptionObstacle::PEDESTRIAN ||
         latest_feature_ptr->type() == PerceptionObstacle::BICYCLE ||
         latest_feature_ptr->type() == PerceptionObstacle::UNKNOWN ||
         latest_feature_ptr->type() == PerceptionObstacle::UNKNOWN_MOVABLE) &&
        is_near_lane;
    bool is_near_junction = obstacle_ptr->IsNearJunction();

    bool need_consider = is_in_scan_area || is_on_lane || is_near_junction ||
                         is_pedestrian_like_in_front_near_lanes;

    if (!need_consider) {
      latest_feature_ptr->mutable_priority()->set_priority(
          ObstaclePriority::IGNORE);
    } else {
      latest_feature_ptr->mutable_priority()->set_priority(
          ObstaclePriority::NORMAL);
    }
  }
}

void ObstaclesPrioritizer::AssignCautionLevelCruiseKeepLane() {
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<
          ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  if (ego_vehicle->history_size() == 0) {
    AERROR << "Ego vehicle has no history";
    return;
  }
  const Feature& ego_latest_feature = ego_vehicle->latest_feature();
  for (const LaneSequence& lane_sequence :
       ego_latest_feature.lane().lane_graph().lane_sequence()) {
    int nearest_front_obstacle_id =
        NearestFrontObstacleIdOnLaneSequence(lane_sequence);
    if (nearest_front_obstacle_id < 0) {
      continue;
    }
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(
        nearest_front_obstacle_id);
    if (obstacle_ptr == nullptr) {
      AERROR << "Obstacle [" << nearest_front_obstacle_id << "] Not found";
      continue;
    }
    obstacle_ptr->SetCaution();
  }
}

void ObstaclesPrioritizer::AssignCautionLevelCruiseChangeLane() {
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<
          ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
  ADCTrajectoryContainer* ego_trajectory_container =
      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
          AdapterConfig::PLANNING_TRAJECTORY);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  if (ego_vehicle->history_size() == 0) {
    AERROR << "Ego vehicle has no history";
    return;
  }
  const Feature& ego_latest_feature = ego_vehicle->latest_feature();
  for (const LaneSequence& lane_sequence :
       ego_latest_feature.lane().lane_graph().lane_sequence()) {
    if (lane_sequence.vehicle_on_lane()) {
      int nearest_front_obstacle_id =
          NearestFrontObstacleIdOnLaneSequence(lane_sequence);
      if (nearest_front_obstacle_id < 0) {
        continue;
      }
      Obstacle* obstacle_ptr = obstacles_container->GetObstacle(
          nearest_front_obstacle_id);
      if (obstacle_ptr == nullptr) {
        AERROR << "Obstacle [" << nearest_front_obstacle_id << "] Not found";
        continue;
      }
      obstacle_ptr->SetCaution();
    } else if (IsLaneSequenceInReferenceLine(lane_sequence,
                                             ego_trajectory_container)) {
      int nearest_front_obstacle_id =
          NearestFrontObstacleIdOnLaneSequence(lane_sequence);
      int nearest_backward_obstacle_id =
          NearestBackwardObstacleIdOnLaneSequence(lane_sequence);
      if (nearest_front_obstacle_id >= 0) {
        Obstacle* front_obstacle_ptr =
            obstacles_container->GetObstacle(nearest_front_obstacle_id);
        if (front_obstacle_ptr != nullptr) {
          front_obstacle_ptr->SetCaution();
        }
      }
      if (nearest_backward_obstacle_id >= 0) {
        Obstacle* backward_obstacle_ptr =
            obstacles_container->GetObstacle(nearest_backward_obstacle_id);
        if (backward_obstacle_ptr != nullptr) {
          backward_obstacle_ptr->SetCaution();
        }
      }
    }
  }
}

void ObstaclesPrioritizer::AssignCautionLevelInJunction(
    const std::shared_ptr<ScenarioFeatures> scenario_features) {
  if (scenario_features->scenario().type() != Scenario::JUNCTION) {
    ADEBUG << "Not in Junction Scenario";
    return;
  }
  std::string junction_id = scenario_features->scenario().junction_id();
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<
          ObstaclesContainer>(AdapterConfig::PERCEPTION_OBSTACLES);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  for (const int id :
       obstacles_container->curr_frame_predictable_obstacle_ids()) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(id);
    if (obstacle_ptr != nullptr && obstacle_ptr->IsInJunction(junction_id)) {
      obstacle_ptr->SetCaution();
      ADEBUG << "SetCaution for obstacle [" << obstacle_ptr->id() << "]";
    }
  }
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>Prediction: change style with clang-format<commit_after>/******************************************************************************
 * Copyright 2019 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.
 *****************************************************************************/

#include "modules/prediction/scenario/prioritization/obstacles_prioritizer.h"

#include <algorithm>
#include <limits>
#include <string>

#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/common/prediction_map.h"
#include "modules/prediction/container/adc_trajectory/adc_trajectory_container.h"
#include "modules/prediction/container/container_manager.h"
#include "modules/prediction/container/obstacles/obstacle_clusters.h"
#include "modules/prediction/container/pose/pose_container.h"

namespace apollo {
namespace prediction {

using apollo::perception::PerceptionObstacle;
using common::adapter::AdapterConfig;
using common::math::Box2d;
using common::math::Vec2d;

namespace {

bool IsLaneSequenceInReferenceLine(
    const LaneSequence& lane_sequence,
    const ADCTrajectoryContainer* ego_trajectory_container) {
  for (const auto& lane_segment : lane_sequence.lane_segment()) {
    std::string lane_id = lane_segment.lane_id();
    if (ego_trajectory_container->IsLaneIdInReferenceLine(lane_id)) {
      return true;
    }
  }
  return false;
}

int NearestFrontObstacleIdOnLaneSequence(const LaneSequence& lane_sequence) {
  int nearest_front_obstacle_id = -std::numeric_limits<int>::infinity();
  double smallest_relative_s = std::numeric_limits<double>::infinity();
  for (const auto& nearby_obs : lane_sequence.nearby_obstacle()) {
    if (nearby_obs.s() < 0.0) {
      continue;
    }
    if (nearby_obs.s() < smallest_relative_s) {
      smallest_relative_s = nearby_obs.s();
      nearest_front_obstacle_id = nearby_obs.id();
    }
  }
  return nearest_front_obstacle_id;
}

int NearestBackwardObstacleIdOnLaneSequence(const LaneSequence& lane_sequence) {
  int nearest_backward_obstacle_id = -std::numeric_limits<int>::infinity();
  double smallest_relative_s = std::numeric_limits<double>::infinity();
  for (const auto& nearby_obs : lane_sequence.nearby_obstacle()) {
    if (nearby_obs.s() > 0.0) {
      continue;
    }
    if (-nearby_obs.s() < smallest_relative_s) {
      smallest_relative_s = -nearby_obs.s();
      nearest_backward_obstacle_id = nearby_obs.id();
    }
  }
  return nearest_backward_obstacle_id;
}

}  // namespace

void ObstaclesPrioritizer::PrioritizeObstacles(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> scenario_features) {
  AssignIgnoreLevel(environment_features, scenario_features);
}

void ObstaclesPrioritizer::AssignIgnoreLevel(
    const EnvironmentFeatures& environment_features,
    const std::shared_ptr<ScenarioFeatures> ptr_scenario_features) {
  auto obstacles_container =
      ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);

  if (obstacles_container == nullptr) {
    AERROR << "Obstacles container pointer is a null pointer.";
    return;
  }

  auto pose_container =
      ContainerManager::Instance()->GetContainer<PoseContainer>(
          AdapterConfig::LOCALIZATION);
  if (pose_container == nullptr) {
    AERROR << "Pose container pointer is a null pointer.";
    return;
  }

  const PerceptionObstacle* pose_obstacle_ptr =
      pose_container->ToPerceptionObstacle();
  if (pose_obstacle_ptr == nullptr) {
    AERROR << "Pose obstacle pointer is a null pointer.";
    return;
  }

  double pose_theta = pose_obstacle_ptr->theta();
  double pose_x = pose_obstacle_ptr->position().x();
  double pose_y = pose_obstacle_ptr->position().y();

  ADEBUG << "Get pose (" << pose_x << ", " << pose_y << ", " << pose_theta
         << ")";

  // Build rectangular scan_area
  Box2d scan_box({pose_x + FLAGS_scan_length / 2.0 * std::cos(pose_theta),
                  pose_y + FLAGS_scan_length / 2.0 * std::sin(pose_theta)},
                 pose_theta, FLAGS_scan_length, FLAGS_scan_width);

  const auto& obstacle_ids =
      obstacles_container->curr_frame_predictable_obstacle_ids();

  for (const int& obstacle_id : obstacle_ids) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
    if (obstacle_ptr->history_size() == 0) {
      AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    double obstacle_x = latest_feature_ptr->position().x();
    double obstacle_y = latest_feature_ptr->position().y();
    Vec2d ego_to_obstacle_vec(obstacle_x - pose_x, obstacle_y - pose_y);
    Vec2d ego_vec = Vec2d::CreateUnitVec2d(pose_theta);
    double s = ego_to_obstacle_vec.InnerProd(ego_vec);

    double pedestrian_like_nearby_lane_radius =
        FLAGS_pedestrian_nearby_lane_search_radius;
    bool is_near_lane = PredictionMap::HasNearbyLane(
        obstacle_x, obstacle_y, pedestrian_like_nearby_lane_radius);

    // Decide if we need consider this obstacle
    bool is_in_scan_area = scan_box.IsPointIn({obstacle_x, obstacle_y});
    bool is_on_lane = obstacle_ptr->IsOnLane();
    bool is_pedestrian_like_in_front_near_lanes =
        s > FLAGS_back_dist_ignore_ped &&
        (latest_feature_ptr->type() == PerceptionObstacle::PEDESTRIAN ||
         latest_feature_ptr->type() == PerceptionObstacle::BICYCLE ||
         latest_feature_ptr->type() == PerceptionObstacle::UNKNOWN ||
         latest_feature_ptr->type() == PerceptionObstacle::UNKNOWN_MOVABLE) &&
        is_near_lane;
    bool is_near_junction = obstacle_ptr->IsNearJunction();

    bool need_consider = is_in_scan_area || is_on_lane || is_near_junction ||
                         is_pedestrian_like_in_front_near_lanes;

    if (!need_consider) {
      latest_feature_ptr->mutable_priority()->set_priority(
          ObstaclePriority::IGNORE);
    } else {
      latest_feature_ptr->mutable_priority()->set_priority(
          ObstaclePriority::NORMAL);
    }
  }
}

void ObstaclesPrioritizer::AssignCautionLevelCruiseKeepLane() {
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  if (ego_vehicle->history_size() == 0) {
    AERROR << "Ego vehicle has no history";
    return;
  }
  const Feature& ego_latest_feature = ego_vehicle->latest_feature();
  for (const LaneSequence& lane_sequence :
       ego_latest_feature.lane().lane_graph().lane_sequence()) {
    int nearest_front_obstacle_id =
        NearestFrontObstacleIdOnLaneSequence(lane_sequence);
    if (nearest_front_obstacle_id < 0) {
      continue;
    }
    Obstacle* obstacle_ptr =
        obstacles_container->GetObstacle(nearest_front_obstacle_id);
    if (obstacle_ptr == nullptr) {
      AERROR << "Obstacle [" << nearest_front_obstacle_id << "] Not found";
      continue;
    }
    obstacle_ptr->SetCaution();
  }
}

void ObstaclesPrioritizer::AssignCautionLevelCruiseChangeLane() {
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);
  ADCTrajectoryContainer* ego_trajectory_container =
      ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
          AdapterConfig::PLANNING_TRAJECTORY);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  if (ego_vehicle->history_size() == 0) {
    AERROR << "Ego vehicle has no history";
    return;
  }
  const Feature& ego_latest_feature = ego_vehicle->latest_feature();
  for (const LaneSequence& lane_sequence :
       ego_latest_feature.lane().lane_graph().lane_sequence()) {
    if (lane_sequence.vehicle_on_lane()) {
      int nearest_front_obstacle_id =
          NearestFrontObstacleIdOnLaneSequence(lane_sequence);
      if (nearest_front_obstacle_id < 0) {
        continue;
      }
      Obstacle* obstacle_ptr =
          obstacles_container->GetObstacle(nearest_front_obstacle_id);
      if (obstacle_ptr == nullptr) {
        AERROR << "Obstacle [" << nearest_front_obstacle_id << "] Not found";
        continue;
      }
      obstacle_ptr->SetCaution();
    } else if (IsLaneSequenceInReferenceLine(lane_sequence,
                                             ego_trajectory_container)) {
      int nearest_front_obstacle_id =
          NearestFrontObstacleIdOnLaneSequence(lane_sequence);
      int nearest_backward_obstacle_id =
          NearestBackwardObstacleIdOnLaneSequence(lane_sequence);
      if (nearest_front_obstacle_id >= 0) {
        Obstacle* front_obstacle_ptr =
            obstacles_container->GetObstacle(nearest_front_obstacle_id);
        if (front_obstacle_ptr != nullptr) {
          front_obstacle_ptr->SetCaution();
        }
      }
      if (nearest_backward_obstacle_id >= 0) {
        Obstacle* backward_obstacle_ptr =
            obstacles_container->GetObstacle(nearest_backward_obstacle_id);
        if (backward_obstacle_ptr != nullptr) {
          backward_obstacle_ptr->SetCaution();
        }
      }
    }
  }
}

void ObstaclesPrioritizer::AssignCautionLevelInJunction(
    const std::shared_ptr<ScenarioFeatures> scenario_features) {
  if (scenario_features->scenario().type() != Scenario::JUNCTION) {
    ADEBUG << "Not in Junction Scenario";
    return;
  }
  std::string junction_id = scenario_features->scenario().junction_id();
  ObstaclesContainer* obstacles_container =
      ContainerManager::Instance()->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);
  Obstacle* ego_vehicle =
      obstacles_container->GetObstacle(FLAGS_ego_vehicle_id);
  if (ego_vehicle == nullptr) {
    AERROR << "Ego vehicle not found";
    return;
  }
  for (const int id :
       obstacles_container->curr_frame_predictable_obstacle_ids()) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(id);
    if (obstacle_ptr != nullptr && obstacle_ptr->IsInJunction(junction_id)) {
      obstacle_ptr->SetCaution();
      ADEBUG << "SetCaution for obstacle [" << obstacle_ptr->id() << "]";
    }
  }
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>


<commit_before>// BMASHINA
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
//
// Copyright 2017 [bk]door.maus

#ifndef BMASHINA_STATE_PROPERTY_HPP
#define BMASHINA_STATE_PROPERTY_HPP

#include "bmashina/config.hpp"

namespace bmashina
{
	namespace detail
	{
		class BaseProperty
		{
		public:
			virtual ~BaseProperty() = default;

			virtual BaseProperty* clone(BasicAllocator& allocator) const = 0;
		};
	}

	template <typename V>
	class Property : public detail::BaseProperty
	{
	public:
		typedef V Value;

		Property() = default;
		Property(const Value& value);
		~Property() = default;

		Value& get();
		const Value& get() const;
		void set(const Value& new_value);

		Value* operator ->();
		const Value* operator ->() const;
		Value& operator *();
		const Value& operator *() const;

		BaseProperty* clone(BasicAllocator& allocator) const;

	private:
		Value value;
	};
}

template <typename V>
bmashina::Property<V>::Property(const Value& value) : value(value)
{
	// Nothing.
}

template <typename V>
typename bmashina::Property<V>::Value&
bmashina::Property<V>::get()
{
	return value;
}

template <typename V>
const typename bmashina::Property<V>::Value&
bmashina::Property<V>::get() const
{
	return value;
}

template <typename V>
void bmashina::Property<V>::set(const Value& new_value)
{
	value = new_value;
}

template <typename V>
typename bmashina::Property<V>::Value*
bmashina::Property<V>::operator ->()
{
	return &get();
}

template <typename V>
const typename bmashina::Property<V>::Value*
bmashina::Property<V>::operator ->() const
{
	return &get();
}

template <typename V>
typename bmashina::Property<V>::Value&
bmashina::Property<V>::operator *()
{
	return get();
}

template <typename V>
const typename bmashina::Property<V>::Value&
bmashina::Property<V>::operator *() const
{
	return get();
}

template <typename V>
bmashina::detail::BaseProperty*
bmashina::Property<V>::clone(BasicAllocator& allocator) const
{
	return BasicAllocator::create<Property<V>>(allocator, *this);
}

#endif
<commit_msg>Added implicit conversion operator.<commit_after>// BMASHINA
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
//
// Copyright 2017 [bk]door.maus

#ifndef BMASHINA_STATE_PROPERTY_HPP
#define BMASHINA_STATE_PROPERTY_HPP

#include "bmashina/config.hpp"

namespace bmashina
{
	namespace detail
	{
		class BaseProperty
		{
		public:
			virtual ~BaseProperty() = default;

			virtual BaseProperty* clone(BasicAllocator& allocator) const = 0;
		};
	}

	template <typename V>
	class Property : public detail::BaseProperty
	{
	public:
		typedef V Value;

		Property() = default;
		Property(const Value& value);
		~Property() = default;

		Value& get();
		const Value& get() const;
		void set(const Value& new_value);

		Value* operator ->();
		const Value* operator ->() const;
		Value& operator *();
		const Value& operator *() const;

		BaseProperty* clone(BasicAllocator& allocator) const;

		operator Value&();
		operator const Value&() const;

	private:
		Value value;
	};
}

template <typename V>
bmashina::Property<V>::Property(const Value& value) : value(value)
{
	// Nothing.
}

template <typename V>
typename bmashina::Property<V>::Value&
bmashina::Property<V>::get()
{
	return value;
}

template <typename V>
const typename bmashina::Property<V>::Value&
bmashina::Property<V>::get() const
{
	return value;
}

template <typename V>
void bmashina::Property<V>::set(const Value& new_value)
{
	value = new_value;
}

template <typename V>
typename bmashina::Property<V>::Value*
bmashina::Property<V>::operator ->()
{
	return &get();
}

template <typename V>
const typename bmashina::Property<V>::Value*
bmashina::Property<V>::operator ->() const
{
	return &get();
}

template <typename V>
typename bmashina::Property<V>::Value&
bmashina::Property<V>::operator *()
{
	return get();
}

template <typename V>
const typename bmashina::Property<V>::Value&
bmashina::Property<V>::operator *() const
{
	return get();
}

template <typename V>
bmashina::detail::BaseProperty*
bmashina::Property<V>::clone(BasicAllocator& allocator) const
{
	return BasicAllocator::create<Property<V>>(allocator, *this);
}

template <typename V>
bmashina::Property<V>::operator Value&()
{
	return get();
}

template <typename V>
bmashina::Property<V>::operator const Value&() const
{
	return get();
}

#endif
<|endoftext|>


<commit_before>//
//  StaticAnalyser.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 20/09/2018.
//  Copyright © 2018 Thomas Harte. All rights reserved.
//

#include "StaticAnalyser.hpp"

#include "Target.hpp"

Analyser::Static::TargetList Analyser::Static::Sega::GetTargets(const Media &media, const std::string &file_name, TargetPlatform::IntType potential_platforms) {
	if(media.cartridges.empty())
		return {};

	TargetList targets;
	std::unique_ptr<Target> target(new Target);

	target->machine = Machine::MasterSystem;

	// Files named .sg are treated as for the SG1000; otherwise assume a Master System.
	if(file_name.size() >= 2 && *(file_name.end() - 2) == 's' && *(file_name.end() - 1) == 'g') {
		target->model = Target::Model::SG1000;
	} else {
		target->model = Target::Model::MasterSystem;
	}

	// If this is a Master System title, look for a ROM header.
	if(target->model == Target::Model::MasterSystem) {
		const auto &data = media.cartridges.front()->get_segments()[0].data;

		// First try to locate a header.
		size_t header_offset = 0;
		size_t potential_offsets[] = {0x1ff0, 0x3ff0, 0x7ff0};
		for(auto potential_offset: potential_offsets) {
			if(!memcmp(&data[potential_offset], "TMR SEGA", 8)) {
				header_offset = potential_offset;
				break;
			}
		}

		// If a header was found, use it to crib region.
		if(header_offset) {
			// Treat export titles as European by default; decline to
			// do so only if (US) or (NTSC) is in the file name.
			const uint8_t region = data[header_offset + 0x0f] >> 4;
			switch(region) {
				default: break;
				case 4: {
					std::string lowercase_name = file_name;
					std::transform(lowercase_name.begin(), lowercase_name.end(), lowercase_name.begin(), ::tolower);
					if(lowercase_name.find("(jp)") == std::string::npos) {
						target->region =
							(lowercase_name.find("(us)") == std::string::npos &&
							lowercase_name.find("(ntsc)") == std::string::npos) ? Target::Region::Europe : Target::Region::USA;
					}
				} break;
			}

			// Also check for a Codemasters header.
			// If one is found, set the paging scheme appropriately.
			const uint16_t inverse_checksum = uint16_t(0x10000 - (data[0x7fe6] | (data[0x7fe7] << 8)));
			if(
				data[0x7fe3] >= 0x87 && data[0x7fe3] < 0x96 &&									// i.e. game is dated between 1987 and 1996
				(inverse_checksum&0xff) == data[0x7fe8] &&
				(inverse_checksum >> 8) == data[0x7fe9] &&										// i.e. the standard checksum appears to be present
				!data[0x7fea] && !data[0x7feb] && !data[0x7fec] && !data[0x7fed] && !data[0x7fee] && !data[0x7fef]
			) {
				target->paging_scheme = Target::PagingScheme::Codemasters;
			}
		}
	}

	target->media.cartridges = media.cartridges;
	targets.push_back(std::move(target));
	return targets;
}
<commit_msg>Adds missing #includes.<commit_after>//
//  StaticAnalyser.cpp
//  Clock Signal
//
//  Created by Thomas Harte on 20/09/2018.
//  Copyright © 2018 Thomas Harte. All rights reserved.
//

#include "StaticAnalyser.hpp"

#include "Target.hpp"

#include <algorithm>
#include <cstring>

Analyser::Static::TargetList Analyser::Static::Sega::GetTargets(const Media &media, const std::string &file_name, TargetPlatform::IntType potential_platforms) {
	if(media.cartridges.empty())
		return {};

	TargetList targets;
	std::unique_ptr<Target> target(new Target);

	target->machine = Machine::MasterSystem;

	// Files named .sg are treated as for the SG1000; otherwise assume a Master System.
	if(file_name.size() >= 2 && *(file_name.end() - 2) == 's' && *(file_name.end() - 1) == 'g') {
		target->model = Target::Model::SG1000;
	} else {
		target->model = Target::Model::MasterSystem;
	}

	// If this is a Master System title, look for a ROM header.
	if(target->model == Target::Model::MasterSystem) {
		const auto &data = media.cartridges.front()->get_segments()[0].data;

		// First try to locate a header.
		size_t header_offset = 0;
		size_t potential_offsets[] = {0x1ff0, 0x3ff0, 0x7ff0};
		for(auto potential_offset: potential_offsets) {
			if(!memcmp(&data[potential_offset], "TMR SEGA", 8)) {
				header_offset = potential_offset;
				break;
			}
		}

		// If a header was found, use it to crib region.
		if(header_offset) {
			// Treat export titles as European by default; decline to
			// do so only if (US) or (NTSC) is in the file name.
			const uint8_t region = data[header_offset + 0x0f] >> 4;
			switch(region) {
				default: break;
				case 4: {
					std::string lowercase_name = file_name;
					std::transform(lowercase_name.begin(), lowercase_name.end(), lowercase_name.begin(), ::tolower);
					if(lowercase_name.find("(jp)") == std::string::npos) {
						target->region =
							(lowercase_name.find("(us)") == std::string::npos &&
							lowercase_name.find("(ntsc)") == std::string::npos) ? Target::Region::Europe : Target::Region::USA;
					}
				} break;
			}

			// Also check for a Codemasters header.
			// If one is found, set the paging scheme appropriately.
			const uint16_t inverse_checksum = uint16_t(0x10000 - (data[0x7fe6] | (data[0x7fe7] << 8)));
			if(
				data[0x7fe3] >= 0x87 && data[0x7fe3] < 0x96 &&									// i.e. game is dated between 1987 and 1996
				(inverse_checksum&0xff) == data[0x7fe8] &&
				(inverse_checksum >> 8) == data[0x7fe9] &&										// i.e. the standard checksum appears to be present
				!data[0x7fea] && !data[0x7feb] && !data[0x7fec] && !data[0x7fed] && !data[0x7fee] && !data[0x7fef]
			) {
				target->paging_scheme = Target::PagingScheme::Codemasters;
			}
		}
	}

	target->media.cartridges = media.cartridges;
	targets.push_back(std::move(target));
	return targets;
}
<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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 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: Mrinal Kalakrishnan */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <iostream>

namespace chomp
{
ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
                                 double discretization, const std::string& group_name)
  : ChompTrajectory(robot_model, static_cast<unsigned int>(duration / discretization) + 1, discretization, group_name)
{
}

ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, unsigned int num_points,
                                 double discretization, const std::string& group_name)
  : planning_group_name_(group_name)
  , num_points_(num_points)
  , discretization_(discretization)
  , duration_((num_points - 1) * discretization)
  , start_index_(1)
  , end_index_(num_points_ - 2)
{
  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
  num_joints_ = model_group->getActiveJointModels().size();
  init();
}

ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name,
                                 int diff_rule_length)
  : planning_group_name_(group_name), discretization_(source_traj.discretization_)
{
  num_joints_ = source_traj.getNumJoints();

  // figure out the num_points_:
  // we need diff_rule_length-1 extra points on either side:
  int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
  int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);

  num_points_ = source_traj.num_points_ + start_extra + end_extra;
  start_index_ = diff_rule_length - 1;
  end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
  duration_ = (num_points_ - 1) * discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (int i = 0; i < num_points_; i++)
  {
    int source_traj_point = i - start_extra;
    if (source_traj_point < 0)
      source_traj_point = 0;
    if (source_traj_point >= source_traj.num_points_)
      source_traj_point = source_traj.num_points_ - 1;
    full_trajectory_index_[i] = source_traj_point;
    for (int j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = source_traj(source_traj_point, j);
    }
  }
}

ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name,
                                 const trajectory_msgs::JointTrajectory& traj)
  : planning_group_name_(group_name)
{
  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
  num_points_ = traj.points.size();
  num_joints_ = model_group->getActiveJointModels().size();
  assert(num_points_ > 2);

  double discretization = (traj.points[1].time_from_start - traj.points[0].time_from_start).toSec();
  double discretization2 = (traj.points[2].time_from_start - traj.points[1].time_from_start).toSec();
  if (fabs(discretization2 - discretization) > .001)
  {
    ROS_WARN_STREAM("Trajectory Discretization not constant " << discretization << " " << discretization2);
  }
  discretization_ = discretization;

  duration_ = (traj.points.back().time_from_start - traj.points[0].time_from_start).toSec();

  start_index_ = 1;
  end_index_ = num_points_ - 2;

  init();

  for (unsigned int i = 0; i < traj.points.size(); i++)
    trajectory_.row(i) = Eigen::Map<Eigen::VectorXd>(traj.points[i].positions, num_joints_);
}

void ChompTrajectory::init()
{
  trajectory_.resize(num_points_, num_joints_);
}

void ChompTrajectory::updateFromGroupTrajectory(const ChompTrajectory& group_trajectory)
{
  int num_vars_free = end_index_ - start_index_ + 1;
  trajectory_.block(start_index_, 0, num_vars_free, num_joints_) =
      group_trajectory.trajectory_.block(group_trajectory.start_index_, 0, num_vars_free, num_joints_);
}

void ChompTrajectory::fillInLinearInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  for (int i = 0; i < num_joints_; i++)
  {
    double theta = ((*this)(end_index, i) - (*this)(start_index, i)) / (end_index - 1);
    for (int j = start_index + 1; j < end_index; j++)
    {
      (*this)(j, i) = (*this)(start_index, i) + j * theta;
    }
  }
}

void ChompTrajectory::fillInCubicInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double dt = 0.001;
  std::vector<double> coeffs(4, 0);
  double total_time = (end_index - 1) * dt;
  for (int i = 0; i < num_joints_; i++)
  {
    coeffs[0] = (*this)(start_index, i);
    coeffs[2] = (3 / (pow(total_time, 2))) * ((*this)(end_index, i) - (*this)(start_index, i));
    coeffs[3] = (-2 / (pow(total_time, 3))) * ((*this)(end_index, i) - (*this)(start_index, i));

    double t;
    for (int j = start_index + 1; j < end_index; j++)
    {
      t = j * dt;
      (*this)(j, i) = coeffs[0] + coeffs[2] * pow(t, 2) + coeffs[3] * pow(t, 3);
    }
  }
}

void ChompTrajectory::fillInMinJerk()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double td[6];  // powers of the time duration
  td[0] = 1.0;
  td[1] = (end_index - start_index) * discretization_;

  for (int i = 2; i <= 5; i++)
    td[i] = td[i - 1] * td[1];

  // calculate the spline coefficients for each joint:
  // (these are for the special case of zero start and end vel and acc)
  double coeff[num_joints_][6];
  for (int i = 0; i < num_joints_; i++)
  {
    double x0 = (*this)(start_index, i);
    double x1 = (*this)(end_index, i);
    coeff[i][0] = x0;
    coeff[i][1] = 0;
    coeff[i][2] = 0;
    coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]);
    coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]);
    coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]);
  }

  // now fill in the joint positions at each time step
  for (int i = start_index + 1; i < end_index; i++)
  {
    double ti[6];  // powers of the time index point
    ti[0] = 1.0;
    ti[1] = (i - start_index) * discretization_;
    for (int k = 2; k <= 5; k++)
      ti[k] = ti[k - 1] * ti[1];

    for (int j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = 0.0;
      for (int k = 0; k <= 5; k++)
      {
        (*this)(i, j) += ti[k] * coeff[j][k];
      }
    }
  }
}

bool ChompTrajectory::fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse& res)
{
  // create a RobotTrajectory msg to obtain the trajectory from the MotionPlanDetailedResponse object
  moveit_msgs::RobotTrajectory trajectory_msg = res.trajectory[0];

  // get the default number of points in the CHOMP trajectory
  int num_chomp_trajectory_points = (*this).getNumPoints();
  // get the number of points in the response trajectory obtained from OMPL
  int num_response_points = trajectory_msg.joint_trajectory.points.size();

  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
  if (num_response_points < 2)
    return false;

  // get the number of joints for each robot state
  int num_joints_trajectory = trajectory_msg.joint_trajectory.points[0].positions.size();

  // variables for populating the CHOMP trajectory with correct number of trajectory points
  int repeated_factor = num_chomp_trajectory_points / num_response_points;
  int repeated_balance_factor = num_chomp_trajectory_points % num_response_points;

  // response_point_id stores the point at the stored index location.
  int response_point_id = 0;
  if (num_chomp_trajectory_points >= num_response_points)
  {
    for (int i = 0; i < num_response_points; i++)
    {
      // following for loop repeats each OMPL trajectory pose/row 'repeated_factor' times; alternatively, there could
      // also be a linear interpolation between these points later if required
      for (int k = 0; k < repeated_factor; k++)
      {
        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
        response_point_id++;
      }

      // this populates the CHOMP trajectory row  for the remainder number of rows.
      if (i < repeated_balance_factor)
      {
        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
        response_point_id++;
      }  // end of if
    }    // end of for loop for loading in the trajectory poses/rows
  }
  else
  {
    // perform a decimation sampling in this block if the number of trajectory points in the MotionPlanDetailedResponse
    // res object is more than the number of points in the CHOMP trajectory
    double decimation_sampling_factor = ((double)num_response_points) / ((double)num_chomp_trajectory_points);

    for (int i = 0; i < num_chomp_trajectory_points; i++)
    {
      int sampled_point = floor(i * decimation_sampling_factor);
      assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, sampled_point, i);
    }
  }  // end of else
  return true;
}

void ChompTrajectory::assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg,
                                                                         int num_joints_trajectory,
                                                                         int trajectory_msgs_point_index,
                                                                         int chomp_trajectory_point_index)
{
  // following loop iterates over all joints for a single robot pose, it assigns the corresponding input trajectory
  // points into CHOMP trajectory points
  for (int j = 0; j < num_joints_trajectory; j++)
    (*this)(chomp_trajectory_point_index, j) =
        trajectory_msg.joint_trajectory.points[trajectory_msgs_point_index].positions[j];
}

}  // namespace chomp
<commit_msg>remove ChompTrajectory(trajectory_msg)<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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 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: Mrinal Kalakrishnan */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <iostream>

namespace chomp
{
ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
                                 double discretization, const std::string& group_name)
  : ChompTrajectory(robot_model, static_cast<unsigned int>(duration / discretization) + 1, discretization, group_name)
{
}

ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, unsigned int num_points,
                                 double discretization, const std::string& group_name)
  : planning_group_name_(group_name)
  , num_points_(num_points)
  , discretization_(discretization)
  , duration_((num_points - 1) * discretization)
  , start_index_(1)
  , end_index_(num_points_ - 2)
{
  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
  num_joints_ = model_group->getActiveJointModels().size();
  init();
}

ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name,
                                 int diff_rule_length)
  : planning_group_name_(group_name), discretization_(source_traj.discretization_)
{
  num_joints_ = source_traj.getNumJoints();

  // figure out the num_points_:
  // we need diff_rule_length-1 extra points on either side:
  int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
  int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);

  num_points_ = source_traj.num_points_ + start_extra + end_extra;
  start_index_ = diff_rule_length - 1;
  end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
  duration_ = (num_points_ - 1) * discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (int i = 0; i < num_points_; i++)
  {
    int source_traj_point = i - start_extra;
    if (source_traj_point < 0)
      source_traj_point = 0;
    if (source_traj_point >= source_traj.num_points_)
      source_traj_point = source_traj.num_points_ - 1;
    full_trajectory_index_[i] = source_traj_point;
    for (int j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = source_traj(source_traj_point, j);
    }
  }
}

void ChompTrajectory::init()
{
  trajectory_.resize(num_points_, num_joints_);
}

void ChompTrajectory::updateFromGroupTrajectory(const ChompTrajectory& group_trajectory)
{
  int num_vars_free = end_index_ - start_index_ + 1;
  trajectory_.block(start_index_, 0, num_vars_free, num_joints_) =
      group_trajectory.trajectory_.block(group_trajectory.start_index_, 0, num_vars_free, num_joints_);
}

void ChompTrajectory::fillInLinearInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  for (int i = 0; i < num_joints_; i++)
  {
    double theta = ((*this)(end_index, i) - (*this)(start_index, i)) / (end_index - 1);
    for (int j = start_index + 1; j < end_index; j++)
    {
      (*this)(j, i) = (*this)(start_index, i) + j * theta;
    }
  }
}

void ChompTrajectory::fillInCubicInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double dt = 0.001;
  std::vector<double> coeffs(4, 0);
  double total_time = (end_index - 1) * dt;
  for (int i = 0; i < num_joints_; i++)
  {
    coeffs[0] = (*this)(start_index, i);
    coeffs[2] = (3 / (pow(total_time, 2))) * ((*this)(end_index, i) - (*this)(start_index, i));
    coeffs[3] = (-2 / (pow(total_time, 3))) * ((*this)(end_index, i) - (*this)(start_index, i));

    double t;
    for (int j = start_index + 1; j < end_index; j++)
    {
      t = j * dt;
      (*this)(j, i) = coeffs[0] + coeffs[2] * pow(t, 2) + coeffs[3] * pow(t, 3);
    }
  }
}

void ChompTrajectory::fillInMinJerk()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double td[6];  // powers of the time duration
  td[0] = 1.0;
  td[1] = (end_index - start_index) * discretization_;

  for (int i = 2; i <= 5; i++)
    td[i] = td[i - 1] * td[1];

  // calculate the spline coefficients for each joint:
  // (these are for the special case of zero start and end vel and acc)
  double coeff[num_joints_][6];
  for (int i = 0; i < num_joints_; i++)
  {
    double x0 = (*this)(start_index, i);
    double x1 = (*this)(end_index, i);
    coeff[i][0] = x0;
    coeff[i][1] = 0;
    coeff[i][2] = 0;
    coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]);
    coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]);
    coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]);
  }

  // now fill in the joint positions at each time step
  for (int i = start_index + 1; i < end_index; i++)
  {
    double ti[6];  // powers of the time index point
    ti[0] = 1.0;
    ti[1] = (i - start_index) * discretization_;
    for (int k = 2; k <= 5; k++)
      ti[k] = ti[k - 1] * ti[1];

    for (int j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = 0.0;
      for (int k = 0; k <= 5; k++)
      {
        (*this)(i, j) += ti[k] * coeff[j][k];
      }
    }
  }
}

bool ChompTrajectory::fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse& res)
{
  // create a RobotTrajectory msg to obtain the trajectory from the MotionPlanDetailedResponse object
  moveit_msgs::RobotTrajectory trajectory_msg = res.trajectory[0];

  // get the default number of points in the CHOMP trajectory
  int num_chomp_trajectory_points = (*this).getNumPoints();
  // get the number of points in the response trajectory obtained from OMPL
  int num_response_points = trajectory_msg.joint_trajectory.points.size();

  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
  if (num_response_points < 2)
    return false;

  // get the number of joints for each robot state
  int num_joints_trajectory = trajectory_msg.joint_trajectory.points[0].positions.size();

  // variables for populating the CHOMP trajectory with correct number of trajectory points
  int repeated_factor = num_chomp_trajectory_points / num_response_points;
  int repeated_balance_factor = num_chomp_trajectory_points % num_response_points;

  // response_point_id stores the point at the stored index location.
  int response_point_id = 0;
  if (num_chomp_trajectory_points >= num_response_points)
  {
    for (int i = 0; i < num_response_points; i++)
    {
      // following for loop repeats each OMPL trajectory pose/row 'repeated_factor' times; alternatively, there could
      // also be a linear interpolation between these points later if required
      for (int k = 0; k < repeated_factor; k++)
      {
        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
        response_point_id++;
      }

      // this populates the CHOMP trajectory row  for the remainder number of rows.
      if (i < repeated_balance_factor)
      {
        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
        response_point_id++;
      }  // end of if
    }    // end of for loop for loading in the trajectory poses/rows
  }
  else
  {
    // perform a decimation sampling in this block if the number of trajectory points in the MotionPlanDetailedResponse
    // res object is more than the number of points in the CHOMP trajectory
    double decimation_sampling_factor = ((double)num_response_points) / ((double)num_chomp_trajectory_points);

    for (int i = 0; i < num_chomp_trajectory_points; i++)
    {
      int sampled_point = floor(i * decimation_sampling_factor);
      assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, sampled_point, i);
    }
  }  // end of else
  return true;
}

void ChompTrajectory::assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg,
                                                                         int num_joints_trajectory,
                                                                         int trajectory_msgs_point_index,
                                                                         int chomp_trajectory_point_index)
{
  // following loop iterates over all joints for a single robot pose, it assigns the corresponding input trajectory
  // points into CHOMP trajectory points
  for (int j = 0; j < num_joints_trajectory; j++)
    (*this)(chomp_trajectory_point_index, j) =
        trajectory_msg.joint_trajectory.points[trajectory_msgs_point_index].positions[j];
}

}  // namespace chomp
<|endoftext|>


<commit_before>//
//  FileSystem.cpp
//  CSTest
//  Created by Ian Copland on 13/07/2015.
//
//  The MIT License (MIT)
//
//  Copyright (c) 2014 Tag Games Limited
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//

#include <ChilliSource/ChilliSource.h>
#include <ChilliSource/Core/Base.h>
#include <ChilliSource/Core/Container.h>
#include <ChilliSource/Core/File.h>

#include <catch.hpp>

namespace
{
    //------------------------------------------------------------------------------
    /// Tests that two vectors of strings have the same contents. Both must have
    /// identical contents and be the same size, but the elements do not need to be
    /// in the same order.
    ///
    /// @author Ian Copland
    ///
    /// @param in_a - The first vector
    /// @param in_b - The second vector
    ///
    /// @return Whether or not they have the same contents.
    //------------------------------------------------------------------------------
    bool EqualContents(const std::vector<std::string>& in_a, const std::vector<std::string>& in_b)
    {
        if (in_a.size() != in_b.size())
        {
            return false;
        }

        for (const auto& itemA : in_a)
        {
            if (CSCore::VectorUtils::Contains(in_b, itemA) == false)
            {
                return false;
            }
        }

        return true;
    }
    //------------------------------------------------------------------------------
    /// Reads a binary file and returns the first 4 bytes as an unsigned integer. If
    /// the file cannot be read 0 will be returned.
    ///
    /// @author Ian Copland
    ///
    /// @param in_storageLocation - The storage location
    /// @param in_filePath - The file path
    ///
    /// @return The first 4 bytes as an unsigned integer, or 0
    //------------------------------------------------------------------------------
    u32 ReadBinaryFileTest(CSCore::StorageLocation in_storageLocation, const std::string& in_filePath)
    {
        auto fileSystem = CSCore::Application::Get()->GetFileSystem();

        auto fileStream = fileSystem->CreateFileStream(in_storageLocation, in_filePath, CSCore::FileMode::k_readBinary);
        if (fileStream.get() == nullptr)
        {
            return 0;
        }

        fileStream->SeekG(0, CSCore::SeekDir::k_end);
        u32 length = fileStream->TellG();
        if (length < 4)
        {
            return 0;
        }

        u32 output = 0;
        fileStream->SeekG(0, CSCore::SeekDir::k_beginning);
        fileStream->Read(reinterpret_cast<s8*>(&output), 4);

        return output;
    }
    //------------------------------------------------------------------------------
    /// Clears out the given directory. If the directory couldn't be cleared out
    /// (i.e a previous version couldn't be deleted, or a new one couldn't be
    /// created) then this will assert.
    ///
    /// @author Ian Copland
    ///
    /// @param in_storageLocation - The storage location of the directory.
    /// @param in_directoryPath - The directory which should be cleared.
    //------------------------------------------------------------------------------
    void ClearDirectory(CSCore::StorageLocation in_storageLocation, const std::string& in_directoryPath)
    {
        auto fileSystem = CSCore::Application::Get()->GetFileSystem();

        CS_ASSERT(fileSystem->IsStorageLocationWritable(in_storageLocation) == true, "Cannot clear directory in non-writable storage location.");

        if ((in_storageLocation == CSCore::StorageLocation::k_DLC && fileSystem->DoesDirectoryExistInCachedDLC(in_directoryPath) == true) ||
            (in_storageLocation != CSCore::StorageLocation::k_DLC && fileSystem->DoesDirectoryExist(in_storageLocation, in_directoryPath) == true))
        {
            bool deleted = fileSystem->DeleteDirectory(in_storageLocation, in_directoryPath);
            CS_ASSERT(deleted == true, "Cannot perform unit tests becuase test directory couldn't be deleted.");
        }

        bool created = fileSystem->CreateDirectoryPath(in_storageLocation, in_directoryPath);
        CS_ASSERT(created == true, "Cannot perform unit tests becuase test directory couldn't be created.");
    }
}

TEST_CASE("FileSystem:Package", "[FileSystem]")
{
    const char k_textFilePath[] = "UnitTest/FileSystem/TextFileA.txt";
    const char k_binFilePath[] = "UnitTest/FileSystem/BinaryFileA.bin";
    const char k_directoryPathA[] = "UnitTest/FileSystem/DirectoryA/";
    const char k_directoryPathC[] = "UnitTest/FileSystem/DirectoryA/DirectoryC/";
    const char k_textFilePathFake[] = "UnitTest/FileSystem/TextFileAFake.txt";
    const char k_directoryPathFake[] = "UnitTest/FileSystem/DirectoryFake/";
    const char k_searchFileName[] = "BinaryFileD.bin";
    const std::vector<std::string> k_expectedDirectoryContentsA = { "DirectoryB/BinaryFileB.bin", "DirectoryB/TextFileB.txt", "DirectoryC/BinaryFileC.bin", "DirectoryC/TextFileC.txt", "DirectoryC/DirectoryD/BinaryFileD.bin", "DirectoryC/DirectoryD/TextFileD.txt" };
    const std::vector<std::string> k_expectedDirectoryContentsC = { "BinaryFileC.bin", "TextFileC.txt" };
    const std::vector<std::string> k_expectedDirectoryContentsExtA = { "DirectoryB/BinaryFileB.bin", "DirectoryC/BinaryFileC.bin", "DirectoryC/DirectoryD/BinaryFileD.bin" };
    const std::vector<std::string> k_expectedDirectoryContentsExtC = { "BinaryFileC.bin" };
    const std::vector<std::string> k_expectedDirectoryContentsNameA = { "DirectoryC/DirectoryD/BinaryFileD.bin" };
    const std::vector<std::string> k_expectedDirectoryContentsNameC = { };

    auto fileSystem = CSCore::Application::Get()->GetFileSystem();

    SECTION("Checking if files and directories exist")
    {
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_package, k_textFilePath) == true);
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_package, k_textFilePathFake) == false);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_package, k_directoryPathA) == true);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_package, k_directoryPathFake) == false);
    }

    SECTION("Reading text files")
    {
        std::string textFileOutput;
        REQUIRE((fileSystem->ReadFile(CSCore::StorageLocation::k_package, k_textFilePath, textFileOutput) == true && textFileOutput == "Test"));
        REQUIRE(fileSystem->ReadFile(CSCore::StorageLocation::k_package, k_textFilePathFake, textFileOutput) == false);
    }

    SECTION("Reading binary files")
    {
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_package, k_binFilePath) == 9999);
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_chilliSource, k_textFilePathFake) == 0);
    }

    SECTION("Get file paths")
    {
        REQUIRE(EqualContents(fileSystem->GetFilePaths(CSCore::StorageLocation::k_package, k_directoryPathA, true), k_expectedDirectoryContentsA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePaths(CSCore::StorageLocation::k_package, k_directoryPathC, false), k_expectedDirectoryContentsC) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(CSCore::StorageLocation::k_package, k_directoryPathA, true, "bin"), k_expectedDirectoryContentsExtA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(CSCore::StorageLocation::k_package, k_directoryPathC, false, "bin"), k_expectedDirectoryContentsExtC) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(CSCore::StorageLocation::k_package, k_directoryPathA, true, k_searchFileName), k_expectedDirectoryContentsNameA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(CSCore::StorageLocation::k_package, k_directoryPathC, false, k_searchFileName), k_expectedDirectoryContentsNameC) == true);
    }
}

<commit_msg>Added save data, cache and DLC tests.<commit_after>//
//  FileSystem.cpp
//  CSTest
//  Created by Ian Copland on 13/07/2015.
//
//  The MIT License (MIT)
//
//  Copyright (c) 2014 Tag Games Limited
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//

#include <ChilliSource/ChilliSource.h>
#include <ChilliSource/Core/Base.h>
#include <ChilliSource/Core/Container.h>
#include <ChilliSource/Core/File.h>

#include <catch.hpp>

namespace
{
    //------------------------------------------------------------------------------
    /// Tests that two vectors of strings have the same contents. Both must have
    /// identical contents and be the same size, but the elements do not need to be
    /// in the same order.
    ///
    /// @author Ian Copland
    ///
    /// @param in_a - The first vector
    /// @param in_b - The second vector
    ///
    /// @return Whether or not they have the same contents.
    //------------------------------------------------------------------------------
    bool EqualContents(const std::vector<std::string>& in_a, const std::vector<std::string>& in_b)
    {
        if (in_a.size() != in_b.size())
        {
            return false;
        }

        for (const auto& itemA : in_a)
        {
            if (CSCore::VectorUtils::Contains(in_b, itemA) == false)
            {
                return false;
            }
        }

        return true;
    }
    //------------------------------------------------------------------------------
    /// Reads a binary file and returns the first 4 bytes as an unsigned integer. If
    /// the file cannot be read 0 will be returned.
    ///
    /// @author Ian Copland
    ///
    /// @param in_storageLocation - The storage location
    /// @param in_filePath - The file path
    ///
    /// @return The first 4 bytes as an unsigned integer, or 0
    //------------------------------------------------------------------------------
    u32 ReadBinaryFileTest(CSCore::StorageLocation in_storageLocation, const std::string& in_filePath)
    {
        auto fileSystem = CSCore::Application::Get()->GetFileSystem();

        auto fileStream = fileSystem->CreateFileStream(in_storageLocation, in_filePath, CSCore::FileMode::k_readBinary);
        if (fileStream.get() == nullptr)
        {
            return 0;
        }

        fileStream->SeekG(0, CSCore::SeekDir::k_end);
        u32 length = fileStream->TellG();
        if (length < 4)
        {
            return 0;
        }

        u32 output = 0;
        fileStream->SeekG(0, CSCore::SeekDir::k_beginning);
        fileStream->Read(reinterpret_cast<s8*>(&output), 4);

        return output;
    }
    //------------------------------------------------------------------------------
    /// Clears out the given directory. If the directory couldn't be cleared out
    /// (i.e a previous version couldn't be deleted, or a new one couldn't be
    /// created) then this will assert.
    ///
    /// @author Ian Copland
    ///
    /// @param in_storageLocation - The storage location of the directory.
    /// @param in_directoryPath - The directory which should be cleared.
    //------------------------------------------------------------------------------
    void ClearDirectory(CSCore::StorageLocation in_storageLocation, const std::string& in_directoryPath)
    {
        auto fileSystem = CSCore::Application::Get()->GetFileSystem();

        CS_ASSERT(fileSystem->IsStorageLocationWritable(in_storageLocation) == true, "Cannot clear directory in non-writable storage location.");

        if ((in_storageLocation == CSCore::StorageLocation::k_DLC && fileSystem->DoesDirectoryExistInCachedDLC(in_directoryPath) == true) ||
            (in_storageLocation != CSCore::StorageLocation::k_DLC && fileSystem->DoesDirectoryExist(in_storageLocation, in_directoryPath) == true))
        {
            bool deleted = fileSystem->DeleteDirectory(in_storageLocation, in_directoryPath);
            CS_ASSERT(deleted == true, "Cannot perform unit tests becuase test directory couldn't be deleted.");
        }

        bool created = fileSystem->CreateDirectoryPath(in_storageLocation, in_directoryPath);
        CS_ASSERT(created == true, "Cannot perform unit tests becuase test directory couldn't be created.");
    }

    //------------------------------------------------------------------------------
    /// Performs a series of tests on the given storage location. This must be
    /// called from within a test case.
    ///
    /// @author Ian Copland
    ///
    /// @param in_storageLocation - The storage location to perform the tests on
    //------------------------------------------------------------------------------
    void TestWritableStorageLocation(CSCore::StorageLocation in_storageLocation)
    {
        const std::string k_unitTestDirectory = "UnitTest/FileSystem/";
        const std::string k_textFilePath = k_unitTestDirectory + "TextFileZ.txt";
        const std::string k_binFilePath = k_unitTestDirectory + "BinaryFileZ.bin";
        const std::string k_directoryPath = k_unitTestDirectory + "DirectoryZ/";
        const char k_fileCopySource[] = "UnitTest/FileSystem/TextFileA.txt";
        const std::string k_fileCopyDest = k_unitTestDirectory + "CopiedTextFileA.txt";
        const char k_directoryCopySource[] = "UnitTest/FileSystem/DirectoryA/";
        const std::string k_directoryCopyDestA = k_unitTestDirectory + "CopiedDirectoryA/";
        const std::string k_copiedDirectoryC = k_directoryCopyDestA + "DirectoryC/";
        const char k_searchFileName[] = "BinaryFileD.bin";
        const u32 k_binaryFileContents = 9999;
        const std::vector<std::string> k_expectedFilePathsA = { "DirectoryB/BinaryFileB.bin", "DirectoryB/TextFileB.txt", "DirectoryC/BinaryFileC.bin", "DirectoryC/TextFileC.txt", "DirectoryC/DirectoryD/BinaryFileD.bin",
            "DirectoryC/DirectoryD/TextFileD.txt" };
        const std::vector<std::string> k_expectedFilePathsC = { "BinaryFileC.bin", "TextFileC.txt" };
        const std::vector<std::string> k_expectedFilePathsExtA = { "DirectoryB/BinaryFileB.bin", "DirectoryC/BinaryFileC.bin", "DirectoryC/DirectoryD/BinaryFileD.bin" };
        const std::vector<std::string> k_expectedFilePathsExtC = { "BinaryFileC.bin" };
        const std::vector<std::string> k_expectedFilePathsNameA = { "DirectoryC/DirectoryD/BinaryFileD.bin" };
        const std::vector<std::string> k_expectedFilePathsNameC = { };

        auto fileSystem = CSCore::Application::Get()->GetFileSystem();
        CS_ASSERT(fileSystem->IsStorageLocationWritable(in_storageLocation) == true, "Storage location must be writable");

        ClearDirectory(in_storageLocation, k_unitTestDirectory);

        SECTION("Reading and writing text files")
        {
            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_textFilePath) == false);
            REQUIRE(fileSystem->WriteFile(in_storageLocation, k_textFilePath, "Test") == true);
            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_textFilePath) == true);

            std::string textFileOutput;
            REQUIRE((fileSystem->ReadFile(in_storageLocation, k_textFilePath, textFileOutput) == true && textFileOutput == "Test"));
            REQUIRE(fileSystem->ReadFile(in_storageLocation, k_fileCopyDest, textFileOutput) == false);
        }

        SECTION("Reading and writing binary files")
        {
            REQUIRE(fileSystem->WriteFile(in_storageLocation, k_binFilePath, reinterpret_cast<const s8*>(&k_binaryFileContents), 4) == true);
            REQUIRE(ReadBinaryFileTest(in_storageLocation, k_binFilePath) == k_binaryFileContents);
            REQUIRE(ReadBinaryFileTest(in_storageLocation, k_fileCopyDest) == 0);
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryPath) == false);
        }

        SECTION("Creating directories")
        {
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryPath) == false);
            REQUIRE(fileSystem->CreateDirectoryPath(in_storageLocation, k_directoryPath) == true);

            //Check that creating a directory that already exists will not report an error.
            REQUIRE(fileSystem->CreateDirectoryPath(in_storageLocation, k_directoryPath) == true);
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryPath) == true);
        }

        SECTION("Copy file")
        {
            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_fileCopyDest) == false);
            REQUIRE(fileSystem->CopyFile(CSCore::StorageLocation::k_package, k_fileCopySource, in_storageLocation, k_fileCopyDest) == true);
            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_fileCopyDest) == true);
        }

        SECTION("Copy directory")
        {
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryCopyDestA) == false);
            REQUIRE(fileSystem->CopyDirectory(CSCore::StorageLocation::k_package, k_directoryCopySource, in_storageLocation, k_directoryCopyDestA) == true);
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryCopyDestA) == true);
        }

        SECTION("Get file paths")
        {
            fileSystem->CopyDirectory(CSCore::StorageLocation::k_package, k_directoryCopySource, in_storageLocation, k_directoryCopyDestA);

            REQUIRE(EqualContents(fileSystem->GetFilePaths(in_storageLocation, k_directoryCopyDestA, true), k_expectedFilePathsA) == true);
            REQUIRE(EqualContents(fileSystem->GetFilePaths(in_storageLocation, k_copiedDirectoryC, false), k_expectedFilePathsC) == true);
            REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(in_storageLocation, k_directoryCopyDestA, true, "bin"), k_expectedFilePathsExtA) == true);
            REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(in_storageLocation, k_copiedDirectoryC, false, "bin"), k_expectedFilePathsExtC) == true);
            REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(in_storageLocation, k_directoryCopyDestA, true, k_searchFileName), k_expectedFilePathsNameA) == true);
            REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(in_storageLocation, k_copiedDirectoryC, false, k_searchFileName), k_expectedFilePathsNameC) == true);
        }

        //TODO: !? add test for GetDirectories()

        SECTION("Delete file")
        {
            fileSystem->CopyFile(CSCore::StorageLocation::k_package, k_fileCopySource, in_storageLocation, k_fileCopyDest);

            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_fileCopyDest) == true);
            REQUIRE(fileSystem->DeleteFile(in_storageLocation, k_fileCopyDest) == true);
            REQUIRE(fileSystem->DoesFileExist(in_storageLocation, k_fileCopyDest) == false);

            //ensure that trying to delete a file that doesn't exist reports an error.
            REQUIRE(fileSystem->DeleteFile(in_storageLocation, k_fileCopyDest) == false);
        }

        SECTION("Delete directory")
        {
            fileSystem->CopyDirectory(CSCore::StorageLocation::k_package, k_directoryCopySource, in_storageLocation, k_directoryCopyDestA);

            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryCopyDestA) == true);
            REQUIRE(fileSystem->DeleteDirectory(in_storageLocation, k_directoryCopyDestA) == true);
            REQUIRE(fileSystem->DoesDirectoryExist(in_storageLocation, k_directoryCopyDestA) == false);

            //ensure that trying to delete a directory that doesn't exist reports an error.
            REQUIRE(fileSystem->DeleteDirectory(in_storageLocation, k_directoryCopyDestA) == false);
        }

        ClearDirectory(in_storageLocation, k_unitTestDirectory);
    }
}

TEST_CASE("FileSystem: Package storage location", "[FileSystem]")
{
    const char k_textFilePath[] = "UnitTest/FileSystem/TextFileA.txt";
    const char k_binFilePath[] = "UnitTest/FileSystem/BinaryFileA.bin";
    const char k_directoryPathA[] = "UnitTest/FileSystem/DirectoryA/";
    const char k_directoryPathC[] = "UnitTest/FileSystem/DirectoryA/DirectoryC/";
    const char k_textFilePathFake[] = "UnitTest/FileSystem/TextFileAFake.txt";
    const char k_directoryPathFake[] = "UnitTest/FileSystem/DirectoryFake/";
    const char k_searchFileName[] = "BinaryFileD.bin";
    const std::vector<std::string> k_expectedFilePathsA = { "DirectoryB/BinaryFileB.bin", "DirectoryB/TextFileB.txt", "DirectoryC/BinaryFileC.bin", "DirectoryC/TextFileC.txt",
        "DirectoryC/DirectoryD/BinaryFileD.bin", "DirectoryC/DirectoryD/TextFileD.txt" };
    const std::vector<std::string> k_expectedFilePathsC = { "BinaryFileC.bin", "TextFileC.txt" };
    const std::vector<std::string> k_expectedFilePathsExtA = { "DirectoryB/BinaryFileB.bin", "DirectoryC/BinaryFileC.bin", "DirectoryC/DirectoryD/BinaryFileD.bin" };
    const std::vector<std::string> k_expectedFilePathsExtC = { "BinaryFileC.bin" };
    const std::vector<std::string> k_expectedFilePathsNameA = { "DirectoryC/DirectoryD/BinaryFileD.bin" };
    const std::vector<std::string> k_expectedFilePathsNameC = { };

    auto fileSystem = CSCore::Application::Get()->GetFileSystem();

    SECTION("Checking if files and directories exist")
    {
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_package, k_textFilePath) == true);
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_package, k_textFilePathFake) == false);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_package, k_directoryPathA) == true);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_package, k_directoryPathFake) == false);
    }

    SECTION("Reading text files")
    {
        std::string textFileOutput;
        REQUIRE((fileSystem->ReadFile(CSCore::StorageLocation::k_package, k_textFilePath, textFileOutput) == true && textFileOutput == "Test"));
        REQUIRE(fileSystem->ReadFile(CSCore::StorageLocation::k_package, k_textFilePathFake, textFileOutput) == false);
    }

    SECTION("Reading binary files")
    {
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_package, k_binFilePath) == 9999);
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_chilliSource, k_textFilePathFake) == 0);
    }

    SECTION("Get file paths")
    {
        REQUIRE(EqualContents(fileSystem->GetFilePaths(CSCore::StorageLocation::k_package, k_directoryPathA, true), k_expectedFilePathsA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePaths(CSCore::StorageLocation::k_package, k_directoryPathC, false), k_expectedFilePathsC) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(CSCore::StorageLocation::k_package, k_directoryPathA, true, "bin"), k_expectedFilePathsExtA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(CSCore::StorageLocation::k_package, k_directoryPathC, false, "bin"), k_expectedFilePathsExtC) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(CSCore::StorageLocation::k_package, k_directoryPathA, true, k_searchFileName), k_expectedFilePathsNameA) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(CSCore::StorageLocation::k_package, k_directoryPathC, false, k_searchFileName), k_expectedFilePathsNameC) == true);
    }

    //TODO: !? add test for GetDirectories()
}

TEST_CASE("FileSystem: ChilliSource storage location", "[FileSystem]")
{
    const char k_textFilePath[] = "Widgets/Widget.csuidef";
    const char k_binFilePath[] = "Fonts/CarlitoMed.low.csfont";
    const char k_directoryPath[] = "Fonts/";
    const char k_textFilePathFake[] = "TextFileAFake.txt";
    const char k_directoryPathFake[] = "DirectoryFake/";
    const char k_searchFileName[] = "CarlitoMed.high.csfont";
    const char k_widgetFileContents[] = "{\n  \"Type\": \"Widget\",\n  \n  \"DefaultPropertyValues\": {\n    \"Name\": \"Widget\"\n  }\n}";
    const std::vector<std::string> k_expectedFilePaths = { "CarlitoMed.high.csfont", "CarlitoMed.high.csimage", "CarlitoMed.low.csfont", "CarlitoMed.low.csimage", "CarlitoMed.med.csfont", "CarlitoMed.med.csimage" };
    const std::vector<std::string> k_expectedFilePathsExt = { "CarlitoMed.high.csfont", "CarlitoMed.low.csfont", "CarlitoMed.med.csfont", };
    const std::vector<std::string> k_expectedFilePathsName = { "CarlitoMed.high.csfont" };
    const u32 k_cscsInt = 1396921155; //'CSCS' in little endian

    auto fileSystem = CSCore::Application::Get()->GetFileSystem();

    SECTION("Checking if files and directories exist")
    {
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_chilliSource, k_textFilePath) == true);
        REQUIRE(fileSystem->DoesFileExist(CSCore::StorageLocation::k_chilliSource, k_textFilePathFake) == false);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_chilliSource, k_directoryPath) == true);
        REQUIRE(fileSystem->DoesDirectoryExist(CSCore::StorageLocation::k_chilliSource, k_directoryPathFake) == false);
    }

    SECTION("Reading text files")
    {
        std::string textFileOutput;
        REQUIRE((fileSystem->ReadFile(CSCore::StorageLocation::k_chilliSource, k_textFilePath, textFileOutput) && textFileOutput == k_widgetFileContents));
        REQUIRE(fileSystem->ReadFile(CSCore::StorageLocation::k_chilliSource, k_textFilePathFake, textFileOutput) == false);
    }

    SECTION("Reading binary files")
    {
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_chilliSource, k_binFilePath) == k_cscsInt);
        REQUIRE(ReadBinaryFileTest(CSCore::StorageLocation::k_chilliSource, k_textFilePathFake) == 0);
    }

    SECTION("Get file paths")
    {
        REQUIRE(EqualContents(fileSystem->GetFilePaths(CSCore::StorageLocation::k_chilliSource, k_directoryPath, false), k_expectedFilePaths) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithExtension(CSCore::StorageLocation::k_chilliSource, k_directoryPath, false, "csfont"), k_expectedFilePathsExt) == true);
        REQUIRE(EqualContents(fileSystem->GetFilePathsWithFileName(CSCore::StorageLocation::k_chilliSource, k_directoryPath, false, k_searchFileName), k_expectedFilePathsName) == true);
    }

    //TODO: !? add test for GetDirectories()
}

TEST_CASE("FileSystem: Cache storage location", "[FileSystem]")
{
    TestWritableStorageLocation(CSCore::StorageLocation::k_cache);
}

TEST_CASE("FileSystem: SaveData storage location", "[FileSystem]")
{
    TestWritableStorageLocation(CSCore::StorageLocation::k_saveData);
}

TEST_CASE("FileSystem: DLC storage location", "[FileSystem]")
{
    TestWritableStorageLocation(CSCore::StorageLocation::k_DLC);
}

//TODO: !? Add remaining FileSystem tests.
<|endoftext|>