<commit_before>#include "asocket.h"

// C++
#include <cassert>
#include <cstring>
#include <cstdint>

// STL
#include <iostream>

// PDTK
#include <cxxutils/error_helpers.h>
#include <cxxutils/streamcolors.h>

#ifndef CMSG_LEN
#define CMSG_ALIGN(len) (((len) + sizeof(size_t) - 1) & (size_t) ~ (sizeof(size_t) - 1))
#define CMSG_SPACE(len) (CMSG_ALIGN (len) + CMSG_ALIGN (sizeof (struct cmsghdr)))
#define CMSG_LEN(len)   (CMSG_ALIGN (sizeof (struct cmsghdr)) + (len))
#endif

static_assert(sizeof(uint8_t) == sizeof(char), "size mismatch!");

// NOTE: this is not an enumeration so that each value can be referenced
namespace LocalCommand
{
  static const uint64_t write  = 1;
  static const uint64_t update = 2;
  static const uint64_t exit   = 3;
}

namespace posix
{
  inline bool getpeercred(fd_t sockfd, proccred_t& cred) noexcept
    { return ::getpeercred(sockfd, cred) == posix::success_response; }
}

AsyncSocket::AsyncSocket(EDomain domain, EType type, EProtocol protocol, int flags) noexcept
  : AsyncSocket(posix::socket(domain, type, protocol, flags)) { }

AsyncSocket::AsyncSocket(posix::fd_t socket) noexcept
  : m_socket(socket),
    m_bound(false)
{
  posix::fd_t cmdio[2];
  assert(::pipe(cmdio) != posix::error_response);
  m_read_command  = cmdio[0];
  m_write_command = cmdio[1];

  EventBackend::watch(m_read_command, EventFlags::Readable);
  EventBackend::watch(m_socket, EventFlags::Readable);
}

AsyncSocket::~AsyncSocket(void) noexcept
{
  posix::write(m_write_command, &LocalCommand::exit, sizeof(uint64_t));
  m_iothread.join(); // wait for thread to exit

  if(m_socket != posix::invalid_descriptor)
    posix::close(m_socket);
  if(m_selfaddr != EDomain::unspec)
    ::unlink(m_selfaddr.sun_path);
}

bool AsyncSocket::bind(const char *socket_path, int socket_backlog) noexcept
{
  if(!has_socket())
    return false;

  assert(std::strlen(socket_path) < sizeof(sockaddr_un::sun_path));
  m_selfaddr = socket_path;
  m_selfaddr = EDomain::local;
  return m_bound = posix::bind(m_socket, m_selfaddr, m_selfaddr.size()) &&
                   posix::listen(m_socket, socket_backlog) &&
                   async_spawn();
}

bool AsyncSocket::connect(const char *socket_path) noexcept
{
  if(!has_socket())
    return false;

  assert(std::strlen(socket_path) < sizeof(sockaddr_un::sun_path));
  posix::sockaddr_t peeraddr;
  proccred_t peercred;

  peeraddr = socket_path;
  peeraddr = EDomain::local;
  m_selfaddr = EDomain::unspec;

  return posix::connect(m_socket, peeraddr, peeraddr.size()) &&
         posix::getpeercred(m_socket, peercred) &&
         async_spawn() &&
         Object::enqueue(connectedToPeer, m_socket, peeraddr, peercred);
}

bool AsyncSocket::async_spawn(void) noexcept
{
  if(m_iothread.joinable()) // if thread already active
    return posix::write(m_write_command, &LocalCommand::update, sizeof(uint64_t)) != posix::error_response; // notify thread of an update

  m_iothread = std::thread(&AsyncSocket::async_io , this); // create a new thread
  return m_iothread.joinable(); // if thread creation succeeded
}

void AsyncSocket::async_io(void) noexcept // runs as it's own thread
{
  msghdr msg = {};
  iovec iov = {};
  char aux_buffer[CMSG_SPACE(sizeof(int))] = { 0 };
  uint64_t command_buffer = 0;
  ssize_t byte_count = 0;

  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = aux_buffer;

  for(;;)
  {
    if(EventBackend::getevents())
    {
      for(const auto& pos : EventBackend::results)
      {
        if(pos.first == m_read_command && (pos.second.flags & EventFlags::Readable))
        {
          command_buffer = 0;
          byte_count = posix::read(pos.first, &command_buffer, sizeof(uint64_t));
          if(byte_count == posix::error_response)
            continue;

          switch(command_buffer)
          {
            case LocalCommand::update: continue; // EventBackend::m_queue size changed
            case LocalCommand::exit:
              std::cout << "thread exiting!" << std::endl << std::flush;
              return; // thread exit
            case LocalCommand::write: // m_messages isn't empty
            {
              for(auto& msg_pos : m_messages)
              {
                msg.msg_iov = &iov;
                msg.msg_iovlen = 1;
                msg.msg_control = aux_buffer;

                iov.iov_base = msg_pos.buffer.begin();
                iov.iov_len = msg_pos.buffer.size();

                msg.msg_controllen = 0;

                if(msg_pos.fd_buffer != posix::invalid_descriptor) // if a file descriptor needs to be sent
                {
                  msg.msg_controllen = CMSG_SPACE(sizeof(int));
                  cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
                  cmsg->cmsg_level = SOL_SOCKET;
                  cmsg->cmsg_type = SCM_RIGHTS;
                  cmsg->cmsg_len = CMSG_LEN(sizeof(int));
                  *reinterpret_cast<int*>(CMSG_DATA(cmsg)) = msg_pos.fd_buffer;
                }

                byte_count = posix::sendmsg(msg_pos.socket, &msg, 0);

                if(byte_count == posix::error_response) // error
                  std::cout << std::flush << std::endl << std::red << "sendmsg error: " << std::strerror(errno) << std::none << std::endl << std::flush;
                else
                  Object::enqueue(writeFinished, msg_pos.socket, byte_count);
              }
              m_messages.clear();
            }
          }
        }
        else if(m_bound && // if in server mode
                pos.first == m_socket && // and this is a primary socket
                pos.second.flags & EventFlags::Readable) // and it's a read event
        {
          proccred_t peercred;
          posix::sockaddr_t peeraddr;
          socklen_t addrlen = 0;
          posix::fd_t fd = posix::accept(m_socket, peeraddr, &addrlen); // accept a new socket connection
          if(fd == posix::error_response)
            std::cout << "accept error: " << std::strerror(errno) << std::endl << std::flush;
          else
          {
            if(!EventBackend::watch(fd, EventFlags::Readable)) // monitor new socket connection
              std::cout << "watch failure: " << std::strerror(errno) << std::endl << std::flush;
            if(!posix::getpeercred(fd, peercred)) // get creditials of connected peer process
              std::cout << "peercred failure: " << std::strerror(errno) << std::endl << std::flush;
            Object::enqueue(connectedToPeer, fd, peeraddr, peercred);
          }
        }
        else
        {
          if(pos.second.flags & (EventFlags::Disconnected | EventFlags::Error)) // connection severed or connection error
          {
            EventBackend::remove(pos.first); // stop watching for events
            posix::close(pos.first); // close connection if open
          }
          else if(pos.second.flags & EventFlags::Readable) // read
          {
            m_incomming.socket = m_bound ? m_socket : m_read_command; // select proper output socket
            m_incomming.buffer.allocate(); // allocate 64KB buffer

            iov.iov_base = m_incomming.buffer.data();
            iov.iov_len = m_incomming.buffer.capacity();
            msg.msg_controllen = sizeof(aux_buffer);

            byte_count = posix::recvmsg(pos.first, &msg, 0);

            if(byte_count == posix::error_response)
              std::cout << std::red << "recvmsg error: " << std::strerror(errno) << std::none << std::endl << std::flush;
            else if(!byte_count)
            {
              EventBackend::remove(pos.first); // stop watching for events
              posix::close(pos.first); // connection severed!
            }
            else if(m_incomming.buffer.expand(byte_count))
            {
              m_incomming.fd_buffer = posix::invalid_descriptor;
              if(msg.msg_controllen == CMSG_SPACE(sizeof(int)))
              {
                cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
                if(cmsg->cmsg_level == SOL_SOCKET &&
                   cmsg->cmsg_type == SCM_RIGHTS &&
                   cmsg->cmsg_len == CMSG_LEN(sizeof(int)))
                 m_incomming.fd_buffer = *reinterpret_cast<int*>(CMSG_DATA(cmsg));
              }
              else if(msg.msg_flags)
                std::cout << std::red << "error, message flags: " << std::hex << msg.msg_flags << std::dec << std::none << std::endl << std::flush;
              Object::enqueue(readFinished, m_incomming.socket, m_incomming);
            }
          }
          else if(pos.second.flags >= 0)
          {
            std::cout << "unknown event at: " << pos.first << " value: " << std::hex << uint32_t(EventFlags(pos.second.flags)) << std::dec << std::endl << std::flush;
          }
        }
      }
    }
  }
}

bool AsyncSocket::write(message_t msg) noexcept
{
  if(!is_connected(msg.socket))
    return false;
  m_messages.push_back(msg);
  posix::write(m_write_command, &LocalCommand::write, sizeof(uint64_t));
  return true;
}
<commit_msg>work in progress<commit_after>#include "asocket.h"

// C++
#include <cassert>
#include <cstring>
#include <cstdint>

// STL
#include <iostream>

// PDTK
#include <cxxutils/error_helpers.h>
#include <cxxutils/streamcolors.h>

#ifndef CMSG_LEN
#define CMSG_ALIGN(len) (((len) + sizeof(size_t) - 1) & (size_t) ~ (sizeof(size_t) - 1))
#define CMSG_SPACE(len) (CMSG_ALIGN (len) + CMSG_ALIGN (sizeof (struct cmsghdr)))
#define CMSG_LEN(len)   (CMSG_ALIGN (sizeof (struct cmsghdr)) + (len))
#endif

static_assert(sizeof(uint8_t) == sizeof(char), "size mismatch!");

// NOTE: this is not an enumeration so that each value can be referenced
namespace LocalCommand
{
  static const uint64_t write  = 1;
  static const uint64_t update = 2;
  static const uint64_t exit   = 3;
}

namespace posix
{
  inline bool getpeercred(fd_t sockfd, proccred_t& cred) noexcept
    { return ::getpeercred(sockfd, cred) == posix::success_response; }
}

AsyncSocket::AsyncSocket(EDomain domain, EType type, EProtocol protocol, int flags) noexcept
  : AsyncSocket(posix::socket(domain, type, protocol, flags)) { }

AsyncSocket::AsyncSocket(posix::fd_t socket) noexcept
  : m_socket(socket),
    m_bound(false)
{
  posix::fd_t cmdio[2];
  assert(::pipe(cmdio) != posix::error_response);
  m_read_command  = cmdio[0];
  m_write_command = cmdio[1];

  EventBackend::watch(m_read_command, EventFlags::Readable);
  EventBackend::watch(m_socket, EventFlags::Readable);
}

AsyncSocket::~AsyncSocket(void) noexcept
{
  posix::write(m_write_command, &LocalCommand::exit, sizeof(uint64_t));
  m_iothread.join(); // wait for thread to exit

  if(m_socket != posix::invalid_descriptor)
    posix::close(m_socket);
  if(m_selfaddr != EDomain::unspec)
    ::unlink(m_selfaddr.sun_path);
}

bool AsyncSocket::bind(const char *socket_path, int socket_backlog) noexcept
{
  if(!has_socket())
    return false;

  assert(std::strlen(socket_path) < sizeof(sockaddr_un::sun_path));
  m_selfaddr = socket_path;
  m_selfaddr = EDomain::local;
  return m_bound = posix::bind(m_socket, m_selfaddr, m_selfaddr.size()) &&
                   posix::listen(m_socket, socket_backlog) &&
                   async_spawn();
}

bool AsyncSocket::connect(const char *socket_path) noexcept
{
  if(!has_socket())
    return false;

  assert(std::strlen(socket_path) < sizeof(sockaddr_un::sun_path));
  posix::sockaddr_t peeraddr;
  proccred_t peercred;

  peeraddr = socket_path;
  peeraddr = EDomain::local;
  m_selfaddr = EDomain::unspec;

  return posix::connect(m_socket, peeraddr, peeraddr.size()) &&
         posix::getpeercred(m_socket, peercred) &&
         async_spawn() &&
         Object::enqueue(connectedToPeer, m_socket, peeraddr, peercred);
}

