#include <HardwareBridge.h>

Inheritance diagram for HardwareBridge:

Collaboration diagram for HardwareBridge:

Public Member Functions
	HardwareBridge (RobotController *robot_ctrl)

void	prefaultStack ()

void	setupScheduler ()

void	initError (const char *reason, bool printErrno=false)

void	initCommon ()

	~HardwareBridge ()

void	handleGamepadLCM (const lcm::ReceiveBuffer rbuf, const std::string &chan, const gamepad_lcmt msg)

void	handleInterfaceLCM ()

void	handleControlParameter (const lcm::ReceiveBuffer rbuf, const std::string &chan, const control_parameter_request_lcmt msg)

void	publishVisualizationLCM ()

void	run_sbus ()

Protected Attributes
PeriodicTaskManager	taskManager

PrintTaskStatus	statusTask

GamepadCommand	_gamepadCommand

VisualizationData	_visualizationData

CheetahVisualization	_mainCheetahVisualization

lcm::LCM	_interfaceLCM

lcm::LCM	_visualizationLCM

control_parameter_respones_lcmt	_parameter_response_lcmt

SpiData	_spiData

SpiCommand	_spiCommand

bool	_firstRun = true

RobotRunner *	_robotRunner = nullptr

RobotControlParameters	_robotParams

u64	_iterations = 0

std::thread	_interfaceLcmThread

volatile bool	_interfaceLcmQuit = false

ControlParameters *	_userControlParameters = nullptr

int	_port

Detailed Description

Definition at line 23 of file HardwareBridge.h.

Constructor & Destructor Documentation

HardwareBridge::HardwareBridge ( RobotController * robot_ctrl )

inline

Definition at line 25 of file HardwareBridge.h.

References _robotRunner, _userControlParameters, f(), RobotController::getUserControlParameters(), initCommon(), initError(), prefaultStack(), setupScheduler(), and taskManager.

       : statusTask(&taskManager, 0.5f),
         _interfaceLCM(getLcmUrl(255)),
         _visualizationLCM(getLcmUrl(255)) {
           _robotRunner = 
             new RobotRunner(robot_ctrl, &taskManager, 0.001f, "robot-control");
     _userControlParameters = robot_ctrl->getUserControlParameters();
         }

Here is the call graph for this function:

HardwareBridge::~HardwareBridge ( )

inline

Definition at line 37 of file HardwareBridge.h.

References _robotRunner, handleControlParameter(), handleGamepadLCM(), handleInterfaceLCM(), publishVisualizationLCM(), and run_sbus().

37 { delete _robotRunner; }

HardwareBridge::_robotRunner

RobotRunner * _robotRunner

Definition: HardwareBridge.h:62

Here is the call graph for this function:

Member Function Documentation

void HardwareBridge::handleControlParameter	(	const lcm::ReceiveBuffer *	rbuf,
		const std::string &	chan,
		const control_parameter_request_lcmt *	msg
	)

Definition at line 92 of file HardwareBridge.cpp.

