<commit_before>/**
 * \file
 * \brief MutexPriorityTestCase class implementation
 *
 * \author Copyright (C) 2014 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * 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/.
 *
 * \date 2014-11-09
 */

#include "MutexPriorityTestCase.hpp"

#include "priorityTestPhases.hpp"
#include "SequenceAsserter.hpp"

#include "distortos/StaticThread.hpp"
#include "distortos/Mutex.hpp"
#include "distortos/ThisThread.hpp"

namespace distortos
{

namespace test
{

namespace
{

/*---------------------------------------------------------------------------------------------------------------------+
| local constants
+---------------------------------------------------------------------------------------------------------------------*/

/// size of stack for test thread, bytes
constexpr size_t testThreadStackSize {256};

/*---------------------------------------------------------------------------------------------------------------------+
| local functions' declarations
+---------------------------------------------------------------------------------------------------------------------*/

void thread(SequenceAsserter& sequenceAsserter, unsigned int sequencePoint, Mutex& mutex);

/*---------------------------------------------------------------------------------------------------------------------+
| local types
+---------------------------------------------------------------------------------------------------------------------*/

/// type of test thread
using TestThread = decltype(makeStaticThread<testThreadStackSize>({}, thread,
		std::ref(std::declval<SequenceAsserter&>()), std::declval<unsigned int>(), std::ref(std::declval<Mutex&>())));

/*---------------------------------------------------------------------------------------------------------------------+
| local functions
+---------------------------------------------------------------------------------------------------------------------*/

/**
 * \brief Test thread.
 *
 * Locks the mutex, marks the sequence point in SequenceAsserter and unlocks the mutex.
 *
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] sequencePoint is the sequence point of this instance
 * \param [in] mutex is a reference to shared mutex
 */

void thread(SequenceAsserter& sequenceAsserter, const unsigned int sequencePoint, Mutex& mutex)
{
	mutex.lock();
	sequenceAsserter.sequencePoint(sequencePoint);
	mutex.unlock();
}

/**
 * \brief Builder of TestThread objects.
 *
 * \param [in] threadParameters is a reference to ThreadParameters object
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] mutex is a reference to shared mutex
 *
 * \return constructed TestThread object
 */

TestThread makeTestThread(const ThreadParameters& threadParameters, SequenceAsserter& sequenceAsserter, Mutex& mutex)
{
	return makeStaticThread<testThreadStackSize>(threadParameters.first, thread, std::ref(sequenceAsserter),
			static_cast<unsigned int>(threadParameters.second), std::ref(mutex));
}

}	// namespace

/*---------------------------------------------------------------------------------------------------------------------+
| private functions
+---------------------------------------------------------------------------------------------------------------------*/

bool MutexPriorityTestCase::run_() const
{
	for (const auto& phase : priorityTestPhases)
	{
		SequenceAsserter sequenceAsserter;
		Mutex mutex;

		std::array<TestThread, totalThreads> threads
		{{
				makeTestThread(phase.first[phase.second[0]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[1]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[2]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[3]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[4]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[5]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[6]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[7]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[8]], sequenceAsserter, mutex),
				makeTestThread(phase.first[phase.second[9]], sequenceAsserter, mutex),
		}};

		// make sure all test threads are blocked on mutex, even if this thread inherits their high priority
		ThisThread::sleepFor(TickClock::duration{2});

		mutex.lock();

		for (auto& thread : threads)
			thread.start();

		mutex.unlock();

		for (auto& thread : threads)
			thread.join();

		if (sequenceAsserter.assertSequence(totalThreads) == false)
			return false;
	}

	return true;
}

}	// namespace test

}	// namespace distortos
<commit_msg>test: execute MutexPriorityTestCase for all valid combinations of mutex types, protocols and priority ceilings<commit_after>/**
 * \file
 * \brief MutexPriorityTestCase class implementation
 *
 * \author Copyright (C) 2014 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * 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/.
 *
 * \date 2014-11-09
 */

#include "MutexPriorityTestCase.hpp"

#include "priorityTestPhases.hpp"
#include "SequenceAsserter.hpp"

#include "distortos/StaticThread.hpp"
#include "distortos/Mutex.hpp"
#include "distortos/ThisThread.hpp"

namespace distortos
{

namespace test
{

namespace
{

/*---------------------------------------------------------------------------------------------------------------------+
| local constants
+---------------------------------------------------------------------------------------------------------------------*/

/// size of stack for test thread, bytes
constexpr size_t testThreadStackSize {256};

/*---------------------------------------------------------------------------------------------------------------------+
| local functions' declarations
+---------------------------------------------------------------------------------------------------------------------*/

void thread(SequenceAsserter& sequenceAsserter, unsigned int sequencePoint, Mutex& mutex);

/*---------------------------------------------------------------------------------------------------------------------+
| local types
+---------------------------------------------------------------------------------------------------------------------*/

/// type of test thread
using TestThread = decltype(makeStaticThread<testThreadStackSize>({}, thread,
		std::ref(std::declval<SequenceAsserter&>()), std::declval<unsigned int>(), std::ref(std::declval<Mutex&>())));

/*---------------------------------------------------------------------------------------------------------------------+
| local functions
+---------------------------------------------------------------------------------------------------------------------*/

/**
 * \brief Test thread.
 *
 * Locks the mutex, marks the sequence point in SequenceAsserter and unlocks the mutex.
 *
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] sequencePoint is the sequence point of this instance
 * \param [in] mutex is a reference to shared mutex
 */

void thread(SequenceAsserter& sequenceAsserter, const unsigned int sequencePoint, Mutex& mutex)
{
	mutex.lock();
	sequenceAsserter.sequencePoint(sequencePoint);
	mutex.unlock();
}

/**
 * \brief Builder of TestThread objects.
 *
 * \param [in] threadParameters is a reference to ThreadParameters object
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] mutex is a reference to shared mutex
 *
 * \return constructed TestThread object
 */

TestThread makeTestThread(const ThreadParameters& threadParameters, SequenceAsserter& sequenceAsserter, Mutex& mutex)
{
	return makeStaticThread<testThreadStackSize>(threadParameters.first, thread, std::ref(sequenceAsserter),
			static_cast<unsigned int>(threadParameters.second), std::ref(mutex));
}

}	// namespace

/*---------------------------------------------------------------------------------------------------------------------+
| private functions
+---------------------------------------------------------------------------------------------------------------------*/

bool MutexPriorityTestCase::run_() const
{
	using Parameters = std::tuple<Mutex::Type, Mutex::Protocol, uint8_t>;
	static const Parameters parametersArray[]
	{
			Parameters{Mutex::Type::Normal, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityInheritance, {}},
	};

	for (const auto& parameters : parametersArray)
		for (const auto& phase : priorityTestPhases)
		{
			SequenceAsserter sequenceAsserter;
			Mutex mutex {std::get<0>(parameters), std::get<1>(parameters), std::get<2>(parameters)};

			std::array<TestThread, totalThreads> threads
			{{
					makeTestThread(phase.first[phase.second[0]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[1]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[2]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[3]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[4]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[5]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[6]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[7]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[8]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[9]], sequenceAsserter, mutex),
			}};

			mutex.lock();

			for (auto& thread : threads)
				thread.start();

			// make sure all test threads are blocked on mutex, even if this thread inherits their high priority
			ThisThread::sleepFor(TickClock::duration{2});

			mutex.unlock();

			for (auto& thread : threads)
				thread.join();

			if (sequenceAsserter.assertSequence(totalThreads) == false)
				return false;
		}

	return true;
}

}	// namespace test

}	// namespace distortos
<|endoftext|>


<commit_before>/**
 * \file
 * \brief MutexPriorityTestCase class implementation
 *
 * \author Copyright (C) 2014-2015 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * 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/.
 *
 * \date 2015-12-02
 */

#include "MutexPriorityTestCase.hpp"

#include "priorityTestPhases.hpp"
#include "SequenceAsserter.hpp"

#include "distortos/StaticThread.hpp"
#include "distortos/Mutex.hpp"
#include "distortos/ThisThread.hpp"

#include "distortos/architecture/InterruptMaskingLock.hpp"

namespace distortos
{

namespace test
{

namespace
{

/*---------------------------------------------------------------------------------------------------------------------+
| local constants
+---------------------------------------------------------------------------------------------------------------------*/

/// size of stack for test thread, bytes
constexpr size_t testThreadStackSize {256};

/*---------------------------------------------------------------------------------------------------------------------+
| local functions' declarations
+---------------------------------------------------------------------------------------------------------------------*/

void thread(SequenceAsserter& sequenceAsserter, unsigned int sequencePoint, Mutex& mutex);

/*---------------------------------------------------------------------------------------------------------------------+
| local types
+---------------------------------------------------------------------------------------------------------------------*/

/// type of test thread
using TestThread = decltype(makeStaticThread<testThreadStackSize>({}, thread,
		std::ref(std::declval<SequenceAsserter&>()), std::declval<unsigned int>(), std::ref(std::declval<Mutex&>())));

/*---------------------------------------------------------------------------------------------------------------------+
| local functions
+---------------------------------------------------------------------------------------------------------------------*/

/**
 * \brief Test thread.
 *
 * Locks the mutex, marks the sequence point in SequenceAsserter and unlocks the mutex.
 *
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] sequencePoint is the sequence point of this instance
 * \param [in] mutex is a reference to shared mutex
 */

void thread(SequenceAsserter& sequenceAsserter, const unsigned int sequencePoint, Mutex& mutex)
{
	mutex.lock();
	sequenceAsserter.sequencePoint(sequencePoint);
	mutex.unlock();
}

/**
 * \brief Builder of TestThread objects.
 *
 * \param [in] threadParameters is a reference to ThreadParameters object
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] mutex is a reference to shared mutex
 *
 * \return constructed TestThread object
 */

TestThread makeTestThread(const ThreadParameters& threadParameters, SequenceAsserter& sequenceAsserter, Mutex& mutex)
{
	return makeStaticThread<testThreadStackSize>(threadParameters.first, thread, std::ref(sequenceAsserter),
			static_cast<unsigned int>(threadParameters.second), std::ref(mutex));
}

}	// namespace

/*---------------------------------------------------------------------------------------------------------------------+
| private functions
+---------------------------------------------------------------------------------------------------------------------*/

bool MutexPriorityTestCase::run_() const
{
	using Parameters = std::tuple<Mutex::Type, Mutex::Protocol, uint8_t>;
	static const Parameters parametersArray[]
	{
			Parameters{Mutex::Type::Normal, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityInheritance, {}},
	};

	for (const auto& parameters : parametersArray)
		for (const auto& phase : priorityTestPhases)
		{
			SequenceAsserter sequenceAsserter;
			Mutex mutex {std::get<0>(parameters), std::get<1>(parameters), std::get<2>(parameters)};

			std::array<TestThread, totalThreads> threads
			{{
					makeTestThread(phase.first[phase.second[0]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[1]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[2]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[3]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[4]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[5]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[6]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[7]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[8]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[9]], sequenceAsserter, mutex),
			}};

			mutex.lock();

			{
				architecture::InterruptMaskingLock interruptMaskingLock;

				for (auto& thread : threads)
				{
					thread.start();
					// make sure each test threads blocks on mutex in the order of starting, even if current thread
					// inherits test thread's high priority
					ThisThread::sleepFor(TickClock::duration{1});
				}
			}

			bool invalidState {};
			for (const auto& thread : threads)
				if (thread.getState() != ThreadState::BlockedOnMutex)
					invalidState = true;

			mutex.unlock();

			for (auto& thread : threads)
				thread.join();

			if (invalidState != false)
				return false;

			if (sequenceAsserter.assertSequence(totalThreads) == false)
				return false;
		}

	return true;
}

}	// namespace test

}	// namespace distortos
<commit_msg>test: convert MutexPriorityTestCase to use dynamic threads<commit_after>/**
 * \file
 * \brief MutexPriorityTestCase class implementation
 *
 * \author Copyright (C) 2014-2016 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * 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/.
 *
 * \date 2016-01-04
 */