bool AsyncSocket::async_spawn(void) noexcept
{
  if(m_iothread.joinable()) // if thread already active
    return posix::write(m_write_command, &LocalCommand::update, sizeof(uint64_t)) != posix::error_response; // notify thread of an update

  m_iothread = std::thread(&AsyncSocket::async_io , this); // create a new thread
  return m_iothread.joinable(); // if thread creation succeeded
}

void AsyncSocket::async_io(void) noexcept // runs as it's own thread
{
  msghdr msg = {};
  iovec iov = {};
  char aux_buffer[CMSG_SPACE(sizeof(int))] = { 0 };
  uint64_t command_buffer = 0;
  ssize_t byte_count = 0;

  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = aux_buffer;

  for(;;)
  {
    if(EventBackend::getevents())
    {
      for(const auto& pos : EventBackend::results)
      {
        if(pos.first == m_read_command && (pos.second.flags & EventFlags::Readable))
        {
          command_buffer = 0;
          byte_count = posix::read(pos.first, &command_buffer, sizeof(uint64_t));
          if(byte_count == posix::error_response)
            continue;

          switch(command_buffer)
          {
            case LocalCommand::update: continue; // EventBackend::m_queue size changed
            case LocalCommand::exit:
              std::cout << "thread exiting!" << std::endl << std::flush;
              return; // thread exit
            case LocalCommand::write: // m_messages isn't empty
            {
              for(auto& msg_pos : m_messages)
              {
                msg.msg_iov = &iov;
                msg.msg_iovlen = 1;
                msg.msg_control = aux_buffer;

                iov.iov_base = msg_pos.buffer.begin();
                iov.iov_len = msg_pos.buffer.size();

                msg.msg_controllen = 0;

                if(msg_pos.fd_buffer != posix::invalid_descriptor) // if a file descriptor needs to be sent
                {
                  msg.msg_controllen = CMSG_SPACE(sizeof(int));
                  cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
                  cmsg->cmsg_level = SOL_SOCKET;
                  cmsg->cmsg_type = SCM_RIGHTS;
                  cmsg->cmsg_len = CMSG_LEN(sizeof(int));
                  *reinterpret_cast<int*>(CMSG_DATA(cmsg)) = msg_pos.fd_buffer;
                }

                byte_count = posix::sendmsg(msg_pos.socket, &msg, 0);

                if(byte_count == posix::error_response) // error
                  std::cout << std::flush << std::endl << posix::fg::red << "sendmsg error: " << std::strerror(errno) << posix::fg::reset << std::endl << std::flush;
                else
                  Object::enqueue(writeFinished, msg_pos.socket, byte_count);
              }
              m_messages.clear();
            }
          }
        }
        else if(m_bound && // if in server mode
                pos.first == m_socket && // and this is a primary socket
                pos.second.flags & EventFlags::Readable) // and it's a read event
        {
          proccred_t peercred;
          posix::sockaddr_t peeraddr;
          socklen_t addrlen = 0;
          posix::fd_t fd = posix::accept(m_socket, peeraddr, &addrlen); // accept a new socket connection
          if(fd == posix::error_response)
            std::cout << "accept error: " << std::strerror(errno) << std::endl << std::flush;
          else
          {
            if(!EventBackend::watch(fd, EventFlags::Readable)) // monitor new socket connection
              std::cout << "watch failure: " << std::strerror(errno) << std::endl << std::flush;
            if(!posix::getpeercred(fd, peercred)) // get creditials of connected peer process
              std::cout << "peercred failure: " << std::strerror(errno) << std::endl << std::flush;
            Object::enqueue(connectedToPeer, fd, peeraddr, peercred);
          }
        }
        else
        {
          if(pos.second.flags & (EventFlags::Disconnected | EventFlags::Error)) // connection severed or connection error
          {
            EventBackend::remove(pos.first); // stop watching for events
            posix::close(pos.first); // close connection if open
          }
          else if(pos.second.flags & EventFlags::Readable) // read
          {
            m_incomming.socket = m_bound ? m_socket : m_read_command; // select proper output socket
            m_incomming.buffer.allocate(); // allocate 64KB buffer

            iov.iov_base = m_incomming.buffer.data();
            iov.iov_len = m_incomming.buffer.capacity();
            msg.msg_controllen = sizeof(aux_buffer);

            byte_count = posix::recvmsg(pos.first, &msg, 0);

            if(byte_count == posix::error_response)
              std::cout << posix::fg::red << "recvmsg error: " << std::strerror(errno) << posix::fg::reset << std::endl << std::flush;
            else if(!byte_count)
            {
              EventBackend::remove(pos.first); // stop watching for events
              posix::close(pos.first); // connection severed!
            }
            else if(m_incomming.buffer.expand(byte_count))
            {
              m_incomming.fd_buffer = posix::invalid_descriptor;
              if(msg.msg_controllen == CMSG_SPACE(sizeof(int)))
              {
                cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
                if(cmsg->cmsg_level == SOL_SOCKET &&
                   cmsg->cmsg_type == SCM_RIGHTS &&
                   cmsg->cmsg_len == CMSG_LEN(sizeof(int)))
                 m_incomming.fd_buffer = *reinterpret_cast<int*>(CMSG_DATA(cmsg));
              }
              else if(msg.msg_flags)
                std::cout << posix::fg::red << "error, message flags: " << std::hex << msg.msg_flags << std::dec << posix::fg::reset << std::endl << std::flush;
              Object::enqueue(readFinished, m_incomming.socket, m_incomming);
            }
          }
          else if(pos.second.flags >= 0)
          {
            std::cout << "unknown event at: " << pos.first << " value: " << std::hex << uint32_t(EventFlags(pos.second.flags)) << std::dec << std::endl << std::flush;
          }
        }
      }
    }
  }
}

bool AsyncSocket::write(message_t msg) noexcept
{
  if(!is_connected(msg.socket))
    return false;
  m_messages.push_back(msg);
  posix::write(m_write_command, &LocalCommand::write, sizeof(uint64_t));
  return true;
}

#if 0
// connect to primary socket
static bool accept(posix::fd_t socket, posix::fd_t& fd) noexcept
{
  proccred_t peercred;
  posix::sockaddr_t peeraddr;
  socklen_t addrlen = 0;
  fd = posix::accept(m_socket, peeraddr, &addrlen); // accept a new socket connection
  if(fd == posix::error_response)
    std::cout << "accept error: " << std::strerror(errno) << std::endl << std::flush;
  else
  {
    if(!EventBackend::watch(fd, EventFlags::Readable)) // monitor new socket connection
      std::cout << "watch failure: " << std::strerror(errno) << std::endl << std::flush;
    if(!posix::getpeercred(fd, peercred)) // get creditials of connected peer process
      std::cout << "peercred failure: " << std::strerror(errno) << std::endl << std::flush;
    Object::enqueue(connectedToPeer, fd, peeraddr, peercred);
  }
}

struct BufferedSocket
{
  bool read(void) noexcept
  {
    msghdr header = {};
    iovec iov = {};
    char aux_buffer[CMSG_SPACE(sizeof(int))] = { 0 };

    header.msg_iov = &iov;
    header.msg_iovlen = 1;
    header.msg_control = aux_buffer;

    iov.iov_base = buffer.begin();
    iov.iov_len = buffer.capacity();
    header.msg_controllen = sizeof(aux_buffer);

    ssize_t byte_count = posix::recvmsg(socket, &header, 0);
    flaw(byte_count > 0xFFFF, errno = EMSGSIZE, false, "Absurdly large socket message!: %i bytes", byte_count)

    if(byte_count == posix::error_response)
    {
      std::cout << posix::fg::red << "recvmsg error: " << std::strerror(errno) << posix::fg::reset << std::endl << std::flush;
      return false;
    }
    else if(!byte_count)
    {
      EventBackend::remove(socket); // stop watching for events
      posix::close(socket); // connection severed!
    }
    else if(buffer.resize(byte_count))
    {
      fd = posix::invalid_descriptor;
      if(header.msg_controllen == CMSG_SPACE(sizeof(int)))
      {
        cmsghdr* cmsg = CMSG_FIRSTHDR(&header);
        if(cmsg->cmsg_level == SOL_SOCKET &&
           cmsg->cmsg_type == SCM_RIGHTS &&
           cmsg->cmsg_len == CMSG_LEN(sizeof(int)))
          fd = *reinterpret_cast<int*>(CMSG_DATA(cmsg));
      }
      else if(header.msg_flags)
      {
        std::cout << posix::fg::red << "error, message flags: " << std::hex << header.msg_flags << std::dec << posix::fg::reset << std::endl << std::flush;
        return false;
      }
    }
    else // buffer too small!
    {
      errno = ENOBUFS;
      return false;
    }
    return true;
  }

  bool write(void) noexcept
  {
    msghdr header = {};
    iovec iov = {};
    char aux_buffer[CMSG_SPACE(sizeof(int))] = { 0 };

    header.msg_iov = &iov;
    header.msg_iovlen = 1;
    header.msg_control = aux_buffer;

    iov.iov_base = buffer.begin();
    iov.iov_len = buffer.size();

    header.msg_controllen = 0;

    if(fd != posix::invalid_descriptor) // if a file descriptor needs to be sent
    {
      header.msg_controllen = CMSG_SPACE(sizeof(int));
      cmsghdr* cmsg = CMSG_FIRSTHDR(&header);
      cmsg->cmsg_level = SOL_SOCKET;
      cmsg->cmsg_type = SCM_RIGHTS;
      cmsg->cmsg_len = CMSG_LEN(sizeof(int));
      *reinterpret_cast<int*>(CMSG_DATA(cmsg)) = fd;
    }

    return posix::sendmsg(socket, &header, 0) != posix::error_response;
  }

  posix::fd_t socket;
  vfifo       buffer;
  posix::fd_t fd;
};
#endif
<|endoftext|>


<commit_before>
#include "SerialData.h"
#include "PIDDataManager.h"
#include "SelfPositionManager.h"
#include "StrategyManager.h"
#include "BlockMoveManager.h"
#include "BlockMoveStrategy.h"
#include "InOutManager.h"
#include "PIDDataManager.h"
#include "SoundPlayTask.h"

#define FIELD 	220
#define EDGE_LINE 120 // 黒線との境界線
#define TURN_POWER 15 // 旋回時のパワー
#define ONLINE 25	  // 黒線上での輝度値
#define NotONLINE 60  // 黒線以外での輝度値

// 次のステートに切り替える
void BlockMoveStrategy::ChangeState(AbstractMoveState* nextState)
{
	delete State;
	State = nextState;
}

void BlockMoveStrategy::Run()
{
	State->Run();
}

// ブロックまで移動中の動作
void ApproachState::Run()
{
	BlockMoveManager* BtManager = BlockMoveManager::GetInstance();
	SelfPositionManager* SpManager = SelfPositionManager::GetInstance();
	InOutManager* IoManager = InOutManager::GetInstance();

	PIDData BlockMovePID = PIDDataManager::GetInstance()->GetPIDData(BlockMovePIDState);

	int currentAngle = SpManager->RobotAngle;

	int targetWaypointAngle = BtManager->GetSrcWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
	int targetBlockAngle = BtManager->GetSrcBlockAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);

	switch(SubState){
	// 初回処理
	case Initialize:
		if(BtManager->CurrentCommand.SourceBlockPosition == 9) {
			// 最寄の場合には、即ライントレース
			LeftEdge = false;
			SubState = LineTrace;
		} else if(CurrentWayPointNo == BtManager->GetSrcWayPointNo()) {
			// すでにウェイポイントの上にいるとき
			SubState = LineTurn;
		} else if(CurrentWayPointNo == 26) {
			// 初回であり、9ではないとき
			SubState = FirstTurn;
		} else {
			SubState = FirstTurn;
		}
		break;

	// 仮想ウェイポイント間を移動中の動作
	case FirstTurn:
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 2) {
			SubState = ImaginaryWaypoint;
		}

		// 旋回動作を実行
		IoManager->Turn(currentAngle, targetWaypointAngle, TURN_POWER);

		break;
	// ウェイポイントに向かって移動する動作
	case ImaginaryWaypoint:
		// ラインを認識した場合には、直進ステートに遷移
		if(IoManager->InputData.ReflectLight < ONLINE) {
			// ラインをまたぐまで直進
			SubState = OverLine;

			// ラインを跨いだので、ウェイポイントの座標に変更
			SpManager->ResetPoint(BtManager->GetLine(BtManager->GetSrcWayPointNo())->WayPoint);

		} else {
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ラインをまたぐまでの処理
	case OverLine:
		if(IoManager->InputData.ReflectLight > FIELD) {
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetSrcWayPointNo();

			// 白認識（ラインを跨いだ）ので、次の状態に繊維
			bool last = BtManager->ArrivalSrcWayPoint();

			// 最終ウェイポイントの場合
			if(last) {
				// ライントレース前の旋回動作に遷移
				SubState = LineTurn;
				CW = IoManager->JudgeTurnCW(currentAngle, targetBlockAngle);

			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		} else {
			// ラインをまたぐまでは直進
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ライントレース前の旋回動作
	case LineTurn:
		// 旋回動作を実行
		if(abs(targetBlockAngle - currentAngle) > 45 || IoManager->InputData.ReflectLight > EDGE_LINE){
			IoManager->Turn(CW, TURN_POWER);
		} else {
			// 角度が一致したため、ライントレースに遷移
			SubState = LineTrace;

			// 旋回方向によって、ラインのどちら側を走行するか決定
			LeftEdge = CW;
		}
		break;
	// ライントレースする動作
	case LineTrace:
		//目標とするブロック置き場の色を取得したら、現在のステートをブロック運搬中に変更
		if(IoManager->HSLKind == BtManager->GetSrcBlockPositionColor()){
			// ブロック置き場の座標に修正
			SpManager->ResetPoint(BtManager->GetSrcBlockPoint());

			// ブロック運搬時の経路がある（最終コマンドではない）場合には、ブロック運搬ステートに遷移
			if(BtManager->CurrentCommand.BlockMoveWaypointCount != 0) {
				// ブロック置き場到達メッセージ
				BtManager->ArrivalSrcBlockPosition();

				auto moveState = new MoveState(ParentStrategy);
				moveState->LeftEdge = LeftEdge;
				int nextWaypointAngle = BtManager->GetDstWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
				moveState->CW = IoManager->JudgeTurnCW(currentAngle, nextWaypointAngle);
				moveState->CurrentWayPointNo = CurrentWayPointNo;
				ParentStrategy->ChangeState(moveState);

				// 積算距離をリセット
				SpManager->Distance = 0;
			} else {
				IoManager->Stop();
			}
			break;
		}

		// ライントレース中は、ラインの角度に修正
		{
			int angle = BtManager->GetLine(BtManager->GetSrcWayPointNo())->GetAngle(BtManager->CurrentCommand.SourceBlockPosition);
			SpManager->ResetAngle(angle);
		}
		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);
		break;

	case FirstStraight:
	case FirstLineTrace:
	case Back:
		break;
	}
}

void MoveState::Run()
{
	BlockMoveManager* BtManager = BlockMoveManager::GetInstance();
	SelfPositionManager* SpManager = SelfPositionManager::GetInstance();
	InOutManager* IoManager = InOutManager::GetInstance();
	SpManager->ParticleFilterON = false;
	PIDData BlockMovePID = PIDDataManager::GetInstance()->GetPIDData(BlockMovePIDState);

	int currentAngle = SpManager->RobotAngle;
	int dstWayPointNo = BtManager->GetDstWayPointNo();
	int moveDistance = 0;
	int targetWaypointAngle = BtManager->GetDstWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
	int targetBlockAngle = BtManager->GetDstBlockAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);

	switch(SubState){
	// 初回処理
	case Initialize:
	/*
		// ウェイポイントが複数ある場合には、最初の１個は無視して、初回旋回に遷移
		if(BtManager->CurrentCommand.BlockMoveWaypointCount > 1) {
			// それ以外の場合には、最初のウェイポイントに即到達して、初回旋回に遷移
			BtManager->ArrivalDstWayPoint();

			// 角度を再計算して、方向を判定
			SubState = FirstTurn;
			break;
		}
*/
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 3) {
			SubState = FirstStraight;
			SpManager->Distance = 0;
			break;
		}

		if(CurrentWayPointNo == 22 && BtManager->CurrentCommand.SourceBlockPosition == 10) {
			CW = false;
		}

		LeftEdge = CW;
		// 旋回動作を実行
		IoManager->TurnWithBlock(CW, TURN_POWER , 0);
		break;
	case FirstStraight:
		if(dstWayPointNo == 0 || dstWayPointNo == 1 || dstWayPointNo == 2){
			moveDistance = 340;
		} else {
			moveDistance = 120;
		}

		if(SpManager->Distance > moveDistance) {
			SubState = FirstLineTrace;
		}
		IoManager->Forward(BlockMovePID.BasePower);
		break;

	// 仮想ウェイポイント間を移動中の動作
	case FirstTurn:
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 2) {
			SubState = ImaginaryWaypoint;

			// 移動距離をクリア
			SpManager->Distance = 0;
			break;
		}
		CW = IoManager->JudgeTurnCW(currentAngle, targetWaypointAngle);

		// 旋回動作を実行
		IoManager->TurnWithBlock(CW, TURN_POWER, -0.2);
		break;
	case FirstLineTrace:
	//	// 10cm程度進む
//		if(SpManager->Distance > 150)
		{
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetDstWayPointNo();

			// 次の状態に繊維
			bool last = BtManager->ArrivalDstWayPoint();

			// 最終ウェイポイントの場合
			if(last) {
				if(CurrentWayPointNo == 7 && BtManager->CurrentCommand.DestinationBlockPosition == 5) {
					CW = true;
				}

				// ライントレースに遷移
				SubState = LineTrace;
			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		}
//		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);
		break;
	// ウェイポイントに向かって移動する動作
	case ImaginaryWaypoint:
		// 一定距離進んだ後、ラインを認識した場合には、直進ステートに遷移
		if(IoManager->InputData.ReflectLight < ONLINE && SpManager->Distance > 30) {
			// ラインをまたぐまで直進
			SubState = OverLine;
			// ラインを跨いだので、ウェイポイントの座標に変更
			SpManager->ResetPoint(BtManager->GetLine(BtManager->GetDstWayPointNo())->WayPoint);

		} else {
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ラインをまたぐまでの処理
	case OverLine:
		if(IoManager->InputData.ReflectLight > EDGE_LINE) {
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetDstWayPointNo();

			// 白認識（ラインを跨いだ）ので、次の状態に繊維
			bool last = BtManager->ArrivalDstWayPoint();
			// 最終ウェイポイントの場合
			if(last) {
				// ライントレース前の旋回動作に遷移
				SubState = LineTurn;

				// 旋回方向を判定
				CW = IoManager->JudgeTurnCW(currentAngle, targetBlockAngle);
			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		} else {
			// ラインをまたぐまでは直進
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ライントレース前の旋回動作
	case LineTurn:
		// 旋回動作を実行
		if(abs(targetBlockAngle - currentAngle) < 70 &&
			(IoManager->InputData.ReflectLight < EDGE_LINE || IoManager->HSLKind == BtManager->GetDstBlockPositionColor()))
		{
			// 角度が一致したため、ライントレースに遷移
			SubState = LineTrace;

			// 旋回方向によって、ラインのどちら側を走行するか決定
			LeftEdge = CW;
			break;
		}

		IoManager->TurnWithBlock(CW, TURN_POWER, -0.3);
		break;

	// ライントレースする動作
	case LineTrace:
		//目標とするブロック置き場の色を取得したら、現在のステートをブロック運搬中に変更
		if(IoManager->HSLKind == BtManager->GetDstBlockPositionColor()) ColorDetectCount++;
		if(IoManager->HSLKind == HSLBlack) ColorDetectCount = 0;

		if(ColorDetectCount > 10)
		{
			// ブロック置き場の座標に修正
			SpManager->ResetPoint(BtManager->GetDstBlockPoint());
			SpManager->Distance = 200;
			SubState = Back;

			break;
		}

		// ライントレース中は、ラインの角度に修正
		{
			int angle = BtManager->GetLine(CurrentWayPointNo)->GetAngle(BtManager->CurrentCommand.DestinationBlockPosition);
			SpManager->ResetAngle(angle);
		}
		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);

		break;
	// 後退する動作
	case Back:
		if(SpManager->Distance < 80) {
			// 12cm後退したら、アプローチに遷移する

			// ブロック置き場到達メッセージ
			bool last = BtManager->ArrivalDstBlockPosition();

			// 最後までやったら、やりきった感を出す
			if (last) {
				IoManager->Stop();
				IoManager->WriteOutputMotor();
				PlaySound(BlockEnd);

				ParentStrategy->Manager->SetStrategy(NULL);
				break;
			}

			auto approachState = new ApproachState(ParentStrategy);
			approachState->CurrentWayPointNo = CurrentWayPointNo;
			ParentStrategy->ChangeState(approachState);

			break;
		}
		IoManager->Back(BlockMovePID.BasePower);
		break;
	}

}

<commit_msg>ウェイポイント番号が違っているバグを修正<commit_after>
#include "SerialData.h"
#include "PIDDataManager.h"
#include "SelfPositionManager.h"
#include "StrategyManager.h"
#include "BlockMoveManager.h"
#include "BlockMoveStrategy.h"
#include "InOutManager.h"
#include "PIDDataManager.h"
#include "SoundPlayTask.h"

#define FIELD 	220
#define EDGE_LINE 120 // 黒線との境界線
#define TURN_POWER 15 // 旋回時のパワー
#define ONLINE 25	  // 黒線上での輝度値
#define NotONLINE 60  // 黒線以外での輝度値

// 次のステートに切り替える
void BlockMoveStrategy::ChangeState(AbstractMoveState* nextState)
{
	delete State;
	State = nextState;
}

void BlockMoveStrategy::Run()
{
	State->Run();
}

// ブロックまで移動中の動作
void ApproachState::Run()
{
	BlockMoveManager* BtManager = BlockMoveManager::GetInstance();
	SelfPositionManager* SpManager = SelfPositionManager::GetInstance();
	InOutManager* IoManager = InOutManager::GetInstance();

	PIDData BlockMovePID = PIDDataManager::GetInstance()->GetPIDData(BlockMovePIDState);

	int currentAngle = SpManager->RobotAngle;

	int targetWaypointAngle = BtManager->GetSrcWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
	int targetBlockAngle = BtManager->GetSrcBlockAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);

	switch(SubState){
	// 初回処理
	case Initialize:
		if(BtManager->CurrentCommand.SourceBlockPosition == 9) {
			// 最寄の場合には、即ライントレース
			LeftEdge = false;
			SubState = LineTrace;
		} else if(CurrentWayPointNo == BtManager->GetSrcWayPointNo()) {
			// すでにウェイポイントの上にいるとき
			SubState = LineTurn;
		} else if(CurrentWayPointNo == 26) {
			// 初回であり、9ではないとき
			SubState = FirstTurn;
		} else {
			SubState = FirstTurn;
		}
		break;

	// 仮想ウェイポイント間を移動中の動作
	case FirstTurn:
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 2) {
			SubState = ImaginaryWaypoint;
		}

		// 旋回動作を実行
		IoManager->Turn(currentAngle, targetWaypointAngle, TURN_POWER);

		break;
	// ウェイポイントに向かって移動する動作
	case ImaginaryWaypoint:
		// ラインを認識した場合には、直進ステートに遷移
		if(IoManager->InputData.ReflectLight < ONLINE) {
			// ラインをまたぐまで直進
			SubState = OverLine;

			// ラインを跨いだので、ウェイポイントの座標に変更
			SpManager->ResetPoint(BtManager->GetLine(BtManager->GetSrcWayPointNo())->WayPoint);

		} else {
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ラインをまたぐまでの処理
	case OverLine:
		if(IoManager->InputData.ReflectLight > FIELD) {
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetSrcWayPointNo();

			// 白認識（ラインを跨いだ）ので、次の状態に繊維
			bool last = BtManager->ArrivalSrcWayPoint();

			// 最終ウェイポイントの場合
			if(last) {
				// ライントレース前の旋回動作に遷移
				SubState = LineTurn;
				CW = IoManager->JudgeTurnCW(currentAngle, targetBlockAngle);

			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		} else {
			// ラインをまたぐまでは直進
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ライントレース前の旋回動作
	case LineTurn:
		// 旋回動作を実行
		if(abs(targetBlockAngle - currentAngle) > 45 || IoManager->InputData.ReflectLight > EDGE_LINE){
			IoManager->Turn(CW, TURN_POWER);
		} else {
			// 角度が一致したため、ライントレースに遷移
			SubState = LineTrace;

			// 旋回方向によって、ラインのどちら側を走行するか決定
			LeftEdge = CW;
		}
		break;
	// ライントレースする動作
	case LineTrace:
		//目標とするブロック置き場の色を取得したら、現在のステートをブロック運搬中に変更
		if(IoManager->HSLKind == BtManager->GetSrcBlockPositionColor()){
			// ブロック置き場の座標に修正
			SpManager->ResetPoint(BtManager->GetSrcBlockPoint());

			// ブロック運搬時の経路がある（最終コマンドではない）場合には、ブロック運搬ステートに遷移
			if(BtManager->CurrentCommand.BlockMoveWaypointCount != 0) {
				// ブロック置き場到達メッセージ
				BtManager->ArrivalSrcBlockPosition();

				auto moveState = new MoveState(ParentStrategy);
				moveState->LeftEdge = LeftEdge;
				int nextWaypointAngle = BtManager->GetDstWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
				moveState->CW = IoManager->JudgeTurnCW(currentAngle, nextWaypointAngle);
				moveState->CurrentWayPointNo = CurrentWayPointNo;
				ParentStrategy->ChangeState(moveState);

				// 積算距離をリセット
				SpManager->Distance = 0;
			} else {
				IoManager->Stop();
			}
			break;
		}

		// ライントレース中は、ラインの角度に修正
		{
			int angle = BtManager->GetLine(BtManager->GetSrcWayPointNo())->GetAngle(BtManager->CurrentCommand.SourceBlockPosition);
			SpManager->ResetAngle(angle);
		}
		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);
		break;

	case FirstStraight:
	case FirstLineTrace:
	case Back:
		break;
	}
}

void MoveState::Run()
{
	BlockMoveManager* BtManager = BlockMoveManager::GetInstance();
	SelfPositionManager* SpManager = SelfPositionManager::GetInstance();
	InOutManager* IoManager = InOutManager::GetInstance();
	SpManager->ParticleFilterON = false;
	PIDData BlockMovePID = PIDDataManager::GetInstance()->GetPIDData(BlockMovePIDState);

	int currentAngle = SpManager->RobotAngle;
	int dstWayPointNo = BtManager->GetDstWayPointNo();
	int moveDistance = 0;
	int targetWaypointAngle = BtManager->GetDstWaypointAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);
	int targetBlockAngle = BtManager->GetDstBlockAngle(SpManager->RobotPoint.X, SpManager->RobotPoint.Y);

	switch(SubState){
	// 初回処理
	case Initialize:
	/*
		// ウェイポイントが複数ある場合には、最初の１個は無視して、初回旋回に遷移
		if(BtManager->CurrentCommand.BlockMoveWaypointCount > 1) {
			// それ以外の場合には、最初のウェイポイントに即到達して、初回旋回に遷移
			BtManager->ArrivalDstWayPoint();

			// 角度を再計算して、方向を判定
			SubState = FirstTurn;
			break;
		}
*/
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 3) {
			SubState = FirstStraight;
			SpManager->Distance = 0;
			break;
		}

		if(CurrentWayPointNo == 22 && BtManager->CurrentCommand.SourceBlockPosition == 10) {
			CW = false;
		}

		LeftEdge = CW;
		// 旋回動作を実行
		IoManager->TurnWithBlock(CW, TURN_POWER , 0);
		break;
	case FirstStraight:
		if(dstWayPointNo == 0 || dstWayPointNo == 1 || dstWayPointNo == 2){
			moveDistance = 340;
		} else {
			moveDistance = 120;
		}

		if(SpManager->Distance > moveDistance) {
			SubState = FirstLineTrace;
		}
		IoManager->Forward(BlockMovePID.BasePower);
		break;

	// 仮想ウェイポイント間を移動中の動作
	case FirstTurn:
		// 角度が一致したため、仮想ウェイポイント間の移動に遷移
		if (abs(targetWaypointAngle - currentAngle) < 2) {
			SubState = ImaginaryWaypoint;

			// 移動距離をクリア
			SpManager->Distance = 0;
			break;
		}
		CW = IoManager->JudgeTurnCW(currentAngle, targetWaypointAngle);

		// 旋回動作を実行
		IoManager->TurnWithBlock(CW, TURN_POWER, -0.2);
		break;
	case FirstLineTrace:
	//	// 10cm程度進む
//		if(SpManager->Distance > 150)
		{
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetDstWayPointNo();

			// 次の状態に繊維
			bool last = BtManager->ArrivalDstWayPoint();

			// 最終ウェイポイントの場合
			if(last) {
				if(CurrentWayPointNo == 6 && BtManager->CurrentCommand.DestinationBlockPosition == 5) {
					CW = true;
				}

				// ライントレースに遷移
				SubState = LineTrace;
			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		}
//		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);
		break;
	// ウェイポイントに向かって移動する動作
	case ImaginaryWaypoint:
		// 一定距離進んだ後、ラインを認識した場合には、直進ステートに遷移
		if(IoManager->InputData.ReflectLight < ONLINE && SpManager->Distance > 30) {
			// ラインをまたぐまで直進
			SubState = OverLine;
			// ラインを跨いだので、ウェイポイントの座標に変更
			SpManager->ResetPoint(BtManager->GetLine(BtManager->GetDstWayPointNo())->WayPoint);

		} else {
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ラインをまたぐまでの処理
	case OverLine:
		if(IoManager->InputData.ReflectLight > EDGE_LINE) {
			// ウェイポイントに到達したので、現在いるウェイポイントNoを更新
			CurrentWayPointNo = BtManager->GetDstWayPointNo();

			// 白認識（ラインを跨いだ）ので、次の状態に繊維
			bool last = BtManager->ArrivalDstWayPoint();
			// 最終ウェイポイントの場合
			if(last) {
				// ライントレース前の旋回動作に遷移
				SubState = LineTurn;

				// 旋回方向を判定
				CW = IoManager->JudgeTurnCW(currentAngle, targetBlockAngle);
			} else {
				// 初回旋回に遷移
				SubState = FirstTurn;
			}
		} else {
			// ラインをまたぐまでは直進
			IoManager->Forward(BlockMovePID.BasePower);
		}
		break;
	// ライントレース前の旋回動作
	case LineTurn:
		// 旋回動作を実行
		if(abs(targetBlockAngle - currentAngle) < 70 &&
			(IoManager->InputData.ReflectLight < EDGE_LINE || IoManager->HSLKind == BtManager->GetDstBlockPositionColor()))
		{
			// 角度が一致したため、ライントレースに遷移
			SubState = LineTrace;

			// 旋回方向によって、ラインのどちら側を走行するか決定
			LeftEdge = CW;
			break;
		}

		IoManager->TurnWithBlock(CW, TURN_POWER, -0.3);
		break;

	// ライントレースする動作
	case LineTrace:
		//目標とするブロック置き場の色を取得したら、現在のステートをブロック運搬中に変更
		if(IoManager->HSLKind == BtManager->GetDstBlockPositionColor()) ColorDetectCount++;
		if(IoManager->HSLKind == HSLBlack) ColorDetectCount = 0;

		if(ColorDetectCount > 10)
		{
			// ブロック置き場の座標に修正
			SpManager->ResetPoint(BtManager->GetDstBlockPoint());
			SpManager->Distance = 200;
			SubState = Back;

			break;
		}

		// ライントレース中は、ラインの角度に修正
		{
			int angle = BtManager->GetLine(CurrentWayPointNo)->GetAngle(BtManager->CurrentCommand.DestinationBlockPosition);
			SpManager->ResetAngle(angle);
		}
		IoManager->LineTraceAction(BlockMovePID, EDGE_LINE, LeftEdge);

		break;
	// 後退する動作
	case Back:
		if(SpManager->Distance < 80) {
			// 12cm後退したら、アプローチに遷移する

			// ブロック置き場到達メッセージ
			bool last = BtManager->ArrivalDstBlockPosition();

			// 最後までやったら、やりきった感を出す
			if (last) {
				IoManager->Stop();
				IoManager->WriteOutputMotor();
				PlaySound(BlockEnd);

				ParentStrategy->Manager->SetStrategy(NULL);
				break;
			}

			auto approachState = new ApproachState(ParentStrategy);
			approachState->CurrentWayPointNo = CurrentWayPointNo;
			ParentStrategy->ChangeState(approachState);

			break;
		}
		IoManager->Back(BlockMovePID.BasePower);
		break;
	}

}

<|endoftext|>


<commit_before>#include "physics/n_body_system.hpp"

#include <cmath>
#include <functional>
#include <vector>

#include "geometry/r3_element.hpp"
#ifndef _MANAGED
#include "glog/logging.h"
#endif
#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"
#include "quantities/quantities.hpp"

using principia::geometry::R3Element;
using principia::integrators::SPRKIntegrator;
using principia::integrators::SymplecticIntegrator;
using principia::quantities::Acceleration;
using principia::quantities::Exponentiation;
using principia::quantities::GravitationalParameter;
using principia::quantities::Length;
using principia::quantities::Speed;
using principia::quantities::SIUnit;

namespace principia {
namespace physics {

template<typename InertialFrame>
NBodySystem<InertialFrame>::NBodySystem(
    std::vector<Body<InertialFrame>*> const* bodies)
    : bodies_(bodies) {}

template<typename InertialFrame>
NBodySystem<InertialFrame>::~NBodySystem() {
  for (Body<InertialFrame>* body : *bodies_) {
    delete body;
  }
}

template<typename InertialFrame>
std::vector<Body<InertialFrame>*> const&
NBodySystem<InertialFrame>::bodies() const {
  return *bodies_;
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::Integrate(
    SymplecticIntegrator<Length, Speed> const& integrator,
    Time const& tmax,
    Time const& Δt,
    int const sampling_period) {
  SymplecticIntegrator<Length, Speed>::Parameters parameters;
  SymplecticIntegrator<Length, Speed>::Solution solution;

  // Prepare the input data.
  std::unique_ptr<Time> reference_time;
  for (Body<InertialFrame> const* body : *bodies_) {
    R3Element<Length> const& position = body->positions().back().coordinates();
    R3Element<Speed> const& velocity = body->velocities().back().coordinates();
    Time const& time = body->times().back();
    for (int i = 0; i < 3; ++i) {
      parameters.q0.push_back(position[i]);
    }
    for (int i = 0; i < 3; ++i) {
      parameters.p0.push_back(velocity[i]);
    }
    parameters.t0 = time;
    // All the positions/velocities must be for the same time.
    if (reference_time == nullptr) {
      reference_time.reset(new Time(time));
    } else {
#ifndef _MANAGED
      CHECK_EQ(*reference_time, time);
#endif
    }
  }

  parameters.tmax = tmax;
  parameters.Δt = Δt;
  parameters.sampling_period = sampling_period;
  dynamic_cast<const SPRKIntegrator<Length, Speed>*>(&integrator)->Solve(
      std::bind(&NBodySystem::ComputeGravitationalAccelerations, this,
                std::placeholders::_1,
                std::placeholders::_2,
                std::placeholders::_3),
      &ComputeGravitationalVelocities,
      parameters, &solution);

#ifndef _MANAGED
  // TODO(phl): Ignoring errors for now.
  CHECK_EQ(solution.position.size(), solution.momentum.size());
#endif
  std::vector<Time> const& t = solution.time.quantities;
  // Loop over the bodies.
  // TODO(phl): It looks like we are transposing in the integrator and then
  // transposing here again.
  for (size_t i = 0; i < solution.position.size(); i += 3) {
    Body<InertialFrame>* body = (*bodies_)[i / 3];
    std::vector<Length> const& q0 = solution.position[i + 0].quantities;
    std::vector<Length> const& q1 = solution.position[i + 1].quantities;
    std::vector<Length> const& q2 = solution.position[i + 2].quantities;
    std::vector<Speed> const& p0 = solution.momentum[i + 0].quantities;
    std::vector<Speed> const& p1 = solution.momentum[i + 1].quantities;
    std::vector<Speed> const& p2 = solution.momentum[i + 2].quantities;
#ifndef _MANAGED
    CHECK_EQ(t.size(), q0.size());
    CHECK_EQ(t.size(), q1.size());
    CHECK_EQ(t.size(), q2.size());
    CHECK_EQ(t.size(), p0.size());
    CHECK_EQ(t.size(), p1.size());
    CHECK_EQ(t.size(), p2.size());
#endif
    for (size_t j = 0; j < t.size(); ++j) {
      Vector<Length, InertialFrame> const position(
          R3Element<Length>(q0[j], q1[j], q2[j]));
      Vector<Speed, InertialFrame> const velocity(
          R3Element<Speed>(p0[j], p1[j], p2[j]));
      Time const time = t[j];
      body->AppendToTrajectory(position, velocity, time);
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalAccelerations(
    Time const& t,
    std::vector<Length> const& q,
    std::vector<Acceleration>* result) const {
  result->assign(result->size(), Acceleration());

  // TODO(phl): Used to deal with proper accelerations here.
  for (size_t b1 = 0, three_b1 = 0; b1 < bodies_->size(); ++b1, three_b1 += 3) {
    Body<InertialFrame> const& body1 = *(*bodies_)[b1];
    bool const body1_is_massless = body1.is_massless();
    for (size_t b2 = b1 + 1; b2 < bodies_->size(); ++b2) {
      Body<InertialFrame> const& body2 = *(*bodies_)[b2];
      bool const body2_is_massless = body2.is_massless();
      size_t three_b2 = 3 * b2;
      if (!body1_is_massless || !body2_is_massless) {
        Length const Δq0 = q[three_b1] - q[three_b2];
        Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
        Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

        Exponentiation<Length, 2> const squared_distance =
            Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
        Exponentiation<Length, -3> const multiplier =
            Sqrt(squared_distance) / (squared_distance * squared_distance);

        if (!body2_is_massless) {
          auto const μ2OverRSquared =
              body2.gravitational_parameter() * multiplier;
          (*result)[three_b1] -= Δq0 * μ2OverRSquared;
          (*result)[three_b1 + 1] -= Δq1 * μ2OverRSquared;
          (*result)[three_b1 + 2] -= Δq2 * μ2OverRSquared;
        }
        // Lex. III. Actioni contrariam semper & æqualem esse reactionem:
        // sive corporum duorum actiones in se mutuo semper esse æquales &
        // in partes contrarias dirigi.
        if (!body1_is_massless) {
          auto const μ1OverRSquared =
              body1.gravitational_parameter() * multiplier;
          (*result)[three_b2] += Δq0 * μ1OverRSquared;
          (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
          (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
        }
      }
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalVelocities(
    std::vector<Speed> const& p,
    std::vector<Speed>* result) {
  *result = p;
}

}  // namespace physics
}  // namespace principia
<commit_msg>After eggrobin's review.<commit_after>#include "physics/n_body_system.hpp"

#include <cmath>
#include <functional>
#include <vector>

#include "geometry/r3_element.hpp"
#ifndef _MANAGED
#include "glog/logging.h"
#endif
#include "integrators/symplectic_partitioned_runge_kutta_integrator.hpp"
#include "quantities/quantities.hpp"

using principia::geometry::R3Element;
using principia::integrators::SPRKIntegrator;
using principia::integrators::SymplecticIntegrator;
using principia::quantities::Acceleration;
using principia::quantities::Exponentiation;
using principia::quantities::GravitationalParameter;
using principia::quantities::Length;
using principia::quantities::Speed;
using principia::quantities::SIUnit;

namespace principia {
namespace physics {

template<typename InertialFrame>
NBodySystem<InertialFrame>::NBodySystem(
    std::vector<Body<InertialFrame>*> const* bodies)
    : bodies_(bodies) {}

template<typename InertialFrame>
NBodySystem<InertialFrame>::~NBodySystem() {
  for (Body<InertialFrame>* body : *bodies_) {
    delete body;
  }
}

template<typename InertialFrame>
std::vector<Body<InertialFrame>*> const&
NBodySystem<InertialFrame>::bodies() const {
  return *bodies_;
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::Integrate(
    SymplecticIntegrator<Length, Speed> const& integrator,
    Time const& tmax,
    Time const& Δt,
    int const sampling_period) {
  SymplecticIntegrator<Length, Speed>::Parameters parameters;
  SymplecticIntegrator<Length, Speed>::Solution solution;

  // Prepare the input data.
  std::unique_ptr<Time> reference_time;
  for (Body<InertialFrame> const* body : *bodies_) {
    R3Element<Length> const& position = body->positions().back().coordinates();
    R3Element<Speed> const& velocity = body->velocities().back().coordinates();
    Time const& time = body->times().back();
    for (int i = 0; i < 3; ++i) {
      parameters.q0.push_back(position[i]);
    }
    for (int i = 0; i < 3; ++i) {
      parameters.p0.push_back(velocity[i]);
    }
    parameters.t0 = time;
    // All the positions/velocities must be for the same time.
    if (reference_time == nullptr) {
      reference_time.reset(new Time(time));
    } else {
#ifndef _MANAGED
      CHECK_EQ(*reference_time, time);
#endif
    }
  }

  parameters.tmax = tmax;
  parameters.Δt = Δt;
  parameters.sampling_period = sampling_period;
  dynamic_cast<const SPRKIntegrator<Length, Speed>*>(&integrator)->Solve(
      std::bind(&NBodySystem::ComputeGravitationalAccelerations, this,
                std::placeholders::_1,
                std::placeholders::_2,
                std::placeholders::_3),
      &ComputeGravitationalVelocities,
      parameters, &solution);

#ifndef _MANAGED
  // TODO(phl): Ignoring errors for now.
  CHECK_EQ(solution.position.size(), solution.momentum.size());
#endif
  std::vector<Time> const& t = solution.time.quantities;
  // Loop over the bodies.
  // TODO(phl): It looks like we are transposing in the integrator and then
  // transposing here again.
  for (size_t i = 0; i < solution.position.size(); i += 3) {
    Body<InertialFrame>* body = (*bodies_)[i / 3];
    std::vector<Length> const& q0 = solution.position[i + 0].quantities;
    std::vector<Length> const& q1 = solution.position[i + 1].quantities;
    std::vector<Length> const& q2 = solution.position[i + 2].quantities;
    std::vector<Speed> const& p0 = solution.momentum[i + 0].quantities;
    std::vector<Speed> const& p1 = solution.momentum[i + 1].quantities;
    std::vector<Speed> const& p2 = solution.momentum[i + 2].quantities;
#ifndef _MANAGED
    CHECK_EQ(t.size(), q0.size());
    CHECK_EQ(t.size(), q1.size());
    CHECK_EQ(t.size(), q2.size());
    CHECK_EQ(t.size(), p0.size());
    CHECK_EQ(t.size(), p1.size());
    CHECK_EQ(t.size(), p2.size());
#endif
    for (size_t j = 0; j < t.size(); ++j) {
      Vector<Length, InertialFrame> const position(
          R3Element<Length>(q0[j], q1[j], q2[j]));
      Vector<Speed, InertialFrame> const velocity(
          R3Element<Speed>(p0[j], p1[j], p2[j]));
      Time const time = t[j];
      body->AppendToTrajectory(position, velocity, time);
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalAccelerations(
    Time const& t,
    std::vector<Length> const& q,
    std::vector<Acceleration>* result) const {
  result->assign(result->size(), Acceleration());

  // TODO(phl): Used to deal with proper accelerations here.
  for (size_t b1 = 0, three_b1 = 0; b1 < bodies_->size(); ++b1, three_b1 += 3) {
    Body<InertialFrame> const& body1 = *(*bodies_)[b1];
    bool const body1_is_massless = body1.is_massless();
    for (size_t b2 = b1 + 1; b2 < bodies_->size(); ++b2) {
      Body<InertialFrame> const& body2 = *(*bodies_)[b2];
      bool const body2_is_massless = body2.is_massless();
      size_t const three_b2 = 3 * b2;
      if (!body1_is_massless || !body2_is_massless) {
        Length const Δq0 = q[three_b1] - q[three_b2];
        Length const Δq1 = q[three_b1 + 1] - q[three_b2 + 1];
        Length const Δq2 = q[three_b1 + 2] - q[three_b2 + 2];

        Exponentiation<Length, 2> const squared_distance =
            Δq0 * Δq0 + Δq1 * Δq1 + Δq2 * Δq2;
        Exponentiation<Length, -3> const multiplier =
            Sqrt(squared_distance) / (squared_distance * squared_distance);

        if (!body2_is_massless) {
          auto const μ2OverRSquared =
              body2.gravitational_parameter() * multiplier;
          (*result)[three_b1] -= Δq0 * μ2OverRSquared;
          (*result)[three_b1 + 1] -= Δq1 * μ2OverRSquared;
          (*result)[three_b1 + 2] -= Δq2 * μ2OverRSquared;
        }
        // Lex. III. Actioni contrariam semper & æqualem esse reactionem:
        // sive corporum duorum actiones in se mutuo semper esse æquales &
        // in partes contrarias dirigi.
        if (!body1_is_massless) {
          auto const μ1OverRSquared =
              body1.gravitational_parameter() * multiplier;
          (*result)[three_b2] += Δq0 * μ1OverRSquared;
          (*result)[three_b2 + 1] += Δq1 * μ1OverRSquared;
          (*result)[three_b2 + 2] += Δq2 * μ1OverRSquared;
        }
      }
    }
  }
}

template<typename InertialFrame>
void NBodySystem<InertialFrame>::ComputeGravitationalVelocities(
    std::vector<Speed> const& p,
    std::vector<Speed>* result) {
  *result = p;
}

}  // namespace physics
}  // namespace principia
<|endoftext|>


<commit_before><commit_msg>bugfix<commit_after><|endoftext|>


<commit_before>#include "physics/n_body_system.hpp"

#include <map>
#include <memory>
#include <string>
#include <vector>

#include "geometry/grassmann.hpp"
#include "geometry/point.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "physics/body.hpp"
#include "physics/trajectory.hpp"
#include "quantities/constants.hpp"
#include "quantities/numbers.hpp"
#include "testing_utilities/almost_equals.hpp"
#include "testing_utilities/death_message.hpp"

using principia::constants::GravitationalConstant;
using principia::geometry::Barycentre;
using principia::geometry::Point;
using principia::geometry::Vector;
using principia::quantities::Pow;
using principia::quantities::SIUnit;
using principia::testing_utilities::AlmostEquals;
using principia::testing_utilities::DeathMessage;
using testing::Eq;
using testing::Lt;

namespace principia {
namespace physics {

class NBodySystemTest : public testing::Test {
 protected:
  struct EarthMoonBarycentricFrame;

  void SetUp() override {
    integrator_.Initialize(integrator_.Order5Optimal());

    // The Earth-Moon system, roughly, with a circular orbit with velocities
    // in the center-of-mass frame.
    body1_ = std::make_unique<Body>(6E24 * SIUnit<Mass>());
    body2_ = std::make_unique<Body>(7E22 * SIUnit<Mass>());

    // A massless probe.
    body3_ = std::make_unique<Body>(0 * SIUnit<Mass>());

    trajectory1_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body1_);
    trajectory2_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body2_);
    trajectory3_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body3_);
    Point<Vector<Length, EarthMoonBarycentricFrame>> const
        q1(Vector<Length, EarthMoonBarycentricFrame>({0 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>()}));
    Point<Vector<Length, EarthMoonBarycentricFrame>> const
        q2(Vector<Length, EarthMoonBarycentricFrame>({0 * SIUnit<Length>(),
                                                      4E8 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>()}));
    Point<Vector<Length, EarthMoonBarycentricFrame>> const centre_of_mass =
        Barycentre(q1, body1_->mass(), q2, body2_->mass());
    Length const semi_major_axis = (q1 - q2).Norm();
    period_ = 2 * π * Sqrt(Pow<3>(semi_major_axis) /
                               (body1_->gravitational_parameter() +
                                body2_->gravitational_parameter()));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const
        v1(Vector<Speed, EarthMoonBarycentricFrame>({
            -2 * π * (q1 - centre_of_mass).Norm() / period_,
            0 * SIUnit<Speed>(),
            0 * SIUnit<Speed>()}));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const
        v2(Vector<Speed, EarthMoonBarycentricFrame>({
            2 * π * (q2 - centre_of_mass).Norm() / period_,
            0 * SIUnit<Speed>(),
            0 * SIUnit<Speed>()}));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const overall_velocity =
        Barycentre(v1, body1_->mass(), v2, body2_->mass());
    trajectory1_->Append(0 * SIUnit<Time>(),
                         {q1 - centre_of_mass, v1 - overall_velocity});
    trajectory2_->Append(0 * SIUnit<Time>(),
                         {q2 - centre_of_mass, v2 - overall_velocity});
    system_ = std::make_unique<NBodySystem<EarthMoonBarycentricFrame>>();
  }

  template<typename Scalar, typename Frame>
  std::string ToMathematicaString(Vector<Scalar, Frame> const& vector) {
    R3Element<Scalar> const& coordinates = vector.coordinates();
    std::string result = "{";
    result += quantities::DebugString(coordinates.x);
    result += ",";
    result += quantities::DebugString(coordinates.y);
    result += ",";
    result += quantities::DebugString(coordinates.z);
    result += "}";
    return result;
  }

  template<typename Scalar, typename Frame>
  std::string ToMathematicaString(
      std::vector<Vector<Scalar, Frame>> const& vectors) {
    static std::string const mathematica_line =
        "\n(*****************************************************)\n";
    std::string result = mathematica_line;
    result += "ToExpression[StringReplace[\"\n{";
    std::string separator = "";
    for (const auto& vector : vectors) {
      result += separator;
      result += ToMathematicaString(vector);
      separator = ",\n";
    }
    result +=
        "}\",\n{\" m\"->\"\",\"e\"->\"*^\", \"\\n\"->\"\", \" \"->\"\"}]];";
    result += mathematica_line;
    return result;
  }

  template<typename T1, typename T2>
  std::vector<T2> ValuesOf(std::map<T1, T2> const& m) {
    std::vector<T2> result;
    for (auto const it : m) {
      result.push_back(it.second);
    }
    return result;
  }

  std::unique_ptr<Body> body1_;
  std::unique_ptr<Body> body2_;
  std::unique_ptr<Body> body3_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory1_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory2_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory3_;
  SPRKIntegrator<Length, Speed> integrator_;
  Time period_;
  std::unique_ptr<NBodySystem<EarthMoonBarycentricFrame>> system_;
};

typedef NBodySystemTest NBodySystemDeathTest;

TEST_F(NBodySystemDeathTest, IntegrateError) {
  EXPECT_DEATH({
    system_->Integrate(integrator_,
                       period_,
                       period_ / 100,
                       1,
                       {trajectory1_.get(),
                        trajectory2_.get(),
                        trajectory1_.get()});
  }, DeathMessage("Multiple trajectories"));
  EXPECT_DEATH({
    std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory(
        new Trajectory<EarthMoonBarycentricFrame>(*body2_));
    trajectory->Append(1 * SIUnit<Time>(),
                       {Vector<Length, EarthMoonBarycentricFrame>(),
                        Vector<Speed, EarthMoonBarycentricFrame>()});
    system_->Integrate(integrator_,
                       period_,
                       period_ / 100,
                       1,
                       {trajectory1_.get(), trajectory.get()});
  }, DeathMessage("Inconsistent last time"));
}

// The canonical Earth-Moon system, tuned to produce circular orbits.
TEST_F(NBodySystemTest, EarthMoon) {
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> positions;
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory1_.get(), trajectory2_.get()});

  positions = ValuesOf(trajectory1_->Positions());
  EXPECT_THAT(positions.size(), Eq(101));
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(3E-2 * SIUnit<Length>()));

  positions = ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(2 * SIUnit<Length>()));
}

// Same as above, but the trajectories are passed in the reverse order.
TEST_F(NBodySystemTest, MoonEarth) {
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> positions;
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory2_.get(), trajectory1_.get()});

  positions = ValuesOf(trajectory1_->Positions());
  EXPECT_THAT(positions.size(), Eq(101));
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(3E-2 * SIUnit<Length>()));

  positions = ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(2 * SIUnit<Length>()));
}

// The Moon alone.  It moves in straight line.
TEST_F(NBodySystemTest, Moon) {
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory2_.get()});

  Length const q2 = trajectory2_->last_position().coordinates().y;
  Speed const v2 = trajectory2_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions =
      ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(positions[25].coordinates().x, Eq(0.25 * period_ * v2));
  EXPECT_THAT(positions[25].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[50].coordinates().x, Eq(0.50 * period_ * v2));
  EXPECT_THAT(positions[50].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v2, 1));
  EXPECT_THAT(positions[75].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[100].coordinates().x, Eq(1.00 * period_ * v2));
  EXPECT_THAT(positions[100].coordinates().y, Eq(q2));
}

// The Earth and a massless probe 1 billion meters away, with the same velocity,
// and an acceleration which exactly compensates gravitational attraction.  Both
// bodies move in straight lines.
TEST_F(NBodySystemTest, EarthProbe) {
  Length const distance = 1E9 * SIUnit<Length>();
  trajectory3_->Append(trajectory1_->last_time(),
                       {trajectory1_->last_position() +
                            Vector<Length, EarthMoonBarycentricFrame>(
                                {0 * SIUnit<Length>(),
                                 distance,
                                 0 * SIUnit<Length>()}),
                        trajectory1_->last_velocity()});
  trajectory3_->set_intrinsic_acceleration(
      [this, distance](Time const& t) {
    return Vector<Acceleration, EarthMoonBarycentricFrame>(
        {0 * SIUnit<Acceleration>(),
         body1_->gravitational_parameter() / (distance * distance),
         0 * SIUnit<Acceleration>()});});

  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory1_.get(), trajectory3_.get()});

  Length const q1 = trajectory1_->last_position().coordinates().y;
  Speed const v1 = trajectory1_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions1 =
      ValuesOf(trajectory1_->Positions());
  LOG(INFO) << ToMathematicaString(positions1);
  EXPECT_THAT(positions1.size(), Eq(101));
  EXPECT_THAT(positions1[25].coordinates().x,
              AlmostEquals(0.25 * period_ * v1, 1));
  EXPECT_THAT(positions1[25].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[50].coordinates().x,
              AlmostEquals(0.50 * period_ * v1, 1));
  EXPECT_THAT(positions1[50].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v1, 1));
  EXPECT_THAT(positions1[75].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[100].coordinates().x,
              AlmostEquals(1.00 * period_ * v1, 1));
  EXPECT_THAT(positions1[100].coordinates().y, Eq(q1));

  Length const q3 = trajectory3_->last_position().coordinates().y;
  Speed const v3 = trajectory3_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions3 =
      ValuesOf(trajectory3_->Positions());
  LOG(INFO) << ToMathematicaString(positions3);
  EXPECT_THAT(positions3.size(), Eq(101));
  EXPECT_THAT(positions3[25].coordinates().x,
              AlmostEquals(0.25 * period_ * v3, 1));
  EXPECT_THAT(positions3[25].coordinates().y, AlmostEquals(q3, 2));
  EXPECT_THAT(positions3[50].coordinates().x,
              AlmostEquals(0.50 * period_ * v3, 1));
  EXPECT_THAT(positions3[50].coordinates().y, AlmostEquals(q3, 2));
  EXPECT_THAT(positions3[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v3, 1));
  EXPECT_THAT(positions3[75].coordinates().y, AlmostEquals(q3, 1));
  EXPECT_THAT(positions3[100].coordinates().x,
              AlmostEquals(1.00 * period_ * v3, 1));
  EXPECT_THAT(positions3[100].coordinates().y, Eq(q3));
}

}  // namespace physics
}  // namespace principia
<commit_msg>Fix test (partially).<commit_after>#include "physics/n_body_system.hpp"

#include <map>
#include <memory>
#include <string>
#include <vector>

#include "geometry/grassmann.hpp"
#include "geometry/point.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "physics/body.hpp"
#include "physics/trajectory.hpp"
#include "quantities/constants.hpp"
#include "quantities/numbers.hpp"
#include "testing_utilities/almost_equals.hpp"
#include "testing_utilities/death_message.hpp"

using principia::constants::GravitationalConstant;
using principia::geometry::Barycentre;
using principia::geometry::Point;
using principia::geometry::Vector;
using principia::quantities::Pow;
using principia::quantities::SIUnit;
using principia::testing_utilities::AlmostEquals;
using principia::testing_utilities::DeathMessage;
using testing::Eq;
using testing::Lt;

namespace principia {
namespace physics {

class NBodySystemTest : public testing::Test {
 protected:
  struct EarthMoonBarycentricFrame;

  void SetUp() override {
    integrator_.Initialize(integrator_.Order5Optimal());

    // The Earth-Moon system, roughly, with a circular orbit with velocities
    // in the center-of-mass frame.
    body1_ = std::make_unique<Body>(6E24 * SIUnit<Mass>());
    body2_ = std::make_unique<Body>(7E22 * SIUnit<Mass>());

    // A massless probe.
    body3_ = std::make_unique<Body>(0 * SIUnit<Mass>());

    trajectory1_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body1_);
    trajectory2_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body2_);
    trajectory3_ =
        std::make_unique<Trajectory<EarthMoonBarycentricFrame>>(*body3_);
    Point<Vector<Length, EarthMoonBarycentricFrame>> const
        q1(Vector<Length, EarthMoonBarycentricFrame>({0 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>()}));
    Point<Vector<Length, EarthMoonBarycentricFrame>> const
        q2(Vector<Length, EarthMoonBarycentricFrame>({0 * SIUnit<Length>(),
                                                      4E8 * SIUnit<Length>(),
                                                      0 * SIUnit<Length>()}));
    Point<Vector<Length, EarthMoonBarycentricFrame>> const centre_of_mass =
        Barycentre<Vector<Length, EarthMoonBarycentricFrame>, Mass>(
            {q1, q2}, {body1_->mass(), body2_->mass()});
    Length const semi_major_axis = (q1 - q2).Norm();
    period_ = 2 * π * Sqrt(Pow<3>(semi_major_axis) /
                               (body1_->gravitational_parameter() +
                                body2_->gravitational_parameter()));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const
        v1(Vector<Speed, EarthMoonBarycentricFrame>({
            -2 * π * (q1 - centre_of_mass).Norm() / period_,
            0 * SIUnit<Speed>(),
            0 * SIUnit<Speed>()}));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const
        v2(Vector<Speed, EarthMoonBarycentricFrame>({
            2 * π * (q2 - centre_of_mass).Norm() / period_,
            0 * SIUnit<Speed>(),
            0 * SIUnit<Speed>()}));
    Point<Vector<Speed, EarthMoonBarycentricFrame>> const overall_velocity =
        Barycentre<Vector<Speed, EarthMoonBarycentricFrame>, Mass>(
            {v1, v2}, {body1_->mass(), body2_->mass()});
    trajectory1_->Append(0 * SIUnit<Time>(), {q1, v1});
    trajectory2_->Append(0 * SIUnit<Time>(), {q2, v2});
    system_ = std::make_unique<NBodySystem<EarthMoonBarycentricFrame>>();
  }