References _interfaceLCM, ControlParameter::_kind, _parameter_response_lcmt, _robotParams, _userControlParameters, ControlParameters::collection, controlParameterValueKindToString(), controlParameterValueToString(), ControlParameterCollection::lookup(), ControlParameter::set(), SET_ROBOT_PARAM_BY_NAME, and SET_USER_PARAM_BY_NAME.

                                                {
   (void)rbuf;
   (void)chan;
   if (msg->requestNumber <= _parameter_response_lcmt.requestNumber) {
     // nothing to do!
     printf(
         "[HardwareBridge] Warning: the interface has run a ControlParameter "
         "iteration, but there is no new request!\n");
     // return;
   }
 
   // sanity check
   s64 nRequests = msg->requestNumber - _parameter_response_lcmt.requestNumber;
   if (nRequests != 1) {
     printf("[ERROR] Hardware bridge: we've missed %ld requests\n",
            nRequests - 1);
   }
 
   switch (msg->requestKind) {
     case (s8)ControlParameterRequestKind::SET_USER_PARAM_BY_NAME: {
       if(!_userControlParameters) {
         printf("[Warning] Got user param %s, but not using user parameters!\n",
                (char*)msg->name);
       } else {
         std::string name((char*)msg->name);
         ControlParameter& param = _userControlParameters->collection.lookup(name);
 
         // type check
         if ((s8)param._kind != msg->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(
                   (ControlParameterValueKind)msg->parameterKind));
         }
 
         // do the actual set
         ControlParameterValue v;
         memcpy(&v, msg->value, sizeof(v));
         param.set(v, (ControlParameterValueKind)msg->parameterKind);
 
         // respond:
         _parameter_response_lcmt.requestNumber =
             msg->requestNumber;  // acknowledge that the set has happened
         _parameter_response_lcmt.parameterKind =
             msg->parameterKind;  // just for debugging print statements
         memcpy(_parameter_response_lcmt.value, msg->value, 64);
         //_parameter_response_lcmt.value = _parameter_request_lcmt.value; // just
         //for debugging print statements
         strcpy((char*)_parameter_response_lcmt.name,
                name.c_str());  // just for debugging print statements
         _parameter_response_lcmt.requestKind = msg->requestKind;
 
         printf("[User Control Parameter] set %s to %s\n", name.c_str(),
                controlParameterValueToString(
                    v, (ControlParameterValueKind)msg->parameterKind)
                    .c_str());
       }
     } break;
 
     case (s8)ControlParameterRequestKind::SET_ROBOT_PARAM_BY_NAME: {
       std::string name((char*)msg->name);
       ControlParameter& param = _robotParams.collection.lookup(name);
 
       // type check
       if ((s8)param._kind != msg->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(
                 (ControlParameterValueKind)msg->parameterKind));
       }
 
       // do the actual set
       ControlParameterValue v;
       memcpy(&v, msg->value, sizeof(v));
       param.set(v, (ControlParameterValueKind)msg->parameterKind);
 
       // respond:
       _parameter_response_lcmt.requestNumber =
           msg->requestNumber;  // acknowledge that the set has happened
       _parameter_response_lcmt.parameterKind =
           msg->parameterKind;  // just for debugging print statements
       memcpy(_parameter_response_lcmt.value, msg->value, 64);
       //_parameter_response_lcmt.value = _parameter_request_lcmt.value; // just
       //for debugging print statements
       strcpy((char*)_parameter_response_lcmt.name,
              name.c_str());  // just for debugging print statements
       _parameter_response_lcmt.requestKind = msg->requestKind;
 
       printf("[Robot Control Parameter] set %s to %s\n", name.c_str(),
              controlParameterValueToString(
                  v, (ControlParameterValueKind)msg->parameterKind)
                  .c_str());
 
     } break;
 
     default: {
       throw std::runtime_error("parameter type unsupported");
     }
     break;
   }
   _interfaceLCM.publish("interface_response", &_parameter_response_lcmt);
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void HardwareBridge::handleGamepadLCM	(	const lcm::ReceiveBuffer *	rbuf,
		const std::string &	chan,
		const gamepad_lcmt *	msg
	)

Definition at line 84 of file HardwareBridge.cpp.

