<commit_before>#include <gtest/gtest.h>

#include <atomic>
#include <cstring>
#include <limits>

#include <robot_impl.h>

#include "helpers.h"
#include "mock_server.h"

using namespace std::chrono_literals;
using namespace research_interface::robot;

using franka::ControlException;
using franka::NetworkException;

class Robot : public ::franka::Robot {
 public:
  using ::franka::Robot::Impl;
};

TEST(RobotImpl, CanReceiveRobotState) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  RobotState sent_robot_state;
  server.sendRandomState<RobotState>([](auto s) { randomRobotState(s); }, &sent_robot_state)
      .spinOnce();

  auto received_robot_state = robot.update();
  testRobotStatesAreEqual(sent_robot_state, received_robot_state);
}

TEST(RobotImpl, CanReceiveReorderedRobotStatesCorrectly) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  server.onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 2; })
      .spinOnce();

  auto received_robot_state = robot.update();
  EXPECT_EQ(2u, received_robot_state.time.ms());

  server.onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 1; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 4; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 2; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 3; })
      .spinOnce();

  received_robot_state = robot.update();
  EXPECT_EQ(4u, received_robot_state.time.ms());
}

TEST(RobotImpl, ThrowsTimeoutIfNoRobotStateArrives) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort, 200ms));

  EXPECT_THROW(robot.update(), NetworkException);
}

TEST(RobotImpl, StopsIfControlConnectionClosed) {
  std::unique_ptr<Robot::Impl> robot;
  {
    RobotMockServer server;

    robot.reset(
        new Robot::Impl(std::make_unique<franka::Network>("127.0.0.1", kCommandPort, 200ms)));

    RobotState robot_state;
    server.sendRandomState<RobotState>([](auto s) { randomRobotState(s); }, &robot_state)
        .spinOnce();

    testRobotStatesAreEqual(franka::convertRobotState(robot_state), robot->update());
  }

  EXPECT_THROW(robot->update(), NetworkException);
}

TEST(RobotImpl, CanStartMotion) {
  RobotMockServer server;
  Move::Deviation maximum_path_deviation{0, 1, 2};
  Move::Deviation maximum_goal_pose_deviation{3, 4, 5};

  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .waitForCommand<Move>([=](const Move::Request& request) {
        EXPECT_EQ(Move::MotionGeneratorMode::kJointPosition, request.motion_generator_mode);
        EXPECT_EQ(Move::ControllerMode::kJointPosition, request.controller_mode);
        EXPECT_EQ(maximum_path_deviation, request.maximum_path_deviation);
        EXPECT_EQ(maximum_goal_pose_deviation, request.maximum_goal_pose_deviation);
        return Move::Response(Move::Status::kMotionStarted);
      })
      .spinOnce();

  EXPECT_NO_THROW(robot.startMotion(Move::ControllerMode::kJointPosition,
                                    Move::MotionGeneratorMode::kJointPosition,
                                    maximum_path_deviation, maximum_goal_pose_deviation));
  EXPECT_TRUE(robot.motionGeneratorRunning());
  EXPECT_FALSE(robot.controllerRunning());

  // Test exceptions if wrong update() overload is called
  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce();
  EXPECT_NO_THROW(robot.update(nullptr, nullptr));

  ControllerCommand control_command{};
  MotionGeneratorCommand motion_command{};
  EXPECT_THROW(robot.update(nullptr, &control_command), ControlException);
  EXPECT_THROW(robot.update(&motion_command, &control_command), ControlException);

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .onReceiveRobotCommand([](const RobotCommand&) {})
      .spinOnce();
  EXPECT_NO_THROW(robot.update(&motion_command, nullptr));
}

TEST(RobotImpl, ReadsCorrectTimeDuringMotion) {
  RobotMockServer server;
  Move::Deviation maximum_path_deviation{0, 1, 2};
  Move::Deviation maximum_goal_pose_deviation{3, 4, 5};

  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .waitForCommand<Move>([=](const Move::Request& request) {
        EXPECT_EQ(Move::MotionGeneratorMode::kJointPosition, request.motion_generator_mode);
        EXPECT_EQ(Move::ControllerMode::kJointPosition, request.controller_mode);
        EXPECT_EQ(maximum_path_deviation, request.maximum_path_deviation);
        EXPECT_EQ(maximum_goal_pose_deviation, request.maximum_goal_pose_deviation);
        return Move::Response(Move::Status::kMotionStarted);
      })
      .spinOnce();

  robot.startMotion(Move::ControllerMode::kJointPosition, Move::MotionGeneratorMode::kJointPosition,
                    maximum_path_deviation, maximum_goal_pose_deviation);

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce();

  auto robot_state = robot.update();
  EXPECT_EQ(server.sequenceNumber(), robot_state.time.ms());

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .onReceiveRobotCommand([](const RobotCommand&) {})
      .spinOnce();

  MotionGeneratorCommand motion_command{};
  robot_state = robot.update(&motion_command, nullptr);
  EXPECT_EQ(server.sequenceNumber(), robot_state.time.ms());
}
<commit_msg>Remove now-redundant test.<commit_after>#include <gtest/gtest.h>

#include <atomic>
#include <cstring>
#include <limits>

#include <robot_impl.h>

#include "helpers.h"
#include "mock_server.h"

using namespace std::chrono_literals;
using namespace research_interface::robot;

using franka::ControlException;
using franka::NetworkException;

class Robot : public ::franka::Robot {
 public:
  using ::franka::Robot::Impl;
};

TEST(RobotImpl, CanReceiveRobotState) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  RobotState sent_robot_state;
  server.sendRandomState<RobotState>([](auto s) { randomRobotState(s); }, &sent_robot_state)
      .spinOnce();

  auto received_robot_state = robot.update();
  testRobotStatesAreEqual(sent_robot_state, received_robot_state);
}

TEST(RobotImpl, CanReceiveReorderedRobotStatesCorrectly) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  server.onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 2; })
      .spinOnce();

  auto received_robot_state = robot.update();
  EXPECT_EQ(2u, received_robot_state.time.ms());

  server.onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 1; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 4; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 2; })
      .onSendUDP<RobotState>([](RobotState& robot_state) { robot_state.message_id = 3; })
      .spinOnce();

  received_robot_state = robot.update();
  EXPECT_EQ(4u, received_robot_state.time.ms());
}

TEST(RobotImpl, ThrowsTimeoutIfNoRobotStateArrives) {
  RobotMockServer server;
  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort, 200ms));

  EXPECT_THROW(robot.update(), NetworkException);
}

TEST(RobotImpl, StopsIfControlConnectionClosed) {
  std::unique_ptr<Robot::Impl> robot;
  {
    RobotMockServer server;

    robot.reset(
        new Robot::Impl(std::make_unique<franka::Network>("127.0.0.1", kCommandPort, 200ms)));

    RobotState robot_state;
    server.sendRandomState<RobotState>([](auto s) { randomRobotState(s); }, &robot_state)
        .spinOnce();

    testRobotStatesAreEqual(franka::convertRobotState(robot_state), robot->update());
  }

  EXPECT_THROW(robot->update(), NetworkException);
}

TEST(RobotImpl, CanStartMotion) {
  RobotMockServer server;
  Move::Deviation maximum_path_deviation{0, 1, 2};
  Move::Deviation maximum_goal_pose_deviation{3, 4, 5};

  Robot::Impl robot(std::make_unique<franka::Network>("127.0.0.1", kCommandPort));

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .waitForCommand<Move>([=](const Move::Request& request) {
        EXPECT_EQ(Move::MotionGeneratorMode::kJointPosition, request.motion_generator_mode);
        EXPECT_EQ(Move::ControllerMode::kJointPosition, request.controller_mode);
        EXPECT_EQ(maximum_path_deviation, request.maximum_path_deviation);
        EXPECT_EQ(maximum_goal_pose_deviation, request.maximum_goal_pose_deviation);
        return Move::Response(Move::Status::kMotionStarted);
      })
      .spinOnce();

  EXPECT_NO_THROW(robot.startMotion(Move::ControllerMode::kJointPosition,
                                    Move::MotionGeneratorMode::kJointPosition,
                                    maximum_path_deviation, maximum_goal_pose_deviation));
  EXPECT_TRUE(robot.motionGeneratorRunning());
  EXPECT_FALSE(robot.controllerRunning());

  // Test exceptions if wrong update() overload is called
  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce();
  EXPECT_NO_THROW(robot.update(nullptr, nullptr));

  ControllerCommand control_command{};
  MotionGeneratorCommand motion_command{};
  EXPECT_THROW(robot.update(nullptr, &control_command), ControlException);
  EXPECT_THROW(robot.update(&motion_command, &control_command), ControlException);

  server
      .onSendUDP<RobotState>([](RobotState& robot_state) {
        robot_state.motion_generator_mode = MotionGeneratorMode::kJointPosition;
        robot_state.controller_mode = ControllerMode::kJointPosition;
        robot_state.robot_mode = RobotMode::kMove;
      })
      .spinOnce()
      .onReceiveRobotCommand([](const RobotCommand&) {})
      .spinOnce();
  EXPECT_NO_THROW(robot.update(&motion_command, nullptr));
}

{'fraction_non_alphanumeric': 0.1260032102728732, 'fraction_numerical': 0.00882825040128411, 'mean_word_length': 4.990865384615384, 'pattern_counts': {'":': 0, '<': 54, '<?xml version=': 0, '>': 58, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 11, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2068963', 'n_tokens_mistral': 4187, 'n_tokens_neox': 4047, 'n_words': 516}