  template<typename Scalar, typename Frame>
  std::string ToMathematicaString(Vector<Scalar, Frame> const& vector) {
    R3Element<Scalar> const& coordinates = vector.coordinates();
    std::string result = "{";
    result += quantities::DebugString(coordinates.x);
    result += ",";
    result += quantities::DebugString(coordinates.y);
    result += ",";
    result += quantities::DebugString(coordinates.z);
    result += "}";
    return result;
  }

  template<typename Scalar, typename Frame>
  std::string ToMathematicaString(
      std::vector<Vector<Scalar, Frame>> const& vectors) {
    static std::string const mathematica_line =
        "\n(*****************************************************)\n";
    std::string result = mathematica_line;
    result += "ToExpression[StringReplace[\"\n{";
    std::string separator = "";
    for (const auto& vector : vectors) {
      result += separator;
      result += ToMathematicaString(vector);
      separator = ",\n";
    }
    result +=
        "}\",\n{\" m\"->\"\",\"e\"->\"*^\", \"\\n\"->\"\", \" \"->\"\"}]];";
    result += mathematica_line;
    return result;
  }

  template<typename T1, typename T2>
  std::vector<T2> ValuesOf(std::map<T1, T2> const& m) {
    std::vector<T2> result;
    for (auto const it : m) {
      result.push_back(it.second);
    }
    return result;
  }

  std::unique_ptr<Body> body1_;
  std::unique_ptr<Body> body2_;
  std::unique_ptr<Body> body3_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory1_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory2_;
  std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory3_;
  SPRKIntegrator<Length, Speed> integrator_;
  Time period_;
  std::unique_ptr<NBodySystem<EarthMoonBarycentricFrame>> system_;
};

typedef NBodySystemTest NBodySystemDeathTest;

TEST_F(NBodySystemDeathTest, IntegrateError) {
  EXPECT_DEATH({
    system_->Integrate(integrator_,
                       period_,
                       period_ / 100,
                       1,
                       {trajectory1_.get(),
                        trajectory2_.get(),
                        trajectory1_.get()});
  }, DeathMessage("Multiple trajectories"));
  EXPECT_DEATH({
    std::unique_ptr<Trajectory<EarthMoonBarycentricFrame>> trajectory(
        new Trajectory<EarthMoonBarycentricFrame>(*body2_));
    trajectory->Append(1 * SIUnit<Time>(),
                       {Point<Vector<Length, EarthMoonBarycentricFrame>>(),
                        Point<Vector<Speed, EarthMoonBarycentricFrame>>()});
    system_->Integrate(integrator_,
                       period_,
                       period_ / 100,
                       1,
                       {trajectory1_.get(), trajectory.get()});
  }, DeathMessage("Inconsistent last time"));
}

// The canonical Earth-Moon system, tuned to produce circular orbits.
TEST_F(NBodySystemTest, EarthMoon) {
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> positions;
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory1_.get(), trajectory2_.get()});

  positions = ValuesOf(trajectory1_->Positions());
  EXPECT_THAT(positions.size(), Eq(101));
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(3E-2 * SIUnit<Length>()));

  positions = ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(2 * SIUnit<Length>()));
}

// Same as above, but the trajectories are passed in the reverse order.
TEST_F(NBodySystemTest, MoonEarth) {
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> positions;
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory2_.get(), trajectory1_.get()});

  positions = ValuesOf(trajectory1_->Positions());
  EXPECT_THAT(positions.size(), Eq(101));
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(3E-2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(3E-2 * SIUnit<Length>()));

  positions = ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(Abs(positions[25].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[50].coordinates().x), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[75].coordinates().y), Lt(2 * SIUnit<Length>()));
  EXPECT_THAT(Abs(positions[100].coordinates().x), Lt(2 * SIUnit<Length>()));
}

// The Moon alone.  It moves in straight line.
TEST_F(NBodySystemTest, Moon) {
  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory2_.get()});

  Length const q2 = trajectory2_->last_position().coordinates().y;
  Speed const v2 = trajectory2_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions =
      ValuesOf(trajectory2_->Positions());
  LOG(INFO) << ToMathematicaString(positions);
  EXPECT_THAT(positions.size(), Eq(101));
  EXPECT_THAT(positions[25].coordinates().x, Eq(0.25 * period_ * v2));
  EXPECT_THAT(positions[25].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[50].coordinates().x, Eq(0.50 * period_ * v2));
  EXPECT_THAT(positions[50].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v2, 1));
  EXPECT_THAT(positions[75].coordinates().y, Eq(q2));
  EXPECT_THAT(positions[100].coordinates().x, Eq(1.00 * period_ * v2));
  EXPECT_THAT(positions[100].coordinates().y, Eq(q2));
}

// The Earth and a massless probe 1 billion meters away, with the same velocity,
// and an acceleration which exactly compensates gravitational attraction.  Both
// bodies move in straight lines.
TEST_F(NBodySystemTest, EarthProbe) {
  Length const distance = 1E9 * SIUnit<Length>();
  trajectory3_->Append(trajectory1_->last_time(),
                       {trajectory1_->last_position() +
                            Vector<Length, EarthMoonBarycentricFrame>(
                                {0 * SIUnit<Length>(),
                                 distance,
                                 0 * SIUnit<Length>()}),
                        trajectory1_->last_velocity()});
  trajectory3_->set_intrinsic_acceleration(
      [this, distance](Time const& t) {
    return Vector<Acceleration, EarthMoonBarycentricFrame>(
        {0 * SIUnit<Acceleration>(),
         body1_->gravitational_parameter() / (distance * distance),
         0 * SIUnit<Acceleration>()});});

  system_->Integrate(integrator_,
                     period_,
                     period_ / 100,
                     1,
                     {trajectory1_.get(), trajectory3_.get()});

  Length const q1 = trajectory1_->last_position().coordinates().y;
  Speed const v1 = trajectory1_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions1 =
      ValuesOf(trajectory1_->Positions());
  LOG(INFO) << ToMathematicaString(positions1);
  EXPECT_THAT(positions1.size(), Eq(101));
  EXPECT_THAT(positions1[25].coordinates().x,
              AlmostEquals(0.25 * period_ * v1, 1));
  EXPECT_THAT(positions1[25].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[50].coordinates().x,
              AlmostEquals(0.50 * period_ * v1, 1));
  EXPECT_THAT(positions1[50].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v1, 1));
  EXPECT_THAT(positions1[75].coordinates().y, Eq(q1));
  EXPECT_THAT(positions1[100].coordinates().x,
              AlmostEquals(1.00 * period_ * v1, 1));
  EXPECT_THAT(positions1[100].coordinates().y, Eq(q1));

  Length const q3 = trajectory3_->last_position().coordinates().y;
  Speed const v3 = trajectory3_->last_velocity().coordinates().x;
  std::vector<Vector<Length, EarthMoonBarycentricFrame>> const positions3 =
      ValuesOf(trajectory3_->Positions());
  LOG(INFO) << ToMathematicaString(positions3);
  EXPECT_THAT(positions3.size(), Eq(101));
  EXPECT_THAT(positions3[25].coordinates().x,
              AlmostEquals(0.25 * period_ * v3, 1));
  EXPECT_THAT(positions3[25].coordinates().y, AlmostEquals(q3, 2));
  EXPECT_THAT(positions3[50].coordinates().x,
              AlmostEquals(0.50 * period_ * v3, 1));
  EXPECT_THAT(positions3[50].coordinates().y, AlmostEquals(q3, 2));
  EXPECT_THAT(positions3[75].coordinates().x,
              AlmostEquals(0.75 * period_ * v3, 1));
  EXPECT_THAT(positions3[75].coordinates().y, AlmostEquals(q3, 1));
  EXPECT_THAT(positions3[100].coordinates().x,
              AlmostEquals(1.00 * period_ * v3, 1));
  EXPECT_THAT(positions3[100].coordinates().y, Eq(q3));
}

}  // namespace physics
}  // namespace principia
<|endoftext|>


<commit_before>/*
 * LocationController.cpp
 *
 *  Created on: 29-07-2014
 *      Author: szsz
 */

#include <log4cxx/propertyconfigurator.h>
#include <boost/program_options.hpp>
#include <string>
#include <cmath>
#include <stdio.h>

#include "LocationCommon.h"
#include "LocationController.h"

using namespace std;
using namespace boost;
using namespace boost::interprocess;
using namespace boost::program_options;
using namespace log4cxx;
using namespace amber;

LoggerPtr LocationController::_logger(Logger::getLogger("LocationController"));

LocationController::LocationController(int pipeInFd, int pipeOutFd, const char *confFilename)
{
	LOG4CXX_INFO(_logger, "LocationController");

	parseConfigurationFile(confFilename);

	_amberPipes = new AmberPipes(this, pipeInFd, pipeOutFd);

	//char* mapPath = "/home/szsz/git/Lokalizacja/Lokalizacja/Debug/labNowe3.roson";
    //char* mapPath = "/home//panda//Lokalizacja//Lokalizacja//labNowe3.roson";

	char* mapPath = new char[_configuration->mapPath.length()];
	strcpy(mapPath,_configuration->mapPath.c_str());

	unsigned int numberParticles = _configuration->NumberParticles;
	double epsilon = _configuration->epsilon;
	int generation = _configuration->generation;
	unsigned int ilosc_losowanych_nowych_czastek = _configuration->ilosc_losowanych_nowych_czastek;
	unsigned int przlieczenie_dla_pomiaru_skanera = _configuration->przlieczenie_dla_pomiaru_skanera;

	char* sIPPart = new char[_configuration->sIPPart.length()];
	strcpy(sIPPart,_configuration->sIPPart.c_str());

	lok = new Location(_logger,mapPath,numberParticles,epsilon,generation,ilosc_losowanych_nowych_czastek,przlieczenie_dla_pomiaru_skanera,sIPPart);

	locationThread = new boost::thread(boost::bind(&LocationController::locationMathod, this));
}

LocationController::~LocationController() {
    LOG4CXX_INFO(_logger, "Stopping controller.");

    delete _amberPipes;
    delete lok;
}

void LocationController::handleDataMsg(amber::DriverHdr *driverHdr, amber::DriverMsg *driverMsg)
{
    if (_logger->isDebugEnabled()) {
        LOG4CXX_DEBUG(_logger, "Message came");
    }

    int clientId = driverHdr->clientids_size() > 0 ? driverHdr->clientids(0) : 0;

    // DataRequest
    if (driverMsg->HasExtension(location_proto::get_location))
	{
        if (!driverMsg->has_synnum()) {
            LOG4CXX_WARN(_logger, "Got CurrentLocationRequest, but syn num not set. Ignoring.");
            return;
        }

        if (driverMsg->GetExtension(location_proto::get_location))
        {
            handleCurrentLocationRequest(clientId, driverMsg->synnum());
        }
    }
}

void LocationController::handleClientDiedMsg(int clientID) {
    LOG4CXX_INFO(_logger, "Client " << clientID << " died");

}

void LocationController::operator()() {
    _amberPipes->operator ()();
}

amber::DriverMsg *LocationController::buildCurrentLocationMsg()
{
LOG4CXX_INFO(_logger, "build current location msg");

 	amber::DriverMsg *message = new amber::DriverMsg();
    message->set_type(amber::DriverMsg_MsgType_DATA);

    location_proto::Location *currentLocation = message->MutableExtension(location_proto::currentLocation);

    location_mutex.lock();

    currentLocation->set_x(lok->Pos_X);
    currentLocation->set_y(lok->Pos_Y);
    currentLocation->set_p(lok->Prop);
    currentLocation->set_alfa(lok->Pos_Alfa);
    currentLocation->set_timestamp(lok->timeStamp);

    location_mutex.unlock();

    return message;
}

void LocationController::sendCurrentLocationMsg(int receiver, int ackNum)
{
    if (_logger->isDebugEnabled()) {
        LOG4CXX_DEBUG(_logger, "Sending currentLocationRequest message");
    }

    amber::DriverMsg *currentLocationMsg = buildCurrentLocationMsg();
    currentLocationMsg->set_acknum(ackNum);
    amber::DriverHdr *header = new amber::DriverHdr();
    header->add_clientids(receiver);

    _amberPipes->writeMsgToPipe(header, currentLocationMsg);

    delete currentLocationMsg;
    delete header;
}

void LocationController::handleCurrentLocationRequest(int sender, int synNum)
{
    if (_logger->isDebugEnabled()) {
        LOG4CXX_DEBUG(_logger, "Handling currentLocationRequest message");
    }

    sendCurrentLocationMsg(sender, synNum);
}

void LocationController::parseConfigurationFile(const char *filename) {
    LOG4CXX_INFO(_logger, "Parsing configuration file: " << filename);

    _configuration = new LocationConfiguration();

   // unsigned int front_rc_address;
    //unsigned int rear_rc_address;

    options_description desc("Location options");
    desc.add_options()
            ("location.mapPath", value<string>(&_configuration->mapPath)->default_value("."))
            ("location.NumberParticles", value<unsigned int>(&_configuration->NumberParticles)->default_value(20))
            ("location.epsilon", value<double>(&_configuration->epsilon)->default_value(0.8))
            ("location.generation", value<int>(&_configuration->generation)->default_value(1))
            ("location.ilosc_losowanych_nowych_czastek", value<unsigned int>(&_configuration->ilosc_losowanych_nowych_czastek)->default_value(20))
            ("location.przlieczenie_dla_pomiaru_skanera", value<unsigned int>(&_configuration->przlieczenie_dla_pomiaru_skanera)->default_value(12))
            ("location.RandomWalkMaxDistance", value<double>(&_configuration->RandomWalkMaxDistance)->default_value(0.7))
            ("location.StandardDeviation", value<double>(&_configuration->StandardDeviation)->default_value(0.3))
            ("location.sIPPart", value<string>(&_configuration->sIPPart)->default_value("127.0.0.1"));

            /*("roboclaw.motors_max_qpps", value<unsigned int>(&_configuration->motors_max_qpps)->default_value(13800))
            ("roboclaw.motors_p_const", value<unsigned int>(&_configuration->motors_p_const)->default_value(65536))
            ("roboclaw.motors_i_const", value<unsigned int>(&_configuration->motors_i_const)->default_value(32768))
            ("roboclaw.motors_d_const", value<unsigned int>(&_configuration->motors_d_const)->default_value(16384))
            ("roboclaw.pulses_per_revolution", value<unsigned int>(&_configuration->pulses_per_revolution)->default_value(1865))
            ("roboclaw.wheel_radius", value<unsigned int>(&_configuration->wheel_radius)->default_value(60))
            ("roboclaw.battery_monitor_interval", value<unsigned int>(&_configuration->battery_monitor_interval)->default_value(0))
            ("roboclaw.error_monitor_interval", value<unsigned int>(&_configuration->error_monitor_interval)->default_value(0))
            ("roboclaw.temperature_monitor_interval", value<unsigned int>(&_configuration->temperature_monitor_interval)->default_value(0))
            ("roboclaw.temperature_critical", value<__u16>(&_configuration->temperature_critical)->default_value(70))
            ("roboclaw.temperature_drop", value<__u16>(&_configuration->temperature_drop)->default_value(60))
            ("roboclaw.critical_read_repeats", value<unsigned int>(&_configuration->critical_read_repeats)->default_value(0));

*/
    variables_map vm;

    try {
        store(parse_config_file<char>(filename, desc), vm);
        notify(vm);

        //_configuration->front_rc_address = (__u8) front_rc_address;
        //_configuration->rear_rc_address = (__u8) rear_rc_address;

    } catch (std::exception& e) {
        LOG4CXX_ERROR(_logger, "Error in parsing configuration file: " << e.what());
        exit(1);
    }
}

void LocationController::locationMathod()
{
    LOG4CXX_INFO(_logger, "Location Method -> RunLocation");

	lok->RunLocation();
}

int main(int argc, char *argv[]) {

   if (argc < 3) {
       return 1;
    }

    const char *confFile =  argv[1];
    const char *logConfFile = argv[2];

    PropertyConfigurator::configure(logConfFile);

    // STDIN_FD = 0, STDOUT_FD = 1
    // pipe_in_fd = 0, pipe_out_fd = 1
    LoggerPtr logger(Logger::getLogger("main"));

    LOG4CXX_INFO(logger, "-------------");
    LOG4CXX_INFO(logger, "Creating controller, config_file: " << argv[1] << ", log_config_file: " << argv[2]);

    LOG4CXX_INFO(logger, "jestem w main 3 ");

    LocationController controller(0, 1, confFile);
    controller();
}
<commit_msg>obsługa zmiany mapy<commit_after>/*
 * LocationController.cpp
 *
 *  Created on: 29-07-2014
 *      Author: szsz
 */

#include <log4cxx/propertyconfigurator.h>
#include <boost/program_options.hpp>
#include <string>
#include <cmath>
#include <stdio.h>

#include "LocationCommon.h"
#include "LocationController.h"

using namespace std;
using namespace boost;
using namespace boost::interprocess;
using namespace boost::program_options;
using namespace log4cxx;
using namespace amber;

LoggerPtr LocationController::_logger(Logger::getLogger("LocationController"));

LocationController::LocationController(int pipeInFd, int pipeOutFd, const char *confFilename)
{
	LOG4CXX_INFO(_logger, "LocationController");

	parseConfigurationFile(confFilename);

	_amberPipes = new AmberPipes(this, pipeInFd, pipeOutFd);

	initLocation();
}

void LocationController::initLocation()
{
	//char* mapPath = "/home/szsz/git/Lokalizacja/Lokalizacja/Debug/labNowe3.roson";
	//char* mapPath = "/home//panda//Lokalizacja//Lokalizacja//labNowe3.roson";

	char* mapPath = new char[_configuration->mapPath.length()];
	strcpy(mapPath,_configuration->mapPath.c_str());

	initLocation(mapPath);
}

void LocationController::initLocation(char* mapPath)
{
	unsigned int numberParticles = _configuration->NumberParticles;
	double epsilon = _configuration->epsilon;
	int generation = _configuration->generation;
	unsigned int ilosc_losowanych_nowych_czastek = _configuration->ilosc_losowanych_nowych_czastek;
	unsigned int przlieczenie_dla_pomiaru_skanera = _configuration->przlieczenie_dla_pomiaru_skanera;

	char* sIPPart = new char[_configuration->sIPPart.length()];
	strcpy(sIPPart,_configuration->sIPPart.c_str());

	lok = new Location(_logger,mapPath,numberParticles,epsilon,generation,ilosc_losowanych_nowych_czastek,przlieczenie_dla_pomiaru_skanera,sIPPart);

	locationThread = new boost::thread(boost::bind(&LocationController::locationMathod, this));
}

LocationController::~LocationController() {
	LOG4CXX_INFO(_logger, "Stopping controller.");

	delete _amberPipes;
	delete lok;
}

void LocationController::handleDataMsg(amber::DriverHdr *driverHdr, amber::DriverMsg *driverMsg)
{
	if (_logger->isDebugEnabled()) {
		LOG4CXX_DEBUG(_logger, "Message came");
	}

	int clientId = driverHdr->clientids_size() > 0 ? driverHdr->clientids(0) : 0;

	// DataRequest
	if (driverMsg->HasExtension(location_proto::get_location))
	{
		if (!driverMsg->has_synnum()) {
			LOG4CXX_WARN(_logger, "Got CurrentLocationRequest, but syn num not set. Ignoring.");
			return;
		}

		if (driverMsg->GetExtension(location_proto::get_location))
		{
			handleCurrentLocationRequest(clientId, driverMsg->synnum());
		}
	}
    else if(driverMsg->HasExtension(location_proto::upload_map))
    {
    	 if (!driverMsg->has_synnum()) {
    	            LOG4CXX_WARN(_logger, "Got CurrentUploadMapRequest, but syn num not set. Ignoring.");
    	            return;
    	 }
    	 else
    	 {
    		 ::std::string ss = driverMsg->GetExtension(location_proto::upload_map);
    	 }
    }
}

void LocationController::handleClientDiedMsg(int clientID) {
	LOG4CXX_INFO(_logger, "Client " << clientID << " died");

}

void LocationController::operator()() {
	_amberPipes->operator ()();
}

amber::DriverMsg *LocationController::buildCurrentLocationMsg()
{
	LOG4CXX_INFO(_logger, "build current location msg");

	amber::DriverMsg *message = new amber::DriverMsg();
	message->set_type(amber::DriverMsg_MsgType_DATA);

	location_proto::Location *currentLocation = message->MutableExtension(location_proto::currentLocation);

	location_mutex.lock();

	currentLocation->set_x(lok->Pos_X);
	currentLocation->set_y(lok->Pos_Y);
	currentLocation->set_p(lok->Prop);
	currentLocation->set_alfa(lok->Pos_Alfa);
	currentLocation->set_timestamp(lok->timeStamp);

	location_mutex.unlock();

	return message;
}

void LocationController::sendCurrentLocationMsg(int receiver, int ackNum)
{
	if (_logger->isDebugEnabled()) {
		LOG4CXX_DEBUG(_logger, "Sending currentLocationRequest message");
	}

	amber::DriverMsg *currentLocationMsg = buildCurrentLocationMsg();
	currentLocationMsg->set_acknum(ackNum);
	amber::DriverHdr *header = new amber::DriverHdr();
	header->add_clientids(receiver);

	_amberPipes->writeMsgToPipe(header, currentLocationMsg);

	delete currentLocationMsg;
	delete header;
}

void LocationController::handleCurrentLocationRequest(int sender, int synNum)
{
	if (_logger->isDebugEnabled()) {
		LOG4CXX_DEBUG(_logger, "Handling currentLocationRequest message");
	}

	sendCurrentLocationMsg(sender, synNum);
}

void LocationController::handleCurrentUploadMapRequest(std::string sMap)
{
	printf("%s\n",sMap.c_str());
}

void LocationController::parseConfigurationFile(const char *filename) {
	LOG4CXX_INFO(_logger, "Parsing configuration file: " << filename);

	_configuration = new LocationConfiguration();

	// unsigned int front_rc_address;
	//unsigned int rear_rc_address;

    options_description desc("Location options");
    desc.add_options()
            ("location.mapPath", value<string>(&_configuration->mapPath)->default_value("."))
            ("location.NumberParticles", value<unsigned int>(&_configuration->NumberParticles)->default_value(20))
            ("location.epsilon", value<double>(&_configuration->epsilon)->default_value(0.8))
            ("location.generation", value<int>(&_configuration->generation)->default_value(1))
            ("location.ilosc_losowanych_nowych_czastek", value<unsigned int>(&_configuration->ilosc_losowanych_nowych_czastek)->default_value(20))
            ("location.przlieczenie_dla_pomiaru_skanera", value<unsigned int>(&_configuration->przlieczenie_dla_pomiaru_skanera)->default_value(12))
            ("location.RandomWalkMaxDistance", value<double>(&_configuration->RandomWalkMaxDistance)->default_value(0.7))
            ("location.StandardDeviation", value<double>(&_configuration->StandardDeviation)->default_value(0.3))
            ("location.sIPPart", value<string>(&_configuration->sIPPart)->default_value("127.0.0.1"))
            ("location.uploadMapPath",  value<string>(&_configuration->uploadMapPath)->default_value("."));

	/*("roboclaw.motors_max_qpps", value<unsigned int>(&_configuration->motors_max_qpps)->default_value(13800))
            ("roboclaw.motors_p_const", value<unsigned int>(&_configuration->motors_p_const)->default_value(65536))
            ("roboclaw.motors_i_const", value<unsigned int>(&_configuration->motors_i_const)->default_value(32768))
            ("roboclaw.motors_d_const", value<unsigned int>(&_configuration->motors_d_const)->default_value(16384))
            ("roboclaw.pulses_per_revolution", value<unsigned int>(&_configuration->pulses_per_revolution)->default_value(1865))
            ("roboclaw.wheel_radius", value<unsigned int>(&_configuration->wheel_radius)->default_value(60))
            ("roboclaw.battery_monitor_interval", value<unsigned int>(&_configuration->battery_monitor_interval)->default_value(0))
            ("roboclaw.error_monitor_interval", value<unsigned int>(&_configuration->error_monitor_interval)->default_value(0))
            ("roboclaw.temperature_monitor_interval", value<unsigned int>(&_configuration->temperature_monitor_interval)->default_value(0))
            ("roboclaw.temperature_critical", value<__u16>(&_configuration->temperature_critical)->default_value(70))
            ("roboclaw.temperature_drop", value<__u16>(&_configuration->temperature_drop)->default_value(60))
            ("roboclaw.critical_read_repeats", value<unsigned int>(&_configuration->critical_read_repeats)->default_value(0));

	 */
	variables_map vm;

	try {
		store(parse_config_file<char>(filename, desc), vm);
		notify(vm);

		//_configuration->front_rc_address = (__u8) front_rc_address;
		//_configuration->rear_rc_address = (__u8) rear_rc_address;

	} catch (std::exception& e) {
		LOG4CXX_ERROR(_logger, "Error in parsing configuration file: " << e.what());
		exit(1);
	}
}

void LocationController::locationMathod()
{
	LOG4CXX_INFO(_logger, "Location Method -> RunLocation");

	lok->RunLocation();
}

int main(int argc, char *argv[]) {

	if (argc < 3) {
		return 1;
	}

	const char *confFile =  argv[1];
	const char *logConfFile = argv[2];

	PropertyConfigurator::configure(logConfFile);

	// STDIN_FD = 0, STDOUT_FD = 1
	// pipe_in_fd = 0, pipe_out_fd = 1
	LoggerPtr logger(Logger::getLogger("main"));

	LOG4CXX_INFO(logger, "-------------");
	LOG4CXX_INFO(logger, "Creating controller, config_file: " << argv[1] << ", log_config_file: " << argv[2]);

	LocationController controller(0, 1, confFile);
	controller();

}
<|endoftext|>