References _gamepadCommand, and GamepadCommand::set().

                                                                {
   (void)rbuf;
   (void)chan;
   _gamepadCommand.set(msg);
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void HardwareBridge::handleInterfaceLCM ( )

Definition at line 48 of file HardwareBridge.cpp.

References _interfaceLCM, and _interfaceLcmQuit.

                                         {
   while (!_interfaceLcmQuit) _interfaceLCM.handle();
 }

Here is the caller graph for this function:

void HardwareBridge::initCommon ( )

All initialization code that is common between Cheetah 3 and Mini Cheetah

Definition at line 30 of file HardwareBridge.cpp.

References _interfaceLCM, _interfaceLcmThread, handleControlParameter(), handleGamepadLCM(), handleInterfaceLCM(), initError(), prefaultStack(), and setupScheduler().

                                 {
   printf("[HardwareBridge] Init stack\n");
   prefaultStack();
   printf("[HardwareBridge] Init scheduler\n");
   setupScheduler();
   if (!_interfaceLCM.good()) {
     initError("_interfaceLCM failed to initialize\n", false);
   }
 
   printf("[HardwareBridge] Subscribe LCM\n");
   _interfaceLCM.subscribe("interface", &HardwareBridge::handleGamepadLCM, this);
   _interfaceLCM.subscribe("interface_request",
                           &HardwareBridge::handleControlParameter, this);
 
   printf("[HardwareBridge] Start interface LCM handler\n");
   _interfaceLcmThread = std::thread(&HardwareBridge::handleInterfaceLCM, this);
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void HardwareBridge::initError	(	const char *	reason,
		bool	printErrno = `false`
	)

If an error occurs during initialization, before motors are enabled, print error and exit.

Parameters

reason	Error message string
printErrno	If true, also print C errno

Definition at line 17 of file HardwareBridge.cpp.

                                                                   {
   printf("FAILED TO INITIALIZE HARDWARE: %s\n", reason);
 
   if (printErrno) {
     printf("Error: %s\n", strerror(errno));
   }
 
   exit(-1);
 }

Here is the caller graph for this function:

void HardwareBridge::prefaultStack ( )

Writes to a 16 KB buffer on the stack. If we are using 4K pages for our stack, this will make sure that we won't have a page fault when the stack grows. Also mlock's all pages associated with the current process, which prevents the cheetah software from being swapped out. If we do run out of memory, the robot program will be killed by the OOM process killer (and leaves a log) instead of just becoming unresponsive.

Definition at line 60 of file HardwareBridge.cpp.

References initError(), and MAX_STACK_SIZE.

                                    {
   printf("[Init] Prefault stack...\n");
   volatile char stack[MAX_STACK_SIZE];
   memset(const_cast<char*>(stack), 0, MAX_STACK_SIZE);
   if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
     initError(
         "mlockall failed.  This is likely because you didn't run robot as "
         "root.\n",
         true);
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void HardwareBridge::publishVisualizationLCM ( )

Definition at line 305 of file HardwareBridge.cpp.

References _mainCheetahVisualization, _visualizationLCM, CheetahVisualization::color, CheetahVisualization::p, CheetahVisualization::q, and CheetahVisualization::quat.

                                              {
   cheetah_visualization_lcmt visualization_data;
   for (int i = 0; i < 3; i++) {
     visualization_data.x[i] = _mainCheetahVisualization.p[i];
   }
 
   for (int i = 0; i < 4; i++) {
     visualization_data.quat[i] = _mainCheetahVisualization.quat[i];
     visualization_data.rgba[i] = _mainCheetahVisualization.color[i];
   }
 
   for (int i = 0; i < 12; i++) {
     visualization_data.q[i] = _mainCheetahVisualization.q[i];
   }
 
   _visualizationLCM.publish("main_cheetah_visualization", &visualization_data);
 }

Here is the caller graph for this function:

void HardwareBridge::run_sbus ( )

Definition at line 266 of file HardwareBridge.cpp.

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

                               {
   if (_port > 0) {
     int x = receive_sbus(_port);
     if (x) {
       sbus_packet_complete();
     }
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void HardwareBridge::setupScheduler ( )

Configures the

Definition at line 75 of file HardwareBridge.cpp.

References initError(), and TASK_PRIORITY.

                                     {
   printf("[Init] Setup RT Scheduler...\n");
   struct sched_param params;
   params.sched_priority = TASK_PRIORITY;
   if (sched_setscheduler(0, SCHED_FIFO, &params) == -1) {
     initError("sched_setscheduler failed.\n", true);
   }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

bool HardwareBridge::_firstRun = true

protected

Definition at line 61 of file HardwareBridge.h.

GamepadCommand HardwareBridge::_gamepadCommand

protected

Definition at line 52 of file HardwareBridge.h.

lcm::LCM HardwareBridge::_interfaceLCM

protected

Definition at line 55 of file HardwareBridge.h.

volatile bool HardwareBridge::_interfaceLcmQuit = false

protected

Definition at line 66 of file HardwareBridge.h.

std::thread HardwareBridge::_interfaceLcmThread

protected

Definition at line 65 of file HardwareBridge.h.

u64 HardwareBridge::_iterations = 0

protected

Definition at line 64 of file HardwareBridge.h.

CheetahVisualization HardwareBridge::_mainCheetahVisualization

protected

Definition at line 54 of file HardwareBridge.h.

control_parameter_respones_lcmt HardwareBridge::_parameter_response_lcmt

protected

Definition at line 57 of file HardwareBridge.h.

int HardwareBridge::_port

protected

Definition at line 69 of file HardwareBridge.h.

RobotControlParameters HardwareBridge::_robotParams

protected

Definition at line 63 of file HardwareBridge.h.

RobotRunner* HardwareBridge::_robotRunner = nullptr

protected

Definition at line 62 of file HardwareBridge.h.

SpiCommand HardwareBridge::_spiCommand

protected

Definition at line 59 of file HardwareBridge.h.

SpiData HardwareBridge::_spiData

protected

Definition at line 58 of file HardwareBridge.h.

ControlParameters* HardwareBridge::_userControlParameters = nullptr

protected

Definition at line 67 of file HardwareBridge.h.

VisualizationData HardwareBridge::_visualizationData

protected

Definition at line 53 of file HardwareBridge.h.

lcm::LCM HardwareBridge::_visualizationLCM

protected

Definition at line 56 of file HardwareBridge.h.

PrintTaskStatus HardwareBridge::statusTask

protected

Definition at line 51 of file HardwareBridge.h.

PeriodicTaskManager HardwareBridge::taskManager

protected

Definition at line 50 of file HardwareBridge.h.

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

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation