void discoverControllers()
{
// Get an array of all available devices.
int deviceCount;
screen_get_context_property_iv(screen_ctx, SCREEN_PROPERTY_DEVICE_COUNT, &deviceCount);
screen_device_t* devices_found = (screen_device_t*)calloc(deviceCount, sizeof(screen_device_t));
screen_get_context_property_pv(screen_ctx, SCREEN_PROPERTY_DEVICES, (void**)devices_found);
// Scan the list for gamepad and joystick devices.
int i;
for(i=0;i<pads_connected;++i)
initController(&devices[i]);
pads_connected = 0;
for (i = 0; i < deviceCount; i++)
{
int type;
screen_get_device_property_iv(devices_found[i], SCREEN_PROPERTY_TYPE, &type);
if (type == SCREEN_EVENT_GAMEPAD || type == SCREEN_EVENT_JOYSTICK || type == SCREEN_EVENT_KEYBOARD)
{
devices[pads_connected].handle = devices_found[i];
loadController(&devices[pads_connected]);
pads_connected++;
if (pads_connected == MAX_PADS)
break;
}
}
free(devices_found);
}
static void discoverControllers()
{
// Get an array of all available devices.
int deviceCount;
SCREEN_API(screen_get_context_property_iv(_screen_ctx, SCREEN_PROPERTY_DEVICE_COUNT, &deviceCount), "SCREEN_PROPERTY_DEVICE_COUNT");
screen_device_t* devices = (screen_device_t*)calloc(deviceCount, sizeof(screen_device_t));
SCREEN_API(screen_get_context_property_pv(_screen_ctx, SCREEN_PROPERTY_DEVICES, (void**)devices), "SCREEN_PROPERTY_DEVICES");
// Scan the list for gamepad and joystick devices.
int i;
int controllerIndex = 0;
for (i = 0; i < deviceCount; i++) {
int type;
SCREEN_API(screen_get_device_property_iv(devices[i], SCREEN_PROPERTY_TYPE, &type), "SCREEN_PROPERTY_TYPE");
if (!rc && (type == SCREEN_EVENT_GAMEPAD || type == SCREEN_EVENT_JOYSTICK)) {
// Assign this device to control Player 1 or Player 2.
GameController* controller = &_controllers[controllerIndex];
controller->handle = devices[i];
loadController(controller);
// We'll just use the first compatible devices we find.
controllerIndex++;
if (controllerIndex == MAX_CONTROLLERS) {
break;
}
}
}
free(devices);
}
bool ControllerManager::loadControllerSrv(
controller_manager_msgs::LoadController::Request &req,
controller_manager_msgs::LoadController::Response &resp)
{
// lock services
ROS_DEBUG("loading service called for controller %s ",req.name.c_str());
boost::mutex::scoped_lock guard(services_lock_);
ROS_DEBUG("loading service locked");
resp.ok = loadController(req.name);
ROS_DEBUG("loading service finished for controller %s ",req.name.c_str());
return true;
}
void randomBombardment(unsigned int times)
{
while (!bombardment_started_)
ros::Duration(0.1).sleep();
for (unsigned int i=0; i<times; i++){
unsigned int random = rand();
random = random % 4;
switch (random){
case 0:{
loadController(randomController());
break;
}
case 1:{
unloadController(randomController());
break;
}
case 2:{
std::vector<std::string> start, stop;
unsigned int start_size = rand() %10;
unsigned int stop_size = rand() %10;
for (unsigned int i=0; i<start_size; i++)
start.push_back(randomController());
for (unsigned int i=0; i<stop_size; i++)
stop.push_back(randomController());
if (rand() %1 == 0)
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT);
else
switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::BEST_EFFORT);
break;
}
case 3:{
controllerState(randomController());
break;
}
}
}
}
static void discoverControllers(void *data)
{
// Get an array of all available devices.
int deviceCount;
unsigned i;
screen_event_t *event;
qnx_input_t *qnx = (qnx_input_t*)data;
(void)event;
screen_get_context_property_iv(screen_ctx, SCREEN_PROPERTY_DEVICE_COUNT, &deviceCount);
screen_device_t* devices_found = (screen_device_t*)calloc(deviceCount, sizeof(screen_device_t));
screen_get_context_property_pv(screen_ctx, SCREEN_PROPERTY_DEVICES, (void**)devices_found);
// Scan the list for gamepad and joystick devices.
for(i = 0; i < qnx->pads_connected; ++i)
initController(qnx, &qnx->devices[i]);
qnx->pads_connected = 0;
for (i = 0; i < deviceCount; i++)
{
int type;
screen_get_device_property_iv(devices_found[i], SCREEN_PROPERTY_TYPE, &type);
if (type == SCREEN_EVENT_GAMEPAD || type == SCREEN_EVENT_JOYSTICK || type == SCREEN_EVENT_KEYBOARD)
{
qnx->devices[qnx->pads_connected].handle = devices_found[i];
qnx->devices[qnx->pads_connected].index = qnx->pads_connected;
loadController(qnx, &qnx->devices[qnx->pads_connected]);
if (qnx->pads_connected == MAX_PADS)
break;
}
}
free(devices_found);
}
void Controller::loadBackup()
{
loadController(backupFileName);
}
void Controller::load()
{
loadController(fileName);
}
static void handle_screen_event(bps_event_t *event)
{
int type;
screen_event_t screen_event = screen_event_get_event(event);
screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_TYPE, &type);
switch(type)
{
case SCREEN_EVENT_MTOUCH_TOUCH:
case SCREEN_EVENT_MTOUCH_RELEASE:
case SCREEN_EVENT_MTOUCH_MOVE:
process_touch_event(screen_event, type);
break;
case SCREEN_EVENT_KEYBOARD:
process_keyboard_event(screen_event, type);
break;
#ifdef HAVE_BB10
case SCREEN_EVENT_GAMEPAD:
case SCREEN_EVENT_JOYSTICK:
process_gamepad_event(screen_event, type);
break;
case SCREEN_EVENT_DEVICE:
{
// A device was attached or removed.
screen_device_t device;
int attached;
int type;
screen_get_event_property_pv(screen_event,
SCREEN_PROPERTY_DEVICE, (void**)&device);
screen_get_event_property_iv(screen_event,
SCREEN_PROPERTY_ATTACHED, &attached);
if (attached)
screen_get_device_property_iv(device,
SCREEN_PROPERTY_TYPE, &type);
int i;
if (attached && (type == SCREEN_EVENT_GAMEPAD || type == SCREEN_EVENT_JOYSTICK || type == SCREEN_EVENT_KEYBOARD))
{
for (i = 0; i < MAX_PADS; ++i)
{
if (!devices[i].handle)
{
devices[i].handle = device;
loadController(&devices[i]);
break;
}
}
}
else
{
for (i = 0; i < MAX_PADS; ++i)
{
if (device == devices[i].handle)
{
RARCH_LOG("Device %s: Disconnected.\n", devices[i].id);
initController(&devices[i]);
break;
}
}
}
}
break;
#endif
default:
break;
}
}
static void handleScreenEvent(bps_event_t *event)
{
int eventType;
screen_event_t screen_event = screen_event_get_event(event);
screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_TYPE, &eventType);
switch (eventType) {
case SCREEN_EVENT_GAMEPAD:
case SCREEN_EVENT_JOYSTICK:
{
if (!_polling) {
// Determine which controller this is.
screen_device_t device;
SCREEN_API(screen_get_event_property_pv(screen_event, SCREEN_PROPERTY_DEVICE, (void**)&device), "SCREEN_PROPERTY_DEVICE");
GameController* controller = NULL;
int i;
for (i = 0; i < MAX_CONTROLLERS; ++i) {
if (device == _controllers[i].handle) {
controller = &_controllers[i];
break;
}
}
if (!controller) {
break;
}
// Store the controller's new state.
SCREEN_API(screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_BUTTONS, &controller->buttons), "SCREEN_PROPERTY_BUTTONS");
SCREEN_API(screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_ANALOG0, controller->analog0), "SCREEN_PROPERTY_ANALOG0");
if (controller->type == SCREEN_EVENT_GAMEPAD) {
SCREEN_API(screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_ANALOG1, controller->analog1), "SCREEN_PROPERTY_ANALOG1");
}
}
break;
}
case SCREEN_EVENT_DEVICE:
{
// A device was attached or removed.
screen_device_t device;
int attached;
int type;
SCREEN_API(screen_get_event_property_pv(screen_event, SCREEN_PROPERTY_DEVICE, (void**)&device), "SCREEN_PROPERTY_DEVICE");
SCREEN_API(screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_ATTACHED, &attached), "SCREEN_PROPERTY_ATTACHED");
if (attached) {
SCREEN_API(screen_get_device_property_iv(device, SCREEN_PROPERTY_TYPE, &type), "SCREEN_PROPERTY_TYPE");
}
int i;
if (attached && (type == SCREEN_EVENT_GAMEPAD || type == SCREEN_EVENT_JOYSTICK)) {
for (i = 0; i < MAX_CONTROLLERS; ++i) {
if (!_controllers[i].handle) {
_controllers[i].handle = device;
loadController(&_controllers[i]);
break;
}
}
} else {
for (i = 0; i < MAX_CONTROLLERS; ++i) {
if (device == _controllers[i].handle) {
initController(&_controllers[i], i);
break;
}
}
}
break;
}
case SCREEN_EVENT_MTOUCH_RELEASE:
{
int pos[2];
SCREEN_API(screen_get_event_property_iv(screen_event, SCREEN_PROPERTY_SOURCE_POSITION, pos), "SCREEN_PROPERTY_SOURCE_POSITION");
int x = pos[0];
int y = _surfaceHeight - pos[1];
int i;
for (i = 0; i < MAX_CONTROLLERS; ++i) {
bool buttonTapped = false;
int j;
for (j = 0; j < MAX_BUTTONS; ++j) {
Button* button = &_buttons[i][j];
Quad* quad = button->quad;
// Detect that a button was tapped.
if (x > quad->x && x < quad->x + quad->width &&
y > quad->y && y < quad->y + quad->height) {
_activeButton[i] = button;
buttonTapped = true;
break;
}
}
if (_activeButton[i] && !buttonTapped) {
// Cancel the button's active state.
_activeButton[i] = NULL;
}
}
// The polling button is used to switch between handling all device events and polling devices once per frame.
Quad* quad = _pollingButton.quad;
if (x > quad->x && x < quad->x + quad->width &&
y > quad->y && y < quad->y + quad->height) {
_polling = !_polling;
// The polling button lights up when polling is enabled.
if (_polling) {
quad->uvs = _triggerDownUVs;
} else {
quad->uvs = _triggerUpUVs;
}
}
break;
}
}
}
TEST_F(TestController, start_stop_strict)
{
// check initial state
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _unloaded);
EXPECT_EQ(controllerState("controller3"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
EXPECT_EQ(controllerState("controller6"), _unloaded);
EXPECT_EQ(controllerState("controller7"), _unloaded);
EXPECT_EQ(controllerState("controller8"), _unloaded);
EXPECT_EQ(controllerState("controller9"), _unloaded);
// starting already started controller
std::vector<std::string> start, stop;
start.push_back("controller1");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
// starting unloaded controller
start.push_back("controller2");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _unloaded);
// starting one already stated, 1 stopped
EXPECT_TRUE(loadController("controller2"));
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
// start and stop same controller
stop.push_back("controller2");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
// stop unloaded controller
stop.push_back("controller5");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
// stop unloaded and running controller
stop.push_back("controller4");
EXPECT_FALSE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _unloaded);
// stop running and stopped controller
EXPECT_TRUE(loadController("controller5"));
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller4"), _stopped);
EXPECT_EQ(controllerState("controller5"), _stopped);
// stop 2 stopped controllers, and 1 running controller
stop.push_back("controller3");
EXPECT_TRUE(switchController(start, stop, pr2_mechanism_msgs::SwitchController::Request::STRICT));
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _running);
EXPECT_EQ(controllerState("controller3"), _stopped);
EXPECT_EQ(controllerState("controller4"), _stopped);
EXPECT_EQ(controllerState("controller5"), _stopped);
SUCCEED();
}
TEST_F(TestController, loading)
{
// check initial state
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _stopped);
EXPECT_EQ(controllerState("controller3"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _stopped);
EXPECT_EQ(controllerState("controller6"), _stopped);
EXPECT_EQ(controllerState("controller7"), _unloaded);
EXPECT_EQ(controllerState("controller8"), _unloaded);
EXPECT_EQ(controllerState("controller9"), _unloaded);
// these are already loaded
EXPECT_FALSE(loadController("controller1"));
EXPECT_FALSE(loadController("controller2"));
EXPECT_FALSE(loadController("controller3"));
EXPECT_FALSE(loadController("controller4"));
EXPECT_FALSE(loadController("controller5"));
EXPECT_FALSE(loadController("controller6"));
// this one is not loaded yet
EXPECT_TRUE(loadController("controller7"));
// this one is wrongly configured
EXPECT_FALSE(loadController("controller9"));
EXPECT_FALSE(loadController("controller11"));
EXPECT_FALSE(loadController("controller12"));
EXPECT_FALSE(loadController("controller13"));
EXPECT_FALSE(loadController("controller14"));
EXPECT_FALSE(loadController("controller15"));
EXPECT_FALSE(loadController("controller16"));
EXPECT_FALSE(loadController("controller17"));
EXPECT_FALSE(loadController("controller18"));
EXPECT_FALSE(loadController("controller19"));
// this one is not configured
EXPECT_FALSE(loadController("controller10"));
// check end state
EXPECT_EQ(controllerState("controller1"), _running);
EXPECT_EQ(controllerState("controller2"), _stopped);
EXPECT_EQ(controllerState("controller3"), _running);
EXPECT_EQ(controllerState("controller4"), _running);
EXPECT_EQ(controllerState("controller5"), _stopped);
EXPECT_EQ(controllerState("controller6"), _stopped);
EXPECT_EQ(controllerState("controller7"), _stopped);
EXPECT_EQ(controllerState("controller8"), _unloaded);
EXPECT_EQ(controllerState("controller9"), _unloaded);
SUCCEED();
}