SimulationBridge Class Reference

#include <SimulationBridge.h>

Collaboration diagram for SimulationBridge:

Public Member Functions
	SimulationBridge (RobotType robot, RobotController *robot_ctrl)
void	run ()
void	handleControlParameters ()
void	runRobotControl ()
	~SimulationBridge ()
void	run_sbus ()

Private Attributes
PeriodicTaskManager	taskManager
bool	_firstControllerRun = true
PeriodicTaskManager *	_fakeTaskManager = nullptr
RobotType	_robot
RobotRunner *	_robotRunner = nullptr
SimulatorMode	_simMode
SharedMemoryObject< SimulatorSyncronizedMessage >	_sharedMemory
RobotControlParameters	_robotParams
ControlParameters *	_userParams = nullptr
u64	_iterations = 0
std::thread *	sbus_thread

Detailed Description

Definition at line 18 of file SimulationBridge.h.

Constructor & Destructor Documentation

SimulationBridge::SimulationBridge ( RobotType  robot, RobotController *  robot_ctrl  )

inlineexplicit

Definition at line 20 of file SimulationBridge.h.

References _fakeTaskManager, _robotRunner, _userParams, RobotController::getUserControlParameters(), handleControlParameters(), run(), and runRobotControl().

                                                                          : 
    _robot(robot) {
     _fakeTaskManager = new PeriodicTaskManager;
    _robotRunner = new RobotRunner(robot_ctrl, _fakeTaskManager, 0, "robot-task");
    _userParams = robot_ctrl->getUserControlParameters();

 }

Here is the call graph for this function:

SimulationBridge::~SimulationBridge ( )

inline

Definition at line 30 of file SimulationBridge.h.

References _fakeTaskManager, _robotRunner, and run_sbus().

                      {
    delete _fakeTaskManager;
    delete _robotRunner;
  }

Here is the call graph for this function:

Member Function Documentation

void SimulationBridge::handleControlParameters

(

)

This function handles a a control parameter message from the simulator

Definition at line 71 of file SimulationBridge.cpp.

References ControlParameter::_kind, ControlParameterCollection::_map, _robotParams, _sharedMemory, _userParams, ControlParameters::collection, controlParameterValueKindToString(), ControlParameter::get(), GET_ROBOT_PARAM_BY_NAME, ControlParameterCollection::lookup(), ControlParameterRequest::name, ControlParameterResponse::name, ControlParameterResponse::nParameters, ControlParameterRequest::parameterKind, ControlParameterResponse::parameterKind, ControlParameterRequest::requestKind, ControlParameterResponse::requestKind, ControlParameterRequest::requestNumber, ControlParameterResponse::requestNumber, ControlParameter::set(), SET_ROBOT_PARAM_BY_NAME, SET_USER_PARAM_BY_NAME, ControlParameterResponse::toString(), ControlParameterRequest::value, and ControlParameterResponse::value.

                                               {
  ControlParameterRequest& request =
      _sharedMemory().simToRobot.controlParameterRequest;
  ControlParameterResponse& response =
      _sharedMemory().robotToSim.controlParameterResponse;
  if (request.requestNumber <= response.requestNumber) {
    // nothing to do!
    printf(
        "[SimulationBridge] Warning: the simulator has run a ControlParameter "
        "iteration, but there is no new request!\n");
    return;
  }

  // sanity check
  u64 nRequests = request.requestNumber - response.requestNumber;
  assert(nRequests == 1);

  response.nParameters = _robotParams.collection._map
                             .size();  // todo don't do this every single time?

  switch (request.requestKind) {
    case ControlParameterRequestKind::SET_ROBOT_PARAM_BY_NAME: {
      std::string name(request.name);
      ControlParameter& param = _robotParams.collection.lookup(name);

      // type check
      if (param._kind != request.parameterKind) {
        throw std::runtime_error(
            "type mismatch for parameter " + name + ", robot thinks it is " +
            controlParameterValueKindToString(param._kind) +
            " but received a command to set it to " +
            controlParameterValueKindToString(request.parameterKind));
      }

      // do the actual set
      param.set(request.value, request.parameterKind);

      // respond:
      response.requestNumber =
          request.requestNumber;  // acknowledge that the set has happened
      response.parameterKind =
          request.parameterKind;       // just for debugging print statements
      response.value = request.value;  // just for debugging print statements
      strcpy(response.name,
             name.c_str());  // just for debugging print statements
      response.requestKind = request.requestKind;

      printf("%s\n", response.toString().c_str());

    } break;

    case ControlParameterRequestKind::SET_USER_PARAM_BY_NAME: {
      std::string name(request.name);
      if(!_userParams) {
        printf("[Simulation Bridge] Warning: tried to set user parameter, but the robot does not have any!\n");
      } else {
        ControlParameter& param = _userParams->collection.lookup(name);

        // type check
        if (param._kind != request.parameterKind) {
          throw std::runtime_error(
              "type mismatch for parameter " + name + ", robot thinks it is " +
              controlParameterValueKindToString(param._kind) +
              " but received a command to set it to " +
              controlParameterValueKindToString(request.parameterKind));
        }

        // do the actual set
        param.set(request.value, request.parameterKind);
      }

      // respond:
      response.requestNumber =
          request.requestNumber;  // acknowledge that the set has happened
      response.parameterKind =
          request.parameterKind;       // just for debugging print statements
      response.value = request.value;  // just for debugging print statements
      strcpy(response.name,
             name.c_str());  // just for debugging print statements
      response.requestKind = request.requestKind;

      printf("%s\n", response.toString().c_str());

    } break;

    case ControlParameterRequestKind::GET_ROBOT_PARAM_BY_NAME: {
      std::string name(request.name);
      ControlParameter& param = _robotParams.collection.lookup(name);

      // type check
      if (param._kind != request.parameterKind) {
        throw std::runtime_error(
            "type mismatch for parameter " + name + ", robot thinks it is " +
            controlParameterValueKindToString(param._kind) +
            " but received a command to set it to " +
            controlParameterValueKindToString(request.parameterKind));
      }

      // respond
      response.value = param.get(request.parameterKind);
      response.requestNumber = request.requestNumber;  // acknowledge
      response.parameterKind =
          request.parameterKind;  // just for debugging print statements
      strcpy(response.name,
             name.c_str());  // just for debugging print statements
      response.requestKind =
          request.requestKind;  // just for debugging print statements

      printf("%s\n", response.toString().c_str());
    } break;
    default:
      throw std::runtime_error("unhandled get/set");
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

void SimulationBridge::run

(

)

Definition at line 13 of file SimulationBridge.cpp.

References _iterations, _robot, _sharedMemory, _simMode, SharedMemoryObject< T >::attach(), DEVELOPMENT_SIMULATOR_SHARED_MEMORY_NAME, DO_NOTHING, EXIT, handleControlParameters(), RUN_CONTROL_PARAMETERS, RUN_CONTROLLER, and runRobotControl().

                           {
  // init shared memory:
  _sharedMemory.attach(DEVELOPMENT_SIMULATOR_SHARED_MEMORY_NAME);
  _sharedMemory().init();

  // init Quadruped Controller

  printf("[Simulation Driver] Starting main loop...\n");
  bool firstRun = true;
  for (;;) {
    // wait for our turn to access the shared memory
    // on the first loop, this gives the simulator a chance to put stuff in
    // shared memory before we start
    _sharedMemory().waitForSimulator();

    if (firstRun) {
      firstRun = false;
      // check that the robot type is correct:
      if (_robot != _sharedMemory().simToRobot.robotType) {
        printf(
            "simulator and simulatorDriver don't agree on which robot we are "
            "simulating (robot %d, sim %d)\n",
            (int)_robot, (int)_sharedMemory().simToRobot.robotType);
        throw std::runtime_error("robot mismatch!");
      }
    }

    // the simulator tells us which mode to run in
    _simMode = _sharedMemory().simToRobot.mode;
    switch (_simMode) {
      case SimulatorMode::RUN_CONTROL_PARAMETERS:  // there is a new control
                                                   // parameter request
        handleControlParameters();
        break;
      case SimulatorMode::RUN_CONTROLLER:  // the simulator is ready for the
                                           // next robot controller run
        _iterations++;
        runRobotControl();
        break;
      case SimulatorMode::DO_NOTHING:  // the simulator is just checking to see
                                       // if we are alive yet
        break;
      case SimulatorMode::EXIT:  // the simulator is done with us
        printf("[Simulation Driver] Transitioned to exit mode\n");
        return;
        break;
      default:
        throw std::runtime_error("unknown simulator mode");
    }

    // tell the simulator we are done
    _sharedMemory().robotIsDone();
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

void SimulationBridge::run_sbus

(

)

Definition at line 242 of file SimulationBridge.cpp.

References init_sbus(), receive_sbus(), and sbus_packet_complete().

                                {
  printf("[run_sbus] starting...\n");
  int port = init_sbus(true);  // Simulation
  while (true) {
    if (port > 0) {
      int x = receive_sbus(port);
      if (x) {
        sbus_packet_complete();
      }
    }
    usleep(5000);
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

void SimulationBridge::runRobotControl

(

)

Definition at line 186 of file SimulationBridge.cpp.

References _firstControllerRun, ControlParameterCollection::_map, _robot, RobotRunner::_robot_ctrl, _robotParams, _robotRunner, _sharedMemory, _simMode, RobotRunner::cheaterState, RobotRunner::cheetahMainVisualization, ControlParameters::collection, RobotRunner::controlParameters, RobotRunner::driverCommand, ControlParameters::generateUnitializedList(), RobotController::getUserControlParameters(), RobotRunner::init(), ControlParameters::isFullyInitialized(), RobotRunner::robotType, RobotRunner::run(), RUN_CONTROLLER, run_sbus(), sbus_thread, RobotRunner::spiCommand, RobotRunner::spiData, RobotRunner::tiBoardCommand, RobotRunner::tiBoardData, RobotRunner::vectorNavData, and RobotRunner::visualizationData.

                                       {
  if (_firstControllerRun) {
    printf("[Simulator Driver] First run of robot controller...\n");
    if (_robotParams.isFullyInitialized()) {
      printf("\tAll %ld control parameters are initialized\n",
             _robotParams.collection._map.size());
    } else {
      printf(
          "\tbut not all control parameters were initialized. Missing:\n%s\n",
          _robotParams.generateUnitializedList().c_str());
      throw std::runtime_error(
          "not all parameters initialized when going into RUN_CONTROLLER");
    }

    auto* userControlParameters = _robotRunner->_robot_ctrl->getUserControlParameters();
    if(userControlParameters) {
      if (userControlParameters->isFullyInitialized()) {
        printf("\tAll %ld user parameters are initialized\n",
               userControlParameters->collection._map.size());
        _simMode = SimulatorMode::RUN_CONTROLLER;
      } else {
        printf(
            "\tbut not all control parameters were initialized. Missing:\n%s\n",
            userControlParameters->generateUnitializedList().c_str());
        throw std::runtime_error(
            "not all parameters initialized when going into RUN_CONTROLLER");
      }
    } else {
      _simMode = SimulatorMode::RUN_CONTROLLER;
    }


    _robotRunner->driverCommand =
        &_sharedMemory().simToRobot.gamepadCommand;
    _robotRunner->spiData = &_sharedMemory().simToRobot.spiData;
    _robotRunner->tiBoardData = _sharedMemory().simToRobot.tiBoardData;
    _robotRunner->robotType = _robot;
    _robotRunner->vectorNavData = &_sharedMemory().simToRobot.vectorNav;
    _robotRunner->cheaterState = &_sharedMemory().simToRobot.cheaterState;
    _robotRunner->spiCommand = &_sharedMemory().robotToSim.spiCommand;
    _robotRunner->tiBoardCommand =
        _sharedMemory().robotToSim.tiBoardCommand;
    _robotRunner->controlParameters = &_robotParams;
    _robotRunner->visualizationData =
        &_sharedMemory().robotToSim.visualizationData;
    _robotRunner->cheetahMainVisualization =
        &_sharedMemory().robotToSim.mainCheetahVisualization;

    _robotRunner->init();
    _firstControllerRun = false;

    sbus_thread = new std::thread(&SimulationBridge::run_sbus, this);
  }
  _robotRunner->run();
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

PeriodicTaskManager* SimulationBridge::_fakeTaskManager = nullptr

private

Definition at line 39 of file SimulationBridge.h.

bool SimulationBridge::_firstControllerRun = true

private

Definition at line 38 of file SimulationBridge.h.

u64 SimulationBridge::_iterations = 0

private

Definition at line 46 of file SimulationBridge.h.

RobotType SimulationBridge::_robot

private

Definition at line 40 of file SimulationBridge.h.

RobotControlParameters SimulationBridge::_robotParams

private

Definition at line 44 of file SimulationBridge.h.

RobotRunner* SimulationBridge::_robotRunner = nullptr

private

Definition at line 41 of file SimulationBridge.h.

SharedMemoryObject<SimulatorSyncronizedMessage> SimulationBridge::_sharedMemory

private

Definition at line 43 of file SimulationBridge.h.

SimulatorMode SimulationBridge::_simMode

private

Definition at line 42 of file SimulationBridge.h.

ControlParameters* SimulationBridge::_userParams = nullptr

private

Definition at line 45 of file SimulationBridge.h.

std::thread* SimulationBridge::sbus_thread

private

Definition at line 48 of file SimulationBridge.h.

PeriodicTaskManager SimulationBridge::taskManager

private

Definition at line 37 of file SimulationBridge.h.

---

The documentation for this class was generated from the following files:

• SimulationBridge.h
• SimulationBridge.cpp