#include "MutexPriorityTestCase.hpp"

#include "priorityTestPhases.hpp"
#include "SequenceAsserter.hpp"

#include "distortos/architecture/InterruptMaskingLock.hpp"

#include "distortos/DynamicThread.hpp"
#include "distortos/Mutex.hpp"
#include "distortos/ThisThread.hpp"

namespace distortos
{

namespace test
{

namespace
{

/*---------------------------------------------------------------------------------------------------------------------+
| local constants
+---------------------------------------------------------------------------------------------------------------------*/

/// size of stack for test thread, bytes
constexpr size_t testThreadStackSize {256};

/*---------------------------------------------------------------------------------------------------------------------+
| local functions
+---------------------------------------------------------------------------------------------------------------------*/

/**
 * \brief Test thread
 *
 * Locks the mutex, marks the sequence point in SequenceAsserter and unlocks the mutex.
 *
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] sequencePoint is the sequence point of this instance
 * \param [in] mutex is a reference to shared mutex
 */

void thread(SequenceAsserter& sequenceAsserter, const unsigned int sequencePoint, Mutex& mutex)
{
	mutex.lock();
	sequenceAsserter.sequencePoint(sequencePoint);
	mutex.unlock();
}

/**
 * \brief Builder of test threads
 *
 * \param [in] threadParameters is a reference to ThreadParameters object
 * \param [in] sequenceAsserter is a reference to SequenceAsserter shared object
 * \param [in] mutex is a reference to shared mutex
 *
 * \return constructed DynamicThread object
 */

DynamicThread makeTestThread(const ThreadParameters& threadParameters, SequenceAsserter& sequenceAsserter, Mutex& mutex)
{
	return makeDynamicThread({testThreadStackSize, threadParameters.first}, thread, std::ref(sequenceAsserter),
			threadParameters.second, std::ref(mutex));
}

}	// namespace

/*---------------------------------------------------------------------------------------------------------------------+
| private functions
+---------------------------------------------------------------------------------------------------------------------*/

bool MutexPriorityTestCase::run_() const
{
	using Parameters = std::tuple<Mutex::Type, Mutex::Protocol, uint8_t>;
	static const Parameters parametersArray[]
	{
			Parameters{Mutex::Type::Normal, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Normal, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::ErrorChecking, Mutex::Protocol::PriorityInheritance, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::None, {}},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityProtect, UINT8_MAX},
			Parameters{Mutex::Type::Recursive, Mutex::Protocol::PriorityInheritance, {}},
	};

	for (const auto& parameters : parametersArray)
		for (const auto& phase : priorityTestPhases)
		{
			SequenceAsserter sequenceAsserter;
			Mutex mutex {std::get<0>(parameters), std::get<1>(parameters), std::get<2>(parameters)};

			std::array<DynamicThread, totalThreads> threads
			{{
					makeTestThread(phase.first[phase.second[0]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[1]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[2]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[3]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[4]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[5]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[6]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[7]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[8]], sequenceAsserter, mutex),
					makeTestThread(phase.first[phase.second[9]], sequenceAsserter, mutex),
			}};

			mutex.lock();

			{
				architecture::InterruptMaskingLock interruptMaskingLock;

				for (auto& thread : threads)
				{
					thread.start();
					// make sure each test threads blocks on mutex in the order of starting, even if current thread
					// inherits test thread's high priority
					ThisThread::sleepFor(TickClock::duration{1});
				}
			}

			bool invalidState {};
			for (const auto& thread : threads)
				if (thread.getState() != ThreadState::BlockedOnMutex)
					invalidState = true;

			mutex.unlock();

			for (auto& thread : threads)
				thread.join();

			if (invalidState != false)
				return false;

			if (sequenceAsserter.assertSequence(totalThreads) == false)
				return false;
		}

	return true;
}

}	// namespace test

}	// namespace distortos
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 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/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h"

#include <fstream>
#include <iomanip>
#include <map>
#include <sstream>

#include "gtest/gtest.h"

#include "modules/common/log.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/common/file_system_util.h"
#include "modules/perception/obstacle/common/pose_util.h"
#include "modules/perception/obstacle/lidar/object_builder/min_box/min_box.h"

namespace apollo {
namespace perception {

class HmObjectTrackerTest : public testing::Test {
 protected:
  HmObjectTrackerTest() {}
  virtual ~HmObjectTrackerTest() {}
  void SetUp() {
    RegisterFactoryHmObjectTracker();
    FLAGS_work_root = "modules/perception";
    FLAGS_config_manager_path = "conf/config_manager.config";
    if (!ConfigManager::instance()->Init()) {
      AERROR << "failed to Init ConfigManager";
      return;
    }
    hm_tracker_ = new HmObjectTracker();
    object_builder_ = new MinBoxObjectBuilder();
    object_builder_->Init();
    object_builder_options_.ref_center =
        Eigen::Vector3d(0, 0, -1.7);  // velodyne height
    tracker_options_.velodyne_trans.reset(new Eigen::Matrix4d);
  }
  void TearDown() {
    delete hm_tracker_;
    hm_tracker_ = nullptr;
    delete object_builder_;
    object_builder_ = nullptr;
  }

 protected:
  HmObjectTracker* hm_tracker_ = nullptr;
  MinBoxObjectBuilder* object_builder_ = nullptr;
  ObjectBuilderOptions object_builder_options_;
  TrackerOptions tracker_options_;
};

bool ConstructObjects(const std::string& filename,
                      std::vector<ObjectPtr>* objects) {
  std::ifstream ifs(filename);
  if (!ifs.is_open()) {
    AERROR << "failed to open file" << filename;
    return false;
  }
  std::string type;
  int no_point = 0;
  float tmp = 0;
  while (ifs >> type) {
    ifs >> tmp >> tmp >> tmp >> no_point;
    ObjectPtr obj(new Object());
    obj->cloud->resize(no_point);
    for (int j = 0; j < no_point; ++j) {
      ifs >> obj->cloud->points[j].x >> obj->cloud->points[j].y >>
          obj->cloud->points[j].z >> obj->cloud->points[j].intensity;
    }
    (*objects).push_back(obj);
  }
  return true;
}

TEST_F(HmObjectTrackerTest, Track) {
  // test initialization of hm tracker
  EXPECT_TRUE(hm_tracker_->Init());
  // test tracking via hm tracker
  std::string data_path = "modules/perception/data/hm_tracker_test/";
  std::vector<std::string> seg_filenames;
  GetFileNamesInFolderById(data_path, ".seg", &seg_filenames);
  std::vector<std::string> pose_filenames;
  GetFileNamesInFolderById(data_path, ".pose", &pose_filenames);
  int frame_id = -1;
  double time_stamp = 0.0;
  EXPECT_GT(seg_filenames.size(), 0);
  EXPECT_EQ(seg_filenames.size(), pose_filenames.size());
  Eigen::Vector3d global_offset(0, 0, 0);
  for (size_t i = 0; i < seg_filenames.size(); ++i) {
    // read pose
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    if (!ReadPoseFile(data_path + pose_filenames[i], &pose, &frame_id,
                      &time_stamp)) {
      AERROR << "failed to read pose";
      return;
    }
    if (i == 0) {
      global_offset(0) = pose(0, 3);
      global_offset(1) = pose(1, 3);
      global_offset(2) = pose(2, 3);
    }
    pose(0, 3) -= global_offset(0);
    pose(1, 3) -= global_offset(1);
    pose(2, 3) -= global_offset(2);
    // read segments
    std::vector<ObjectPtr> objects;
    if (!ConstructObjects(data_path + seg_filenames[i], &objects)) {
      AERROR << "failed to read segments";
      return;
    }
    // build objects
    object_builder_->Build(object_builder_options_, &objects);
    // test tracking
    *(tracker_options_.velodyne_trans) = pose;
    std::vector<ObjectPtr> result_objects;
    // assert tracking succesfully
    EXPECT_TRUE(hm_tracker_->Track(objects, time_stamp, tracker_options_,
                                   &result_objects));
    // assert reports completly
    EXPECT_TRUE(result_objects.size() >= objects.size());
    std::map<int, int> id_pool;
    for (size_t j = 0; j < result_objects.size(); ++j) {
      int track_id = result_objects[j]->track_id;
      // assert no duplicated track id in the same frame
      EXPECT_TRUE(id_pool.find(track_id) == id_pool.end());
      id_pool[track_id] = 1;
    }
  }
}

}  // namespace perception
}  // namespace apollo
<commit_msg>perception: Fix typo on comments. (#2995)<commit_after>/******************************************************************************
 * Copyright 2017 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/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h"

#include <fstream>
#include <iomanip>
#include <map>
#include <sstream>

#include "gtest/gtest.h"

#include "modules/common/log.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lib/pcl_util/pcl_types.h"
#include "modules/perception/obstacle/common/file_system_util.h"
#include "modules/perception/obstacle/common/pose_util.h"
#include "modules/perception/obstacle/lidar/object_builder/min_box/min_box.h"

namespace apollo {
namespace perception {

class HmObjectTrackerTest : public testing::Test {
 protected:
  HmObjectTrackerTest() {}
  virtual ~HmObjectTrackerTest() {}
  void SetUp() {
    RegisterFactoryHmObjectTracker();
    FLAGS_work_root = "modules/perception";
    FLAGS_config_manager_path = "conf/config_manager.config";
    if (!ConfigManager::instance()->Init()) {
      AERROR << "failed to Init ConfigManager";
      return;
    }
    hm_tracker_ = new HmObjectTracker();
    object_builder_ = new MinBoxObjectBuilder();
    object_builder_->Init();
    object_builder_options_.ref_center =
        Eigen::Vector3d(0, 0, -1.7);  // velodyne height
    tracker_options_.velodyne_trans.reset(new Eigen::Matrix4d);
  }
  void TearDown() {
    delete hm_tracker_;
    hm_tracker_ = nullptr;
    delete object_builder_;
    object_builder_ = nullptr;
  }

 protected:
  HmObjectTracker* hm_tracker_ = nullptr;
  MinBoxObjectBuilder* object_builder_ = nullptr;
  ObjectBuilderOptions object_builder_options_;
  TrackerOptions tracker_options_;
};

bool ConstructObjects(const std::string& filename,
                      std::vector<ObjectPtr>* objects) {
  std::ifstream ifs(filename);
  if (!ifs.is_open()) {
    AERROR << "failed to open file" << filename;
    return false;
  }
  std::string type;
  int no_point = 0;
  float tmp = 0;
  while (ifs >> type) {
    ifs >> tmp >> tmp >> tmp >> no_point;
    ObjectPtr obj(new Object());
    obj->cloud->resize(no_point);
    for (int j = 0; j < no_point; ++j) {
      ifs >> obj->cloud->points[j].x >> obj->cloud->points[j].y >>
          obj->cloud->points[j].z >> obj->cloud->points[j].intensity;
    }
    (*objects).push_back(obj);
  }
  return true;
}

TEST_F(HmObjectTrackerTest, Track) {
  // test initialization of hm tracker
  EXPECT_TRUE(hm_tracker_->Init());
  // test tracking via hm tracker
  std::string data_path = "modules/perception/data/hm_tracker_test/";
  std::vector<std::string> seg_filenames;
  GetFileNamesInFolderById(data_path, ".seg", &seg_filenames);
  std::vector<std::string> pose_filenames;
  GetFileNamesInFolderById(data_path, ".pose", &pose_filenames);
  int frame_id = -1;
  double time_stamp = 0.0;
  EXPECT_GT(seg_filenames.size(), 0);
  EXPECT_EQ(seg_filenames.size(), pose_filenames.size());
  Eigen::Vector3d global_offset(0, 0, 0);
  for (size_t i = 0; i < seg_filenames.size(); ++i) {
    // read pose
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    if (!ReadPoseFile(data_path + pose_filenames[i], &pose, &frame_id,
                      &time_stamp)) {
      AERROR << "failed to read pose";
      return;
    }
    if (i == 0) {
      global_offset(0) = pose(0, 3);
      global_offset(1) = pose(1, 3);
      global_offset(2) = pose(2, 3);
    }
    pose(0, 3) -= global_offset(0);
    pose(1, 3) -= global_offset(1);
    pose(2, 3) -= global_offset(2);
    // read segments
    std::vector<ObjectPtr> objects;
    if (!ConstructObjects(data_path + seg_filenames[i], &objects)) {
      AERROR << "failed to read segments";
      return;
    }
    // build objects
    object_builder_->Build(object_builder_options_, &objects);
    // test tracking
    *(tracker_options_.velodyne_trans) = pose;
    std::vector<ObjectPtr> result_objects;
    // assert tracking successfully
    EXPECT_TRUE(hm_tracker_->Track(objects, time_stamp, tracker_options_,
                                   &result_objects));
    // assert reports completely
    EXPECT_TRUE(result_objects.size() >= objects.size());
    std::map<int, int> id_pool;
    for (size_t j = 0; j < result_objects.size(); ++j) {
      int track_id = result_objects[j]->track_id;
      // assert no duplicated track id in the same frame
      EXPECT_TRUE(id_pool.find(track_id) == id_pool.end());
      id_pool[track_id] = 1;
    }
  }
}

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


<commit_before>/*
 * Copyright (C) 2009 Nokia Corporation.
 *
 * Contact: Marius Vollmer <marius.vollmer@nokia.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * version 2.1 as published by the Free Software Foundation.
 *
 * 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 St, Fifth Floor, Boston, MA
 * 02110-1301 USA
 *
 */

#include <QtTest/QtTest>
#include <QtCore>
#include "nanoxml.h"
#include "fileutils.h"
#include "contexttypeinfo.h"
#include "contexttyperegistryinfo.h"

ContextTypeRegistryInfo* ContextTypeRegistryInfo::registryInstance = new ContextTypeRegistryInfo();

/* Mocked ContextTypeRegistryInfo */

ContextTypeRegistryInfo::ContextTypeRegistryInfo()
{
}

ContextTypeRegistryInfo* ContextTypeRegistryInfo::instance()
{
    return registryInstance;
}

NanoTree ContextTypeRegistryInfo::typeDefinitionForName(QString name)
{
    if (name == "double") {
        QVariantList tree;
        QVariantList doc;
        QVariantList name;
        name << QVariant("name");
        name << QVariant("double");
        doc << QVariant("doc");
        doc << QVariant("double doc");
        tree << QVariant("type");
        tree << QVariant(name);
        tree << QVariant(doc);
        return ContextTypeInfo(QVariant(tree));
    } else if (name == "complex") {
        QVariantList tree;
        QVariantList doc;
        QVariantList base;
        QVariantList name;
        QVariantList params;
        QVariantList p1;
        QVariantList p1Doc;

        name << QVariant("name");
        name << QVariant("complex");

        doc << QVariant("doc");
        doc << QVariant("complex doc");

        base << QVariant("base");
        base << QVariant("double");

        p1 << QVariant("p1");
        p1Doc << QVariant("doc");
        p1Doc << QVariant("p1 doc");
        p1 << QVariant(p1Doc);
        params << QVariant("params");
        params << QVariant(p1);

        tree << QVariant("type");
        tree << QVariant(name);
        tree << QVariant(params);
        tree << QVariant(doc);
        tree << QVariant(base);
        return ContextTypeInfo(QVariant(tree));
    } else
        return ContextTypeInfo();
}

class ContextTypeInfoUnitTest : public QObject
{
    Q_OBJECT

private slots:
    void name();
    void doc();
    void base();
    void definition();
    void parameterDoc();
    void ensureNewTypes();
    void parseDoubleType();
    void parseCustomDoubleType();
    void parseUniformList();
    void parseMap();
};

void ContextTypeInfoUnitTest::name()
{
    QCOMPARE(ContextTypeInfo(QString("bool")).name(), QString("bool"));
    QCOMPARE(ContextTypeInfo(QString("string")).name(), QString("string"));

    QVariantList lst;
    lst << QVariant("int32");
    QCOMPARE(ContextTypeInfo(QVariant(lst)).name(), QString("int32"));
}

void ContextTypeInfoUnitTest::doc()
{
    QCOMPARE(ContextTypeInfo(QString("double")).doc(), QString("double doc"));
}

void ContextTypeInfoUnitTest::definition()
{
    NanoTree def = ContextTypeInfo(QString("double")).definition();
    QCOMPARE(def.keyValue("name").toString(), QString("double"));
    QCOMPARE(def.keyValue("doc").toString(), QString("double doc"));
}

void ContextTypeInfoUnitTest::base()
{
    ContextTypeInfo base = ContextTypeInfo(QString("complex")).base();
    QCOMPARE(base.name(), QString("double"));
    QCOMPARE(base.doc(), QString("double doc"));
}

void ContextTypeInfoUnitTest::parameterDoc()
{
    ContextTypeInfo typeInfo = ContextTypeInfo(QString("complex"));
    QCOMPARE(typeInfo.parameterDoc("p1"), QString("p1 doc"));

}

void ContextTypeInfoUnitTest::ensureNewTypes()
{
    QCOMPARE(ContextTypeInfo(QString("INTEGER")).ensureNewTypes().name(), QString("int32"));
    QCOMPARE(ContextTypeInfo(QString("INT")).ensureNewTypes().name(), QString("int32"));
    QCOMPARE(ContextTypeInfo(QString("TRUTH")).ensureNewTypes().name(), QString("bool"));
    QCOMPARE(ContextTypeInfo(QString("STRING")).ensureNewTypes().name(), QString("string"));
    QCOMPARE(ContextTypeInfo(QString("DOUBLE")).ensureNewTypes().name(), QString("double"));
    QCOMPARE(ContextTypeInfo(QString("bool")).ensureNewTypes().name(), QString("bool"));
}

void ContextTypeInfoUnitTest::parseDoubleType()
{
    /*
    NanoXml parser(LOCAL_FILE("double.xml"));
    QCOMPARE(parser.didFail(), false);

    ContextTypeInfo typeInfo(parser.result());
    QCOMPARE(typeInfo.name(), QString("double"));
    QCOMPARE(typeInfo.parameterDoc("min"), QString("Minimum value"));
    QCOMPARE(typeInfo.parameterDoc("max"), QString("Maximum value"));
    QCOMPARE(typeInfo.doc(), QString("A double value within the given limits."));
    */

    //QStringList params = typeInfo.parameters();
    //QCOMPARE(params.size(), 2);
    //QVERIFY(params.contains(QString("min")));
    //QVERIFY(params.contains(QString("max")));
}

void ContextTypeInfoUnitTest::parseCustomDoubleType()
{
    /*
    NanoXml parser(LOCAL_FILE("custom-double.xml"));
    QCOMPARE(parser.didFail(), false);

    ContextTypeInfo typeInfo(parser.result());
    QCOMPARE(typeInfo.name(), QString("custom-double"));
    QCOMPARE(typeInfo.parameterValue("min"), QString("1"));
    QCOMPARE(typeInfo.parameterValue("max"), QString("666"));
    QCOMPARE(typeInfo.parameterDoc("min"), QString("Minimum value"));
    QCOMPARE(typeInfo.parameterDoc("max"), QString("Maximum value"));
    QCOMPARE(typeInfo.doc(), QString("A double value that represents the level of hell in you."));
    */
}

void ContextTypeInfoUnitTest::parseUniformList()
{
    /*
    NanoXml parser(LOCAL_FILE("uniform-list.xml"));
    QCOMPARE(parser.didFail(), false);

    ContextUniformListTypeInfo listInfo(parser.result());
    QCOMPARE(listInfo.base().name(), QString("list"));
    QCOMPARE(listInfo.name(), QString("uniform-list"));
    ContextTypeInfo elementTypeInfo = listInfo.elementType();
    QCOMPARE(elementTypeInfo.name(), QString("double"));
    */
}

void ContextTypeInfoUnitTest::parseMap()
{
    /*
    NanoXml parser(LOCAL_FILE("person.xml"));
    QCOMPARE(parser.didFail(), false);

    ContextMapTypeInfo personInfo(parser.result());
    QCOMPARE(personInfo.name(), QString("person"));
    QCOMPARE(personInfo.base().name(), QString("map"));
    QCOMPARE(personInfo.keyDoc("name"), QString("Name of the person"));
    QCOMPARE(personInfo.keyDoc("surname"), QString("Surname of the person"));
    QCOMPARE(personInfo.keyDoc("age"), QString("Age of the person"));

    QCOMPARE(personInfo.keyType("name").name(), QString("string"));
    QCOMPARE(personInfo.keyType("surname").name(), QString("string"));
    QCOMPARE(personInfo.keyType("age").name(), QString("int32"));
    */
}

#include "contexttypeinfounittest.moc"
QTEST_MAIN(ContextTypeInfoUnitTest);
<commit_msg>ContextTypeInfo::parameterValue() test.<commit_after>/*
 * Copyright (C) 2009 Nokia Corporation.
 *
 * Contact: Marius Vollmer <marius.vollmer@nokia.com>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * version 2.1 as published by the Free Software Foundation.
 *
 * 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 St, Fifth Floor, Boston, MA
 * 02110-1301 USA
 *
 */

#include <QtTest/QtTest>
#include <QtCore>
#include "nanoxml.h"
#include "fileutils.h"
#include "contexttypeinfo.h"
#include "contexttyperegistryinfo.h"

ContextTypeRegistryInfo* ContextTypeRegistryInfo::registryInstance = new ContextTypeRegistryInfo();

/* Mocked ContextTypeRegistryInfo */

ContextTypeRegistryInfo::ContextTypeRegistryInfo()
{
}

ContextTypeRegistryInfo* ContextTypeRegistryInfo::instance()
{
    return registryInstance;
}

NanoTree ContextTypeRegistryInfo::typeDefinitionForName(QString name)
{
    if (name == "double") {
        QVariantList tree;
        QVariantList doc;
        QVariantList name;
        name << QVariant("name");
        name << QVariant("double");
        doc << QVariant("doc");
        doc << QVariant("double doc");
        tree << QVariant("type");
        tree << QVariant(name);
        tree << QVariant(doc);
        return ContextTypeInfo(QVariant(tree));
    } else if (name == "complex") {
        QVariantList tree;
        QVariantList doc;
        QVariantList base;
        QVariantList name;
        QVariantList params;
        QVariantList p1;
        QVariantList p1Doc;

        name << QVariant("name");
        name << QVariant("complex");

        doc << QVariant("doc");
        doc << QVariant("complex doc");

        base << QVariant("base");
        base << QVariant("double");

        p1 << QVariant("p1");
        p1Doc << QVariant("doc");
        p1Doc << QVariant("p1 doc");
        p1 << QVariant(p1Doc);
        params << QVariant("params");
        params << QVariant(p1);

        tree << QVariant("type");
        tree << QVariant(name);
        tree << QVariant(params);
        tree << QVariant(doc);
        tree << QVariant(base);
        return ContextTypeInfo(QVariant(tree));
    } else
        return ContextTypeInfo();
}

class ContextTypeInfoUnitTest : public QObject
{
    Q_OBJECT

private slots:
    void name();
    void doc();
    void base();
    void definition();
    void parameterDoc();
    void ensureNewTypes();
    void parameterValue();
};

void ContextTypeInfoUnitTest::name()
{
    QCOMPARE(ContextTypeInfo(QString("bool")).name(), QString("bool"));
    QCOMPARE(ContextTypeInfo(QString("string")).name(), QString("string"));

    QVariantList lst;
    lst << QVariant("int32");
    QCOMPARE(ContextTypeInfo(QVariant(lst)).name(), QString("int32"));
}

void ContextTypeInfoUnitTest::doc()
{
    QCOMPARE(ContextTypeInfo(QString("double")).doc(), QString("double doc"));
}

void ContextTypeInfoUnitTest::definition()
{
    NanoTree def = ContextTypeInfo(QString("double")).definition();
    QCOMPARE(def.keyValue("name").toString(), QString("double"));
    QCOMPARE(def.keyValue("doc").toString(), QString("double doc"));
}

void ContextTypeInfoUnitTest::base()
{
    ContextTypeInfo base = ContextTypeInfo(QString("complex")).base();
    QCOMPARE(base.name(), QString("double"));
    QCOMPARE(base.doc(), QString("double doc"));
}

void ContextTypeInfoUnitTest::parameterDoc()
{
    ContextTypeInfo typeInfo = ContextTypeInfo(QString("complex"));
    QCOMPARE(typeInfo.parameterDoc("p1"), QString("p1 doc"));

}

void ContextTypeInfoUnitTest::ensureNewTypes()
{
    QCOMPARE(ContextTypeInfo(QString("INTEGER")).ensureNewTypes().name(), QString("int32"));
    QCOMPARE(ContextTypeInfo(QString("INT")).ensureNewTypes().name(), QString("int32"));
    QCOMPARE(ContextTypeInfo(QString("TRUTH")).ensureNewTypes().name(), QString("bool"));
    QCOMPARE(ContextTypeInfo(QString("STRING")).ensureNewTypes().name(), QString("string"));
    QCOMPARE(ContextTypeInfo(QString("DOUBLE")).ensureNewTypes().name(), QString("double"));
    QCOMPARE(ContextTypeInfo(QString("bool")).ensureNewTypes().name(), QString("bool"));
}

void ContextTypeInfoUnitTest::parameterValue()
{
    QVariantList lst;
    QVariantList minParam;
    lst << QVariant("complex");
    minParam << QVariant("min");
    minParam << QVariant("0");
    lst << QVariant(minParam);
    QVariant tree(lst);

    ContextTypeInfo typeInfo(tree);
    QCOMPARE(typeInfo.parameterValue("min").toString(), QString("0"));
}

#include "contexttypeinfounittest.moc"
QTEST_MAIN(ContextTypeInfoUnitTest);
<|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 <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <eigen_conversions/eigen_msg.h>
#include <ros/console.h>

namespace pick_place
{

// the amount of time (maximum) to wait for achieving a grasp posture
static const double GRASP_POSTURE_COMPLETION_DURATION = 7.0; // seconds

ApproachAndTranslateStage::ApproachAndTranslateStage(const planning_scene::PlanningSceneConstPtr &pre_grasp_scene,
                                                     const planning_scene::PlanningSceneConstPtr &post_grasp_scene,
                                                     const collision_detection::AllowedCollisionMatrixConstPtr &collision_matrix) :
  ManipulationStage("approach & translate"),
  pre_grasp_planning_scene_(pre_grasp_scene),
  post_grasp_planning_scene_(post_grasp_scene),
  collision_matrix_(collision_matrix),
  max_goal_count_(5),
  max_fail_(3),
  max_step_(0.02)
{
}

namespace
{

bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
                          const collision_detection::AllowedCollisionMatrix *collision_matrix,
                          const sensor_msgs::JointState *grasp_posture,
                          kinematic_state::JointStateGroup *joint_state_group,
                          const std::vector<double> &joint_group_variable_values)
{
  joint_state_group->setVariableValues(joint_group_variable_values);
  // apply the grasp posture for the end effector (we always apply it here since it could be the case the sampler changes this posture)
  joint_state_group->getKinematicState()->setStateValues(*grasp_posture);
  return !planning_scene->isStateColliding(*joint_state_group->getKinematicState(), joint_state_group->getName());
}

bool samplePossibleGoalStates(const ManipulationPlanPtr &plan, const kinematic_state::KinematicState &reference_state, double min_distance, unsigned int attempts)
{
  // initialize with scene state
  kinematic_state::KinematicStatePtr token_state(new kinematic_state::KinematicState(reference_state));
  kinematic_state::JointStateGroup *jsg = token_state->getJointStateGroup(plan->planning_group_);
  for (unsigned int j = 0 ; j < attempts ; ++j)
  {
    double min_d = std::numeric_limits<double>::infinity();
    if (plan->goal_sampler_->sample(jsg, *token_state, plan->sampling_attempts_))
    {
      for (std::size_t i = 0 ; i < plan->possible_goal_states_.size() ; ++i)
      {
        double d = plan->possible_goal_states_[i]->getJointStateGroup(plan->planning_group_)->distance(jsg);
        if (d < min_d)
          min_d = d;
      }
      if (min_d >= min_distance)
      {
        plan->possible_goal_states_.push_back(token_state);
        return true;
      }
    }
  }
  return false;
}

void addGraspTrajectory(const ManipulationPlanPtr &plan, const sensor_msgs::JointState &grasp_posture, const std::string &name)
{
  if (!grasp_posture.name.empty())
  {
    moveit_msgs::RobotTrajectory grasp_traj;
    grasp_traj.joint_trajectory.header = grasp_posture.header;
    grasp_traj.joint_trajectory.joint_names = grasp_posture.name;
    grasp_traj.joint_trajectory.points.resize(1);
    grasp_traj.joint_trajectory.points[0].positions = grasp_posture.position;
    grasp_traj.joint_trajectory.points[0].velocities = grasp_posture.velocity;
    grasp_traj.joint_trajectory.points[0].time_from_start = ros::Duration(GRASP_POSTURE_COMPLETION_DURATION);

    plan->trajectories_.push_back(grasp_traj);
    plan->trajectory_descriptions_.push_back(name);
  }
}

}

bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
{
  // compute what the maximum distance reported between any two states in the planning group could be
  double min_distance = 0.0;
  const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
  const std::vector<const kinematic_model::JointModel*> &jmodels = jmg->getJointModels();
  for (std::size_t j = 0 ; j < jmodels.size() ; ++j)
    min_distance += jmodels[j]->getMaximumExtent() * jmodels[j]->getDistanceFactor();
  // now remember the value that is 5% of that maximum distance; this is the minimum we would like goal states to vary,
  // to consider them during the evaluation process
  min_distance *= 0.05;

  // convert approach direction and translation direction to Eigen structures
  Eigen::Vector3d approach_direction, translation_direction;
  tf::vectorMsgToEigen(plan->grasp_.approach_direction, approach_direction);
  tf::vectorMsgToEigen(plan->grasp_.translation_direction, translation_direction);

  // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
  kinematic_state::StateValidityCallbackFn approach_validCallback = boost::bind(&isStateCollisionFree, pre_grasp_planning_scene_.get(),
                                                                                collision_matrix_.get(), &plan->grasp_.pre_grasp_posture, _1, _2);

  // state validity checking during the translation after the grasp must ensure the gripper posture is that of the actual grasp
  kinematic_state::StateValidityCallbackFn translation_validCallback = boost::bind(&isStateCollisionFree, post_grasp_planning_scene_.get(),
                                                                                collision_matrix_.get(), &plan->grasp_.grasp_posture, _1, _2);
  do
  {
    for (std::size_t i = 0 ; i < plan->possible_goal_states_.size() && !signal_stop_ ; ++i)
    {
      // try to compute a straight line path that arrives at the goal using the specified approach direction
      moveit_msgs::RobotTrajectory approach_traj;
      kinematic_state::KinematicStatePtr first_approach_state(new kinematic_state::KinematicState(*plan->possible_goal_states_[i]));
      double d_approach = first_approach_state->getJointStateGroup(plan->planning_group_)->computeCartesianPath(approach_traj, plan->ik_link_name_, -approach_direction,
                                                                                false, plan->grasp_.desired_approach_distance,
                                                                                max_step_, approach_validCallback);

      // if we were able to follow the approach direction for sufficient length, try to compute a translation direction
      if (d_approach > plan->grasp_.min_approach_distance && !signal_stop_)
      {
        if (plan->grasp_.desired_translation_distance > 0.0)
        {

          // try to compute a straight line path that moves from the goal in a desired direction
          moveit_msgs::RobotTrajectory translation_traj;
          kinematic_state::KinematicStatePtr last_translation_state(new kinematic_state::KinematicState(*plan->possible_goal_states_[i]));
          double d_translation = last_translation_state->getJointStateGroup(plan->planning_group_)->computeCartesianPath(translation_traj, plan->ik_link_name_,
                                                                                translation_direction, true,
                                                                                plan->grasp_.desired_translation_distance,
                                                                                max_step_, translation_validCallback);
          // if sufficient progress was made in the desired direction, we have a goal state that we can consider for future stages
          if (d_translation > plan->grasp_.min_translation_distance && !signal_stop_)
          {
            addGraspTrajectory(plan, plan->grasp_.pre_grasp_posture, "pre_grasp");

            plan->approach_state_.swap(first_approach_state);
            plan->translation_state_.swap(last_translation_state);
            trajectory_processing::reverseTrajectory(approach_traj);
            trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), approach_traj.joint_trajectory);
            trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), translation_traj.joint_trajectory);

            const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
            if (jmg)
            {
              const std::vector<moveit_msgs::JointLimits> &jlim = jmg->getVariableLimits();
              time_param_.computeTimeStamps(approach_traj.joint_trajectory, jlim);
              time_param_.computeTimeStamps(translation_traj.joint_trajectory, jlim);
            }

            plan->trajectories_.push_back(approach_traj);
            plan->trajectory_descriptions_.push_back("approach");

            addGraspTrajectory(plan, plan->grasp_.grasp_posture, "grasp");

            plan->trajectories_.push_back(translation_traj);
            plan->trajectory_descriptions_.push_back("translation");

            return true;
          }
        }
        else
        {
          addGraspTrajectory(plan, plan->grasp_.pre_grasp_posture, "pre_grasp");

          plan->approach_state_.swap(first_approach_state);
          trajectory_processing::reverseTrajectory(approach_traj);
          trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), approach_traj.joint_trajectory);

          const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
          if (jmg)
            time_param_.computeTimeStamps(approach_traj.joint_trajectory, jmg->getVariableLimits());

          plan->trajectories_.push_back(approach_traj);
          plan->trajectory_descriptions_.push_back("approach");

          addGraspTrajectory(plan, plan->grasp_.grasp_posture, "grasp");

          return true;
        }
      }

    }
  }
  while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ && samplePossibleGoalStates(plan, pre_grasp_planning_scene_->getCurrentState(), min_distance, max_fail_));
  plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;

  return false;
}

}
<commit_msg>add check for state feasibility as well<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 <moveit/pick_place/approach_and_translate_stage.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <eigen_conversions/eigen_msg.h>
#include <ros/console.h>

namespace pick_place
{

// the amount of time (maximum) to wait for achieving a grasp posture
static const double GRASP_POSTURE_COMPLETION_DURATION = 7.0; // seconds

ApproachAndTranslateStage::ApproachAndTranslateStage(const planning_scene::PlanningSceneConstPtr &pre_grasp_scene,
                                                     const planning_scene::PlanningSceneConstPtr &post_grasp_scene,
                                                     const collision_detection::AllowedCollisionMatrixConstPtr &collision_matrix) :
  ManipulationStage("approach & translate"),
  pre_grasp_planning_scene_(pre_grasp_scene),
  post_grasp_planning_scene_(post_grasp_scene),
  collision_matrix_(collision_matrix),
  max_goal_count_(5),
  max_fail_(3),
  max_step_(0.02)
{
}

namespace
{

bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
                          const collision_detection::AllowedCollisionMatrix *collision_matrix,
                          const sensor_msgs::JointState *grasp_posture,
                          kinematic_state::JointStateGroup *joint_state_group,
                          const std::vector<double> &joint_group_variable_values)
{
  joint_state_group->setVariableValues(joint_group_variable_values);
  // apply the grasp posture for the end effector (we always apply it here since it could be the case the sampler changes this posture)
  joint_state_group->getKinematicState()->setStateValues(*grasp_posture);
  return !planning_scene->isStateColliding(*joint_state_group->getKinematicState(), joint_state_group->getName()) &&
    planning_scene->isStateFeasible(*joint_state_group->getKinematicState());
}

bool samplePossibleGoalStates(const ManipulationPlanPtr &plan, const kinematic_state::KinematicState &reference_state, double min_distance, unsigned int attempts)
{
  // initialize with scene state
  kinematic_state::KinematicStatePtr token_state(new kinematic_state::KinematicState(reference_state));
  kinematic_state::JointStateGroup *jsg = token_state->getJointStateGroup(plan->planning_group_);
  for (unsigned int j = 0 ; j < attempts ; ++j)
  {
    double min_d = std::numeric_limits<double>::infinity();
    if (plan->goal_sampler_->sample(jsg, *token_state, plan->sampling_attempts_))
    {
      for (std::size_t i = 0 ; i < plan->possible_goal_states_.size() ; ++i)
      {
        double d = plan->possible_goal_states_[i]->getJointStateGroup(plan->planning_group_)->distance(jsg);
        if (d < min_d)
          min_d = d;
      }
      if (min_d >= min_distance)
      {
        plan->possible_goal_states_.push_back(token_state);
        return true;
      }
    }
  }
  return false;
}

void addGraspTrajectory(const ManipulationPlanPtr &plan, const sensor_msgs::JointState &grasp_posture, const std::string &name)
{
  if (!grasp_posture.name.empty())
  {
    moveit_msgs::RobotTrajectory grasp_traj;
    grasp_traj.joint_trajectory.header = grasp_posture.header;
    grasp_traj.joint_trajectory.joint_names = grasp_posture.name;
    grasp_traj.joint_trajectory.points.resize(1);
    grasp_traj.joint_trajectory.points[0].positions = grasp_posture.position;
    grasp_traj.joint_trajectory.points[0].velocities = grasp_posture.velocity;
    grasp_traj.joint_trajectory.points[0].time_from_start = ros::Duration(GRASP_POSTURE_COMPLETION_DURATION);

    plan->trajectories_.push_back(grasp_traj);
    plan->trajectory_descriptions_.push_back(name);
  }
}

}

bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
{
  // compute what the maximum distance reported between any two states in the planning group could be
  double min_distance = 0.0;
  const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
  const std::vector<const kinematic_model::JointModel*> &jmodels = jmg->getJointModels();
  for (std::size_t j = 0 ; j < jmodels.size() ; ++j)
    min_distance += jmodels[j]->getMaximumExtent() * jmodels[j]->getDistanceFactor();
  // now remember the value that is 5% of that maximum distance; this is the minimum we would like goal states to vary,
  // to consider them during the evaluation process
  min_distance *= 0.05;

  // convert approach direction and translation direction to Eigen structures
  Eigen::Vector3d approach_direction, translation_direction;
  tf::vectorMsgToEigen(plan->grasp_.approach_direction, approach_direction);
  tf::vectorMsgToEigen(plan->grasp_.translation_direction, translation_direction);

  // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
  kinematic_state::StateValidityCallbackFn approach_validCallback = boost::bind(&isStateCollisionFree, pre_grasp_planning_scene_.get(),
                                                                                collision_matrix_.get(), &plan->grasp_.pre_grasp_posture, _1, _2);

  // state validity checking during the translation after the grasp must ensure the gripper posture is that of the actual grasp
  kinematic_state::StateValidityCallbackFn translation_validCallback = boost::bind(&isStateCollisionFree, post_grasp_planning_scene_.get(),
                                                                                collision_matrix_.get(), &plan->grasp_.grasp_posture, _1, _2);
  do
  {
    for (std::size_t i = 0 ; i < plan->possible_goal_states_.size() && !signal_stop_ ; ++i)
    {
      // try to compute a straight line path that arrives at the goal using the specified approach direction
      moveit_msgs::RobotTrajectory approach_traj;
      kinematic_state::KinematicStatePtr first_approach_state(new kinematic_state::KinematicState(*plan->possible_goal_states_[i]));
      double d_approach = first_approach_state->getJointStateGroup(plan->planning_group_)->computeCartesianPath(approach_traj, plan->ik_link_name_, -approach_direction,
                                                                                false, plan->grasp_.desired_approach_distance,
                                                                                max_step_, approach_validCallback);

      // if we were able to follow the approach direction for sufficient length, try to compute a translation direction
      if (d_approach > plan->grasp_.min_approach_distance && !signal_stop_)
      {
        if (plan->grasp_.desired_translation_distance > 0.0)
        {

          // try to compute a straight line path that moves from the goal in a desired direction
          moveit_msgs::RobotTrajectory translation_traj;
          kinematic_state::KinematicStatePtr last_translation_state(new kinematic_state::KinematicState(*plan->possible_goal_states_[i]));
          double d_translation = last_translation_state->getJointStateGroup(plan->planning_group_)->computeCartesianPath(translation_traj, plan->ik_link_name_,
                                                                                translation_direction, true,
                                                                                plan->grasp_.desired_translation_distance,
                                                                                max_step_, translation_validCallback);
          // if sufficient progress was made in the desired direction, we have a goal state that we can consider for future stages
          if (d_translation > plan->grasp_.min_translation_distance && !signal_stop_)
          {
            addGraspTrajectory(plan, plan->grasp_.pre_grasp_posture, "pre_grasp");

            plan->approach_state_.swap(first_approach_state);
            plan->translation_state_.swap(last_translation_state);
            trajectory_processing::reverseTrajectory(approach_traj);
            trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), approach_traj.joint_trajectory);
            trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), translation_traj.joint_trajectory);

            const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
            if (jmg)
            {
              const std::vector<moveit_msgs::JointLimits> &jlim = jmg->getVariableLimits();
              time_param_.computeTimeStamps(approach_traj.joint_trajectory, jlim);
              time_param_.computeTimeStamps(translation_traj.joint_trajectory, jlim);
            }

            plan->trajectories_.push_back(approach_traj);
            plan->trajectory_descriptions_.push_back("approach");

            addGraspTrajectory(plan, plan->grasp_.grasp_posture, "grasp");

            plan->trajectories_.push_back(translation_traj);
            plan->trajectory_descriptions_.push_back("translation");

            return true;
          }
        }
        else
        {
          addGraspTrajectory(plan, plan->grasp_.pre_grasp_posture, "pre_grasp");

          plan->approach_state_.swap(first_approach_state);
          trajectory_processing::reverseTrajectory(approach_traj);
          trajectory_processing::unwindJointTrajectory(pre_grasp_planning_scene_->getKinematicModel(), approach_traj.joint_trajectory);

          const kinematic_model::JointModelGroup *jmg = pre_grasp_planning_scene_->getKinematicModel()->getJointModelGroup(plan->planning_group_);
          if (jmg)
            time_param_.computeTimeStamps(approach_traj.joint_trajectory, jmg->getVariableLimits());

          plan->trajectories_.push_back(approach_traj);
          plan->trajectory_descriptions_.push_back("approach");

          addGraspTrajectory(plan, plan->grasp_.grasp_posture, "grasp");

          return true;
        }
      }

    }
  }
  while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ && samplePossibleGoalStates(plan, pre_grasp_planning_scene_->getCurrentState(), min_distance, max_fail_));
  plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;

  return false;
}

}
<|endoftext|>


<commit_before>#include <assert.h>
#include <pthread.h>

#include "atomic.hh"
#include "locks.hh"
#include "threadtests.hh"

#define NUM_THREADS 50
#define NUM_TIMES 1000000

class Doodad : public RCValue {
public:
    Doodad() {
        numInstances++;
    }

    ~Doodad() {
        numInstances--;
    }

    static int getNumInstances() {
        return numInstances;
    }

private:
    static Atomic<int> numInstances;
};

Atomic<int> Doodad::numInstances(0);

class AtomicPtrTest : public Generator<bool> {
public:

    AtomicPtrTest(RCPtr<Doodad> p) : ptr(p) {}

    bool operator()() {
        for (int i = 0; i < NUM_TIMES; ++i) {
            switch (rand() % 4) {
            case 0:
                ptr.reset(new Doodad);
                break;
            case 1:
                {
                    RCPtr<Doodad> d(new Doodad);
                    ptr.reset(d);
                }
                break;
            case 2:
                {
                    RCPtr<Doodad> d(new Doodad);
                    d.reset();
                }
                break;
            case 3:
                {
                    RCPtr<Doodad> d(ptr);
                    d.reset();
                }
                break;
            default:
                assert(false);
            }
        }

        return true;
    }

private:
    RCPtr<Doodad> ptr;
};

static void testAtomicPtr() {
    // Just do a bunch.
    RCPtr<Doodad> dd;
    AtomicPtrTest *testGen = new AtomicPtrTest(dd);

    getCompletedThreads<bool>(NUM_THREADS, testGen);

    delete testGen;
    dd.reset();
    assert(Doodad::getNumInstances() == 0);
}

int main() {
    testAtomicPtr();
}
<commit_msg>Fix RCPtr test to share an RCPtr among the threads<commit_after>#include <assert.h>
#include <pthread.h>

#include "atomic.hh"
#include "locks.hh"
#include "threadtests.hh"

#define NUM_THREADS 50
#define NUM_TIMES 1000000

class Doodad : public RCValue {
public:
    Doodad() {
        numInstances++;
    }

    ~Doodad() {
        numInstances--;
    }

    static int getNumInstances() {
        return numInstances;
    }

private:
    static Atomic<int> numInstances;
};

Atomic<int> Doodad::numInstances(0);

class AtomicPtrTest : public Generator<bool> {
public:

    AtomicPtrTest(RCPtr<Doodad> *p) : ptr(p) {}

    bool operator()() {
        for (int i = 0; i < NUM_TIMES; ++i) {
            switch (rand() % 4) {
            case 0:
                ptr->reset(new Doodad);
                break;
            case 1:
                {
                    RCPtr<Doodad> d(new Doodad);
                    ptr->reset(d);
                }
                break;
            case 2:
                {
                    RCPtr<Doodad> d(new Doodad);
                    d.reset();
                }
                break;
            case 3:
                {
                    RCPtr<Doodad> d(*ptr);
                    d.reset();
                }
                break;
            default:
                assert(false);
            }
        }

        return true;
    }

private:
    RCPtr<Doodad> *ptr;
};

static void testAtomicPtr() {
    // Just do a bunch.
    RCPtr<Doodad> dd;
    AtomicPtrTest *testGen = new AtomicPtrTest(&dd);

    getCompletedThreads<bool>(NUM_THREADS, testGen);

    delete testGen;
    dd.reset();
    assert(Doodad::getNumInstances() == 0);
}

int main() {
    testAtomicPtr();
}
<|endoftext|>


<commit_before>#include <PageFrameAllocator.hh>
#include <Parameters.hh>
#include <Memory.hh>
#include <Multiboot.hh>
#include <Debug.hh>
#include <Templates.hh>
#include <Modules.hh>
#include <Memory.hh>
#include <Hal.hh>

#include <X86/ThreadContext.hh>

#include <cstring>

#undef DEBUG

#ifdef DEBUG
# define D(x) x
#else
# define D(x)
#endif

namespace
{
   uintptr_t heapEnd = HeapStart;
   uintptr_t stackEnd = StackStart;
   uintptr_t mapEnd = MapStart;

   MemorySegment memoryMap[MemoryMapMax];
   unsigned int memoryMapCount = 0;

   PageCluster usedPages;
   PageCluster freePages;

   spinlock_softirq_t pagesLock;
   spinlock_softirq_t memoryMapLock;
};

void
Memory::addMemoryMapEntry(uintptr_t start, size_t length)
{
   KASSERT(memoryMapCount < MemoryMapMax);

   memoryMap[memoryMapCount].address = start;
   memoryMap[memoryMapCount].size = length;
   memoryMapCount++;
}

void
Memory::init()
{
   printf("Listing Kernel sections:\n");
   printf(".text: %p-%p\n", &__start_text, &__end_text);
   printf(".data: %p-%p\n", &__start_data, &__end_data);
   printf(".rodata: %p-%p\n", &__start_rodata, &__end_rodata);
   printf(".bss: %p-%p\n", &__start_bss, &__end_bss);

   printf("Page size: %u, PhysicalPage structure size: %zu\n", PageSize, sizeof(PhysicalPage));

   spinlock_softirq_init(&pagesLock);
   spinlock_softirq_init(&memoryMapLock);

   usedPages.init();
   freePages.init();

   uintptr_t firstUsableMemory = max((uintptr_t )&__end_kernel, __end_modules);
   uintptr_t firstFreePage = roundTo<uintptr_t>(firstUsableMemory, PageSize) + PageSize * PageFrameCount;

   // init page clusters
   PhysicalPage* bootstrapPage = (PhysicalPage* )firstFreePage;
   bootstrapPage->init((uintptr_t )bootstrapPage - KernelVirtualBase);

   printf("Inserting bootstrap page to used pages cluster: %p\n", (void* )bootstrapPage->getAddress());
   usedPages.insert(bootstrapPage);

   unsigned int freeStructures = PageSize / sizeof (PhysicalPage) - 1;

   KASSERT(freeStructures > 0);
   printf("Free structures: %u\n", freeStructures);

   PhysicalPage* p = bootstrapPage + 1;
   for (unsigned int i = 0; i < memoryMapCount; i++)
   {
      printf("Adding memory range %p-%p (%zu)\n",
	     (void* )memoryMap[i].address,
	     (void* )(memoryMap[i].address + memoryMap[i].size),
	     memoryMap[i].size);

      for (uintptr_t addr = memoryMap[i].address;
	   addr < memoryMap[i].address + memoryMap[i].size;
	   addr += PageSize)
      {
	 if (freeStructures == 0)
	 {
	    D(printf("Allocating new page structure\n"));
	    //p = (PhysicalPage* )sbrk(PageSize)
	    uintptr_t ppage = getPage();
	    KASSERT(ppage != 0);
	    p = (PhysicalPage* )mapAnonymousPage(ppage); // XXX make a function for this
	    KASSERT(p != 0);
	    freeStructures = PageSize / sizeof (PhysicalPage);
	 }

	 p->init(addr);
	 if ((addr >= KernelLoadAddress) && (addr < firstFreePage - KernelVirtualBase))
	 {
	    D(printf("Found used page: %p\n", (void* )addr));
	    usedPages.insert(p);
	 }
	 else
	 {
	    D(printf("Inserting free page: %p\n", (void* )addr));
	    freePages.insert(p);
	 }

	 p++;
	 freeStructures--;
      }
   }

   uintptr_t start = stackEnd;
   printf("Creating initial kernel stack: %p-%p (%u)\n", (void* )start, (void* )(start - StackSize), StackSize);

   for (uintptr_t stackAddress = (start - StackSize);
	stackAddress < start;
	stackAddress += PageSize)
   {
      uintptr_t stackPage = getPage();
      KASSERT(stackPage != 0);

      bool success = mapPage(stackAddress, stackPage);
      KASSERT(success);
   }

   // add one unmapped page as guard
   stackEnd = start - StackSize - PageSize;

//   KASSERT(success);

//    printf("Freeing initial kernel stack: %p-%p\n", (void* )(initial_stack - InitialStackSize), (void* )initial_stack);
//    for (uintptr_t oldStack = initial_stack - InitialStackSize;
// 	oldStack < initial_stack;
// 	oldStack += PageSize)
//    {
//      unmap(oldStack);
//    }
}

// XXX curently this overlaps anon mappings, se we can only give one anon
// mapped page as the kernel stack area
#if 0
bool
Memory::createKernelStack(uintptr_t& start)
{
   start = stackEnd;

   printf("Creating new kernel stack: %p-%p (%u)\n", (void* )start, (void* )(start - StackSize), StackSize);

   for (uintptr_t stackAddress = (start - StackSize);
	stackAddress < start;
	stackAddress += PageSize)
   {
      uintptr_t stackPage = getPage();
      KASSERT(stackPage != 0);

      bool success = mapPage(stackAddress, stackPage);
      KASSERT(success);
   }

   // add one unmapped page as guard
   stackEnd = start - StackSize - PageSize;

   return true;
}
#else
bool
Memory::createKernelStack(uintptr_t& top)
{
   uintptr_t stackPage = getPage();
   KASSERT(stackPage != 0);

   uintptr_t bottom = mapAnonymousPage(stackPage);
   KASSERT(bottom != 0);

   top = bottom + PageSize;

   printf("Creating new kernel stack: %p-%p (%u)\n", (void* )top, (void* )(bottom), PageSize);

//   uintptr_t newStack = Hal::initKernelStack(top, &Kernel::Thread::main, );

//   top = newStack;

   return true;
}
#endif

// anonymous mapping of a physical page
//
uintptr_t
Memory::mapAnonymousPage(uintptr_t phys, int flags)
{
   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= PageSize;
   uintptr_t virt = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   if (virt <= (uintptr_t)&__end_kernel)
//   if (virt <= heapEnd)
   {
      printf("%p <= %p\n", (void*)virt, (void*)heapEnd);
      Debug::panic("Memory::mapPage: Kernel map namespace exhausted.");
   }

//   Debug::info("Mapping anonymous page: %p to %p\n", phys, mapEnd);
   bool success = mapPage(virt, phys, flags);
   if (!success)
   {
      return 0;
   }

   return virt;
}

// anonymous memory mapping
//
uintptr_t Memory::mapAnonymousRegion(size_t size, int flags)
{
   size_t rsize = roundTo<uintptr_t>(size, PageSize);

   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= rsize;
   uintptr_t vaddr = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   if (vaddr <= (uintptr_t)&__end_kernel)
//   if (vaddr <= heapEnd)
   {
      printf("%p <= %p\n", (void*)vaddr, (void*)heapEnd);
      Debug::panic("Memory::mapAnonymousRegion: Kernel map name space exhausted.");
   }

   for (size_t page = 0; page < rsize; page += PageSize)
   {
      uintptr_t emptyPage = getPage();
      KASSERT(emptyPage != 0);
      bool success = mapPage(vaddr + page, emptyPage, flags);
      if (!success)
      {
         Debug::panic("mapRegion unsuccesful.");
         // XXX unmap and return error
         //return 0;
      }
   }

   return vaddr;
}

// anonymous mapping of a memory region
//
uintptr_t Memory::mapRegion(uintptr_t paddr, size_t size, int flags)
{
   uintptr_t firstPage = roundDown(paddr, PageSize);
   uintptr_t lastPage = roundTo(paddr + size, PageSize);
   size_t rsize = lastPage - firstPage;

   D(printf("Memory::mapRegion: %p-%p\n", (void*)paddr, (void*)(paddr + size)));
   D(printf("Memory::mapRegion: mapping: %p-%p (%zu)\n", (void*)firstPage, (void*)lastPage, rsize));

   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= rsize;
   uintptr_t vaddr = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   uintptr_t offset = paddr - firstPage;

   if (vaddr <= (uintptr_t)&__end_kernel)
//   if (vaddr <= heapEnd)
   {
      printf("%p <= %p\n", (void*)vaddr, (void*)heapEnd);
      Debug::panic("Memory::mapRegion: Kernel map namespace exhausted.");
   }

   for (uintptr_t page = firstPage; page != lastPage; page += PageSize, vaddr += PageSize)
   {
      bool success = mapPage(vaddr, page, flags);
      if (!success)
      {
	 Debug::panic("mapRegion unsuccesful.");
	 // XXX unmap and return error
	 //return 0;
      }
   }

   return mapEnd + offset;
}

bool
Memory::unmapRegion(uintptr_t paddr, std::size_t size)
{
   uintptr_t firstPage = roundDown(paddr, PageSize);
   uintptr_t lastPage = roundTo(paddr + size, PageSize);

   for (uintptr_t page = firstPage; page != lastPage; page += PageSize)
   {
      bool success = unmapPage(page);
      if (!success)
      {
	 Debug::panic("unmapRegion unsuccesful.");
      }
   }

   return true;
}

// get a free physical page
//
uintptr_t
Memory::getPage()
{
   D(printf("Memory::getPage\n"));

   uintptr_t address = 0;

   spinlock_softirq_enter(&pagesLock);

   PhysicalPage* page = freePages.get();
   if (page != 0)
   {
      usedPages.insert(page);
      address = page->getAddress();
   }

   spinlock_softirq_exit(&pagesLock);

   D(printf("Memory::getPage done: %p\n", (void*)address));

   return address;
}

// free a physical page
//
void
Memory::putPage(uintptr_t address)
{
   D(printf("Memory::putPage: %p\n", (void*)address));

   spinlock_softirq_enter(&pagesLock);

   PhysicalPage* page = usedPages.find(address);

   if (page == 0)
   {
      spinlock_softirq_exit(&pagesLock);
      Debug::panic("Trying to free an unallocated physical page: %p\n", (void* )address);
   }

   usedPages.remove(page);
   freePages.insert(page);

   spinlock_softirq_exit(&pagesLock);
}

void*
Memory::readPhysicalMemory(void* destination, const void* source, std::size_t size)
{
   uintptr_t mapFirst = roundDown(uintptr_t(source), PageSize);
   uintptr_t mapLast = roundTo(uintptr_t(source), PageSize);

   uintptr_t mapped = mapRegion(mapFirst, mapLast - mapFirst);

   std::memcpy(destination, reinterpret_cast<void*>(mapped + (uintptr_t(source) - mapFirst)), size);

   unmapRegion(mapped, mapLast - mapFirst);

   return destination;
}
<commit_msg>Fix reading ranges larger than a page size<commit_after>#include <PageFrameAllocator.hh>
#include <Parameters.hh>
#include <Memory.hh>
#include <Multiboot.hh>
#include <Debug.hh>
#include <Templates.hh>
#include <Modules.hh>
#include <Memory.hh>
#include <Hal.hh>

#include <X86/ThreadContext.hh>

#include <cstring>

#undef DEBUG

#ifdef DEBUG
# define D(x) x
#else
# define D(x)
#endif

namespace
{
   uintptr_t heapEnd = HeapStart;
   uintptr_t stackEnd = StackStart;
   uintptr_t mapEnd = MapStart;

   MemorySegment memoryMap[MemoryMapMax];
   unsigned int memoryMapCount = 0;

   PageCluster usedPages;
   PageCluster freePages;

   spinlock_softirq_t pagesLock;
   spinlock_softirq_t memoryMapLock;
};

void
Memory::addMemoryMapEntry(uintptr_t start, size_t length)
{
   KASSERT(memoryMapCount < MemoryMapMax);

   memoryMap[memoryMapCount].address = start;
   memoryMap[memoryMapCount].size = length;
   memoryMapCount++;
}

void
Memory::init()
{
   printf("Listing Kernel sections:\n");
   printf(".text: %p-%p\n", &__start_text, &__end_text);
   printf(".data: %p-%p\n", &__start_data, &__end_data);
   printf(".rodata: %p-%p\n", &__start_rodata, &__end_rodata);
   printf(".bss: %p-%p\n", &__start_bss, &__end_bss);

   printf("Page size: %u, PhysicalPage structure size: %zu\n", PageSize, sizeof(PhysicalPage));

   spinlock_softirq_init(&pagesLock);
   spinlock_softirq_init(&memoryMapLock);

   usedPages.init();
   freePages.init();

   uintptr_t firstUsableMemory = max((uintptr_t )&__end_kernel, __end_modules);
   uintptr_t firstFreePage = roundTo<uintptr_t>(firstUsableMemory, PageSize) + PageSize * PageFrameCount;

   // init page clusters
   PhysicalPage* bootstrapPage = (PhysicalPage* )firstFreePage;
   bootstrapPage->init((uintptr_t )bootstrapPage - KernelVirtualBase);

   printf("Inserting bootstrap page to used pages cluster: %p\n", (void* )bootstrapPage->getAddress());
   usedPages.insert(bootstrapPage);

   unsigned int freeStructures = PageSize / sizeof (PhysicalPage) - 1;

   KASSERT(freeStructures > 0);
   printf("Free structures: %u\n", freeStructures);

   PhysicalPage* p = bootstrapPage + 1;
   for (unsigned int i = 0; i < memoryMapCount; i++)
   {
      printf("Adding memory range %p-%p (%zu)\n",
	     (void* )memoryMap[i].address,
	     (void* )(memoryMap[i].address + memoryMap[i].size),
	     memoryMap[i].size);

      for (uintptr_t addr = memoryMap[i].address;
	   addr < memoryMap[i].address + memoryMap[i].size;
	   addr += PageSize)
      {
	 if (freeStructures == 0)
	 {
	    D(printf("Allocating new page structure\n"));
	    //p = (PhysicalPage* )sbrk(PageSize)
	    uintptr_t ppage = getPage();
	    KASSERT(ppage != 0);
	    p = (PhysicalPage* )mapAnonymousPage(ppage); // XXX make a function for this
	    KASSERT(p != 0);
	    freeStructures = PageSize / sizeof (PhysicalPage);
	 }

	 p->init(addr);
	 if ((addr >= KernelLoadAddress) && (addr < firstFreePage - KernelVirtualBase))
	 {
	    D(printf("Found used page: %p\n", (void* )addr));
	    usedPages.insert(p);
	 }
	 else
	 {
	    D(printf("Inserting free page: %p\n", (void* )addr));
	    freePages.insert(p);
	 }

	 p++;
	 freeStructures--;
      }
   }

   uintptr_t start = stackEnd;
   printf("Creating initial kernel stack: %p-%p (%u)\n", (void* )start, (void* )(start - StackSize), StackSize);

   for (uintptr_t stackAddress = (start - StackSize);
	stackAddress < start;
	stackAddress += PageSize)
   {
      uintptr_t stackPage = getPage();
      KASSERT(stackPage != 0);

      bool success = mapPage(stackAddress, stackPage);
      KASSERT(success);
   }

   // add one unmapped page as guard
   stackEnd = start - StackSize - PageSize;

//   KASSERT(success);

//    printf("Freeing initial kernel stack: %p-%p\n", (void* )(initial_stack - InitialStackSize), (void* )initial_stack);
//    for (uintptr_t oldStack = initial_stack - InitialStackSize;
// 	oldStack < initial_stack;
// 	oldStack += PageSize)
//    {
//      unmap(oldStack);
//    }
}

// XXX curently this overlaps anon mappings, se we can only give one anon
// mapped page as the kernel stack area
#if 0
bool
Memory::createKernelStack(uintptr_t& start)
{
   start = stackEnd;

   printf("Creating new kernel stack: %p-%p (%u)\n", (void* )start, (void* )(start - StackSize), StackSize);

   for (uintptr_t stackAddress = (start - StackSize);
	stackAddress < start;
	stackAddress += PageSize)
   {
      uintptr_t stackPage = getPage();
      KASSERT(stackPage != 0);

      bool success = mapPage(stackAddress, stackPage);
      KASSERT(success);
   }

   // add one unmapped page as guard
   stackEnd = start - StackSize - PageSize;

   return true;
}
#else
bool
Memory::createKernelStack(uintptr_t& top)
{
   uintptr_t stackPage = getPage();
   KASSERT(stackPage != 0);

   uintptr_t bottom = mapAnonymousPage(stackPage);
   KASSERT(bottom != 0);

   top = bottom + PageSize;

   printf("Creating new kernel stack: %p-%p (%u)\n", (void* )top, (void* )(bottom), PageSize);

//   uintptr_t newStack = Hal::initKernelStack(top, &Kernel::Thread::main, );

//   top = newStack;

   return true;
}
#endif

// anonymous mapping of a physical page
//
uintptr_t
Memory::mapAnonymousPage(uintptr_t phys, int flags)
{
   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= PageSize;
   uintptr_t virt = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   if (virt <= (uintptr_t)&__end_kernel)
//   if (virt <= heapEnd)
   {
      printf("%p <= %p\n", (void*)virt, (void*)heapEnd);
      Debug::panic("Memory::mapPage: Kernel map namespace exhausted.");
   }

//   Debug::info("Mapping anonymous page: %p to %p\n", phys, mapEnd);
   bool success = mapPage(virt, phys, flags);
   if (!success)
   {
      return 0;
   }

   return virt;
}

// anonymous memory mapping
//
uintptr_t Memory::mapAnonymousRegion(size_t size, int flags)
{
   size_t rsize = roundTo<uintptr_t>(size, PageSize);

   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= rsize;
   uintptr_t vaddr = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   if (vaddr <= (uintptr_t)&__end_kernel)
//   if (vaddr <= heapEnd)
   {
      printf("%p <= %p\n", (void*)vaddr, (void*)heapEnd);
      Debug::panic("Memory::mapAnonymousRegion: Kernel map name space exhausted.");
   }

   for (size_t page = 0; page < rsize; page += PageSize)
   {
      uintptr_t emptyPage = getPage();
      KASSERT(emptyPage != 0);
      bool success = mapPage(vaddr + page, emptyPage, flags);
      if (!success)
      {
         Debug::panic("mapRegion unsuccesful.");
         // XXX unmap and return error
         //return 0;
      }
   }

   return vaddr;
}

// anonymous mapping of a memory region
//
uintptr_t Memory::mapRegion(uintptr_t paddr, size_t size, int flags)
{
   uintptr_t firstPage = roundDown(paddr, PageSize);
   uintptr_t lastPage = roundTo(paddr + size, PageSize);
   size_t rsize = lastPage - firstPage;

   D(printf("Memory::mapRegion: %p-%p\n", (void*)paddr, (void*)(paddr + size)));
   D(printf("Memory::mapRegion: mapping: %p-%p (%zu)\n", (void*)firstPage, (void*)lastPage, rsize));

   spinlock_softirq_enter(&memoryMapLock);

   mapEnd -= rsize;
   uintptr_t vaddr = mapEnd;

   spinlock_softirq_exit(&memoryMapLock);

   uintptr_t offset = paddr - firstPage;

   if (vaddr <= (uintptr_t)&__end_kernel)
//   if (vaddr <= heapEnd)
   {
      printf("%p <= %p\n", (void*)vaddr, (void*)heapEnd);
      Debug::panic("Memory::mapRegion: Kernel map namespace exhausted.");
   }

   for (uintptr_t page = firstPage; page != lastPage; page += PageSize, vaddr += PageSize)
   {
      bool success = mapPage(vaddr, page, flags);
      if (!success)
      {
	 Debug::panic("mapRegion unsuccesful.");
	 // XXX unmap and return error
	 //return 0;
      }
   }

   return mapEnd + offset;
}

bool
Memory::unmapRegion(uintptr_t paddr, std::size_t size)
{
   uintptr_t firstPage = roundDown(paddr, PageSize);
   uintptr_t lastPage = roundTo(paddr + size, PageSize);

   for (uintptr_t page = firstPage; page != lastPage; page += PageSize)
   {
      bool success = unmapPage(page);
      if (!success)
      {
	 Debug::panic("unmapRegion unsuccesful.");
      }
   }

   return true;
}

// get a free physical page
//
uintptr_t
Memory::getPage()
{
   D(printf("Memory::getPage\n"));

   uintptr_t address = 0;

   spinlock_softirq_enter(&pagesLock);

   PhysicalPage* page = freePages.get();
   if (page != 0)
   {
      usedPages.insert(page);
      address = page->getAddress();
   }

   spinlock_softirq_exit(&pagesLock);

   D(printf("Memory::getPage done: %p\n", (void*)address));

   return address;
}

// free a physical page
//
void
Memory::putPage(uintptr_t address)
{
   D(printf("Memory::putPage: %p\n", (void*)address));

   spinlock_softirq_enter(&pagesLock);

   PhysicalPage* page = usedPages.find(address);

   if (page == 0)
   {
      spinlock_softirq_exit(&pagesLock);
      Debug::panic("Trying to free an unallocated physical page: %p\n", (void* )address);
   }

   usedPages.remove(page);
   freePages.insert(page);

   spinlock_softirq_exit(&pagesLock);
}

void*
Memory::readPhysicalMemory(void* destination, const void* source, std::size_t size)
{
   uintptr_t mapFirst = roundDown(uintptr_t(source), PageSize);
   uintptr_t mapLast = roundTo(uintptr_t(source) + size, PageSize);

   uintptr_t mapped = mapRegion(mapFirst, mapLast - mapFirst);

   std::memcpy(destination, reinterpret_cast<void*>(mapped + (uintptr_t(source) - mapFirst)), size);

   unmapRegion(mapped, mapLast - mapFirst);

   return destination;
}
<|endoftext|>


<commit_before>// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// 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 <gtest/gtest.h>

#include <string>

#include "rclcpp/rate.hpp"

/*
   Tests that funcion_traits calculates arity of several functors.
 */
TEST(TestRate, rate_basics) {
  auto period = std::chrono::milliseconds(10);
  auto delta = std::chrono::milliseconds(1);

  auto start = std::chrono::system_clock::now();
  rclcpp::rate::Rate r(period);
  ASSERT_FALSE(r.is_steady());
  r.sleep();
  auto one = std::chrono::system_clock::now();
  ASSERT_TRUE(period - delta < one - start);
  ASSERT_TRUE(period + delta > one - start);

  rclcpp::utilities::sleep_for(delta * 4);
  r.sleep();
  auto two = std::chrono::system_clock::now();

  ASSERT_TRUE(period - delta < two - one);
  ASSERT_TRUE(period + delta > two - one);

  rclcpp::utilities::sleep_for(delta * 4);
  r.reset();
  r.sleep();
  auto three = std::chrono::system_clock::now();
  ASSERT_TRUE(period + 3 * delta < three - two);
  ASSERT_TRUE(period + 5 * delta > three - two);
}

TEST(TestRate, wallrate_basics) {
  auto period = std::chrono::milliseconds(10);
  auto delta = std::chrono::milliseconds(1);

  auto start = std::chrono::system_clock::now();
  rclcpp::rate::WallRate r(period);
  ASSERT_TRUE(r.is_steady());
  r.sleep();
  auto one = std::chrono::system_clock::now();
  ASSERT_TRUE(period - delta < one - start);
  ASSERT_TRUE(period + delta > one - start);

  rclcpp::utilities::sleep_for(delta * 4);
  r.sleep();
  auto two = std::chrono::system_clock::now();

  ASSERT_TRUE(period - delta < two - one);
  ASSERT_TRUE(period + delta > two - one);

  rclcpp::utilities::sleep_for(delta * 4);
  r.reset();
  r.sleep();
  auto three = std::chrono::system_clock::now();
  ASSERT_TRUE(period + 3 * delta < three - two);
  ASSERT_TRUE(period + 5 * delta > three - two);
}
<commit_msg>softening tolerances on timing to pass on osx<commit_after>// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// 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 <gtest/gtest.h>

#include <string>

#include "rclcpp/rate.hpp"

/*
   Tests that funcion_traits calculates arity of several functors.
 */
TEST(TestRate, rate_basics) {
  auto period = std::chrono::milliseconds(100);
  auto delta = std::chrono::milliseconds(10);

  auto start = std::chrono::system_clock::now();
  rclcpp::rate::Rate r(period);
  ASSERT_FALSE(r.is_steady());
  r.sleep();
  auto one = std::chrono::system_clock::now();
  ASSERT_TRUE(period - delta < one - start);
  ASSERT_TRUE(period + delta > one - start);

  rclcpp::utilities::sleep_for(delta * 4);
  r.sleep();
  auto two = std::chrono::system_clock::now();

  ASSERT_TRUE(period - delta < two - one);
  ASSERT_TRUE(period + delta > two - one);

  rclcpp::utilities::sleep_for(delta * 4);
  r.reset();
  r.sleep();
  auto three = std::chrono::system_clock::now();
  ASSERT_TRUE(period + 3 * delta < three - two);
  ASSERT_TRUE(period + 7 * delta > three - two);
}

TEST(TestRate, wallrate_basics) {
  auto period = std::chrono::milliseconds(100);
  auto delta = std::chrono::milliseconds(10);

  auto start = std::chrono::system_clock::now();
  rclcpp::rate::WallRate r(period);
  ASSERT_TRUE(r.is_steady());
  r.sleep();
  auto one = std::chrono::system_clock::now();
  ASSERT_TRUE(period - delta < one - start);
  ASSERT_TRUE(period + delta > one - start);

  rclcpp::utilities::sleep_for(delta * 4);
  r.sleep();
  auto two = std::chrono::system_clock::now();

  ASSERT_TRUE(period - delta < two - one);
  ASSERT_TRUE(period + delta > two - one);

  rclcpp::utilities::sleep_for(delta * 4);
  r.reset();
  r.sleep();
  auto three = std::chrono::system_clock::now();
  ASSERT_TRUE(period + 3 * delta < three - two);
  ASSERT_TRUE(period + 7 * delta > three - two);
}
<|endoftext|>