Cheetah Software  1.0
HardwareBridge Class Reference

#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.

26  : statusTask(&taskManager, 0.5f),
29  _robotRunner =
30  new RobotRunner(robot_ctrl, &taskManager, 0.001f, "robot-control");
32  }
ControlParameters * _userControlParameters
lcm::LCM _interfaceLCM
std::string getLcmUrl(s64 ttl)
Definition: utilities.cpp:32
lcm::LCM _visualizationLCM
virtual ControlParameters * getUserControlParameters()=0
PrintTaskStatus statusTask
PeriodicTaskManager taskManager
RobotRunner * _robotRunner
MX f(const MX &x, const MX &u)

+ 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; }
RobotRunner * _robotRunner

+ 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.

94  {
95  (void)rbuf;
96  (void)chan;
97  if (msg->requestNumber <= _parameter_response_lcmt.requestNumber) {
98  // nothing to do!
99  printf(
100  "[HardwareBridge] Warning: the interface has run a ControlParameter "
101  "iteration, but there is no new request!\n");
102  // return;
103  }
104 
105  // sanity check
106  s64 nRequests = msg->requestNumber - _parameter_response_lcmt.requestNumber;
107  if (nRequests != 1) {
108  printf("[ERROR] Hardware bridge: we've missed %ld requests\n",
109  nRequests - 1);
110  }
111 
112  switch (msg->requestKind) {
115  printf("[Warning] Got user param %s, but not using user parameters!\n",
116  (char*)msg->name);
117  } else {
118  std::string name((char*)msg->name);
120 
121  // type check
122  if ((s8)param._kind != msg->parameterKind) {
123  throw std::runtime_error(
124  "type mismatch for parameter " + name + ", robot thinks it is " +
126  " but received a command to set it to " +
128  (ControlParameterValueKind)msg->parameterKind));
129  }
130 
131  // do the actual set
133  memcpy(&v, msg->value, sizeof(v));
134  param.set(v, (ControlParameterValueKind)msg->parameterKind);
135 
136  // respond:
137  _parameter_response_lcmt.requestNumber =
138  msg->requestNumber; // acknowledge that the set has happened
139  _parameter_response_lcmt.parameterKind =
140  msg->parameterKind; // just for debugging print statements
141  memcpy(_parameter_response_lcmt.value, msg->value, 64);
142  //_parameter_response_lcmt.value = _parameter_request_lcmt.value; // just
143  //for debugging print statements
144  strcpy((char*)_parameter_response_lcmt.name,
145  name.c_str()); // just for debugging print statements
146  _parameter_response_lcmt.requestKind = msg->requestKind;
147 
148  printf("[User Control Parameter] set %s to %s\n", name.c_str(),
150  v, (ControlParameterValueKind)msg->parameterKind)
151  .c_str());
152  }
153  } break;
154 
156  std::string name((char*)msg->name);
158 
159  // type check
160  if ((s8)param._kind != msg->parameterKind) {
161  throw std::runtime_error(
162  "type mismatch for parameter " + name + ", robot thinks it is " +
164  " but received a command to set it to " +
166  (ControlParameterValueKind)msg->parameterKind));
167  }
168 
169  // do the actual set
171  memcpy(&v, msg->value, sizeof(v));
172  param.set(v, (ControlParameterValueKind)msg->parameterKind);
173 
174  // respond:
175  _parameter_response_lcmt.requestNumber =
176  msg->requestNumber; // acknowledge that the set has happened
177  _parameter_response_lcmt.parameterKind =
178  msg->parameterKind; // just for debugging print statements
179  memcpy(_parameter_response_lcmt.value, msg->value, 64);
180  //_parameter_response_lcmt.value = _parameter_request_lcmt.value; // just
181  //for debugging print statements
182  strcpy((char*)_parameter_response_lcmt.name,
183  name.c_str()); // just for debugging print statements
184  _parameter_response_lcmt.requestKind = msg->requestKind;
185 
186  printf("[Robot Control Parameter] set %s to %s\n", name.c_str(),
188  v, (ControlParameterValueKind)msg->parameterKind)
189  .c_str());
190 
191  } break;
192 
193  default: {
194  throw std::runtime_error("parameter type unsupported");
195  }
196  break;
197  }
198  _interfaceLCM.publish("interface_response", &_parameter_response_lcmt);
199 }
control_parameter_respones_lcmt _parameter_response_lcmt
ControlParameterCollection collection
ControlParameterValueKind _kind
ControlParameters * _userControlParameters
ControlParameterValueKind
std::string controlParameterValueKindToString(ControlParameterValueKind valueKind)
lcm::LCM _interfaceLCM
int64_t s64
Definition: cTypes.h:24
void set(ControlParameterValue value, ControlParameterValueKind kind)
int8_t s8
Definition: cTypes.h:21
ControlParameter & lookup(const std::string &name)
std::string controlParameterValueToString(ControlParameterValue v, ControlParameterValueKind kind)
RobotControlParameters _robotParams

+ 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().

86  {
87  (void)rbuf;
88  (void)chan;
89  _gamepadCommand.set(msg);
90 }
GamepadCommand _gamepadCommand
void set(const gamepad_lcmt *lcmt)

+ 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.

48  {
49  while (!_interfaceLcmQuit) _interfaceLCM.handle();
50 }
volatile bool _interfaceLcmQuit
lcm::LCM _interfaceLCM

+ 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().

30  {
31  printf("[HardwareBridge] Init stack\n");
32  prefaultStack();
33  printf("[HardwareBridge] Init scheduler\n");
35  if (!_interfaceLCM.good()) {
36  initError("_interfaceLCM failed to initialize\n", false);
37  }
38 
39  printf("[HardwareBridge] Subscribe LCM\n");
40  _interfaceLCM.subscribe("interface", &HardwareBridge::handleGamepadLCM, this);
41  _interfaceLCM.subscribe("interface_request",
43 
44  printf("[HardwareBridge] Start interface LCM handler\n");
46 }
void handleInterfaceLCM()
lcm::LCM _interfaceLCM
void handleControlParameter(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const control_parameter_request_lcmt *msg)
void initError(const char *reason, bool printErrno=false)
std::thread _interfaceLcmThread
void handleGamepadLCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const gamepad_lcmt *msg)

+ 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
reasonError message string
printErrnoIf true, also print C errno

Definition at line 17 of file HardwareBridge.cpp.

17  {
18  printf("FAILED TO INITIALIZE HARDWARE: %s\n", reason);
19 
20  if (printErrno) {
21  printf("Error: %s\n", strerror(errno));
22  }
23 
24  exit(-1);
25 }

+ 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.

60  {
61  printf("[Init] Prefault stack...\n");
62  volatile char stack[MAX_STACK_SIZE];
63  memset(const_cast<char*>(stack), 0, MAX_STACK_SIZE);
64  if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
65  initError(
66  "mlockall failed. This is likely because you didn't run robot as "
67  "root.\n",
68  true);
69  }
70 }
void initError(const char *reason, bool printErrno=false)
#define MAX_STACK_SIZE

+ 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.

305  {
306  cheetah_visualization_lcmt visualization_data;
307  for (int i = 0; i < 3; i++) {
308  visualization_data.x[i] = _mainCheetahVisualization.p[i];
309  }
310 
311  for (int i = 0; i < 4; i++) {
312  visualization_data.quat[i] = _mainCheetahVisualization.quat[i];
313  visualization_data.rgba[i] = _mainCheetahVisualization.color[i];
314  }
315 
316  for (int i = 0; i < 12; i++) {
317  visualization_data.q[i] = _mainCheetahVisualization.q[i];
318  }
319 
320  _visualizationLCM.publish("main_cheetah_visualization", &visualization_data);
321 }
lcm::LCM _visualizationLCM
CheetahVisualization _mainCheetahVisualization

+ 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().

266  {
267  if (_port > 0) {
268  int x = receive_sbus(_port);
269  if (x) {
271  }
272  }
273 }
void sbus_packet_complete()
Function which handles the completion of an SBUS Packet and overrides the LCM control settings as des...
int receive_sbus(int port)
Definition: rt_sbus.cpp:108

+ 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.

75  {
76  printf("[Init] Setup RT Scheduler...\n");
77  struct sched_param params;
78  params.sched_priority = TASK_PRIORITY;
79  if (sched_setscheduler(0, SCHED_FIFO, &params) == -1) {
80  initError("sched_setscheduler failed.\n", true);
81  }
82 }
void initError(const char *reason, bool printErrno=false)
#define TASK_PRIORITY

+ 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: