// ------------------------------------------------------
// TestJoystick
// ------------------------------------------------------
void TestJoystick()
{
// Open first joystick:
// ---------------------------
float x,y,z;
vector_bool buttons;
CTicTac tictac;
CJoystick joy;
const int nJoystick = 0; // The first one
printf("Press any key to stop program...\n");
while ( !mrpt::system::os::kbhit() )
{
tictac.Tic();
if (joy.getJoystickPosition(nJoystick,x,y,z,buttons) )
{
double t = tictac.Tac();
printf("Joystick readings: %.03f, %.03f, %.03f (", x, y, z);
for(unsigned b=0;b<buttons.size();b++)
printf("B%u:%c ", b,buttons[b] ? 'X':'-');
printf(") [Query %uus] \r", (unsigned)(t*1e6));
fflush(stdout);
}
else
{
printf("Error reading from joystick, please connect one to the system...\r");
}
mrpt::system::sleep(20);
}
}
PERIPHERAL_ERROR GetJoystickInfo(unsigned int index, JOYSTICK_INFO* info)
{
if (!info)
return PERIPHERAL_ERROR_INVALID_PARAMETERS;
CJoystick* joystick = CJoystickManager::Get().GetJoystick(index);
if (!joystick)
return PERIPHERAL_ERROR_NOT_CONNECTED;
joystick->ToStruct(*info);
return PERIPHERAL_NO_ERROR;
}
PERIPHERAL_ERROR UpdateJoystickFeature(unsigned int index, JOYSTICK_FEATURE* feature)
{
if (!feature)
return PERIPHERAL_ERROR_INVALID_PARAMETERS;
CJoystick* joystick = CJoystickManager::Get().GetJoystick(index);
if (!joystick)
return PERIPHERAL_ERROR_NOT_CONNECTED;
bool bSuccess(false);
// TODO
joystick->Features().push_back(ADDON::JoystickFeatureFactory::Create(*feature));
bSuccess = true;
return bSuccess ? PERIPHERAL_NO_ERROR : PERIPHERAL_ERROR_FAILED;
}
/*--------------------------------
| JOYSTICK CONTROLS |
---------------------------------*/
void JoystickControl(data_st *data)
{
CJoystick joy;
float x,y,z;
vector <bool> buttons;
string response, errormsg;
int action, direction;
//CTicTac tictac;
while(joy.getJoystickPosition(0,x,y,z,buttons))
{
//printf("Joystick readings: %.03f, %.03f, %.03f (", x, y, z);
int speed=(int)(10-(x*x+y*y)*9);
if(x<-0.2 || x>0.2 || y <-0.2 || y>0.2)
{
if(y<-0.2)
{
if(x<0.2 && x>-0.2){//Move Forward
action=18; direction=1;}
if(x<-0.2){//Move Forward & Left
action=18; direction=7;}
if(x>0.2){//Move Forward & Right
action=18; direction=8;}
}
if(y>0.2)
{
if(x<0.2 && x>-0.2){ //Move Backward
action=18; direction=2;}
if(x>0.2){ //Move Backward & Right
action=18; direction=10;}
if(x<-0.2){ //Move Backward & Left
action=18; direction=9;}
}
if(y>-0.2 && y<0.2)
{
if(x<-0.2){ //Move Left
action=18; direction=3;}
if(x>0.2){ //Move Right
action=18; direction=4;}
}
data->Robot.Moving(action, direction, speed, response, errormsg);
}
if((buttons[3] && buttons[2])||(buttons[0] && buttons[1])||(y<-0.5 && buttons[2])||(buttons[0] && buttons[2]))
{
if(buttons[3] && buttons[2]) //Take head up
data->Robot.Moving(18, 11, 5, response, errormsg);
if(buttons[0] && buttons[1]) //Go home
data->Robot.Moving(12, 12, 5, response, errormsg);
if(y<-0.5 && buttons[2]) //Go home & dock
data->Robot.Moving(13, 12, 5, response, errormsg);
if(buttons[0] && buttons[2]) //Get Report
{
data->Robot.Moving(1, 12, 5, response, errormsg);
cout<<"Response:\n"<<response;
}
}
else
{
if(buttons[0]) //Turn to the left
data->Robot.Moving(18, 5, 5, response, errormsg);
if(buttons[1]) //Turn to the right
data->Robot.Moving(18, 6, 5, response, errormsg);
if(buttons[2]) //Take head to the middle
data->Robot.Moving(18, 13, 5, response, errormsg);
if(buttons[3]) //Take head down
data->Robot.Moving(18, 12, 5, response, errormsg);
}
fflush(stdout);
mrpt::system::sleep(20);
}
cout<<"No Joystick connected...\n\n\n"<<endl;
}
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "This program need one input parameter.\n"<<
"The first input parameter is the number of the UAV." << std::endl;
return -1;
}
// The UAV ID is stored in a global variable
std::string uavID="aal_";
uavID.append(std::string(argv[1]));
signal(SIGINT, quit);
string node_name="arm_joystick";
node_name.append(std::string(argv[1]));
ros::init(argc, argv, node_name.c_str());
ros::NodeHandle n(uavID);
string topicname = "/cmd_vel";
//Publisher and Subscribers
ros::Publisher arm_control_ref_pub = n.advertise<arcas_msgs::ArmControlReferencesStamped>("/aal_1/arm_control_references",0);
ros::Subscriber state_estimation_subscriber = n.subscribe("/aal_1/arm_state_estimation",
1000, &armStateEstimationCallback);
int fd;
struct wwvi_js_event wjse;
wjse.stick2_x = 0; // Roll
wjse.stick2_y = 0; // Yaw
wjse.stick_x = 0; // Pitch
wjse.stick_y = 0; // Thrust
wjse.button[0]=0; // Take-Off, Button 1 on Joystick
wjse.button[1]=0; // Land, Button 2 on Joystick
CJoystick joystick;
fd = joystick.open_joystick("/dev/input/js1");
// Initialize ActionClient
AALExtensionActionWrapper aalExtensionActionWrapper;
if(!fd)
{
cerr << "Can not open joystick, is it still connected?\r\n" << endl;
return(-1);
}
ros::AsyncSpinner spinner_(0);
spinner_.start();
while(ros::ok())
{
// Calculate the quad_control_references
if(wjse.stick_x!=0)
{
arm_control_references.arm_control_references.position_ref[2] = arm_state_estimation.arm_state_estimation.position[2] + wjse.stick_x/6000.0;
}
if(wjse.stick2_x!=0)
{
arm_control_references.arm_control_references.position_ref[3] = arm_state_estimation.arm_state_estimation.position[3] + wjse.stick2_x/6000.0;
}
if(wjse.stick_y!=0)
{
arm_control_references.arm_control_references.position_ref[1] = arm_state_estimation.arm_state_estimation.position[1] + wjse.stick_y/6000.0;
}
if( wjse.stick2_y!=0)
{
arm_control_references.arm_control_references.position_ref[0] = arm_state_estimation.arm_state_estimation.position[0] + wjse.stick2_y/6000.0;
}
if(wjse.stick3_x!=0)
{
arm_control_references.arm_control_references.position_ref[4] = arm_state_estimation.arm_state_estimation.position[4] + wjse.stick3_x/12000.0;
}
if(wjse.stick3_y!=0)
{
arm_control_references.arm_control_references.position_ref[5] = arm_state_estimation.arm_state_estimation.position[5] + wjse.stick3_y/12000.0;
}
/*
if(joystick.get_joystick_status(&wjse)!=-1)
{
command_to_send.linear.x = wjse.stick_x/2500.0;
//std::cout << "Pitch control: " << command_to_send.linear.x << std::endl;
command_to_send.linear.y = wjse.stick2_x/2500.0;
//std::cout << "Roll control: " << command_to_send.linear.y << std::endl;
command_to_send.linear.z = wjse.stick_y/4000.0;
//std::cout << "Thrust control: " << command_to_send.linear.z << std::endl;
command_to_send.angular.z = wjse.stick2_y/4000.0;
//std::cout << "Yaw control: " << command_to_send.angular.z << std::endl;
}
else
{
ROS_ERROR_STREAM("Problems with joystick!");
}
*/
// Implement take-off
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[0] == 1))
{
ROS_INFO_STREAM("Extracting Sent");
aalExtensionActionWrapper.extendArm();
usleep(100000); // In order to not send it twice accidentally
}
// Implement landing
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[1] == 1))
{
ROS_INFO_STREAM("Contracting Sent");
aalExtensionActionWrapper.contractArm();
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[2] == 1))
{
arm_control_references.arm_control_references.position_ref[6] = 3.14159/5;
usleep(100000); // In order to not send it twice accidentally
}
if((joystick.get_joystick_status(&wjse)!=-1) && (wjse.button[3] == 1))
{
arm_control_references.arm_control_references.position_ref[6] =0.0;
usleep(100000); // In order to not send it twice accidentally
}
//joystick_publisher.publish(command_to_send);
arm_control_ref_pub.publish(arm_control_references);
//~20Hz
usleep(50000);
}
return 0;
}
bool CJoystickInterfaceLinux::ScanForJoysticks(std::vector<CJoystick*>& joysticks)
{
// TODO: Use udev to grab device names instead of reading /dev/input/js*
std::string inputDir("/dev/input");
DIR *pd = opendir(inputDir.c_str());
if (pd == NULL)
{
esyslog("%s: can't open %s (errno=%d)", __FUNCTION__, inputDir.c_str(), errno);
return false;
}
dirent *pDirent;
while ((pDirent = readdir(pd)) != NULL)
{
if (std::string(pDirent->d_name).substr(0, 2) == "js")
{
// Found a joystick device
std::string filename(inputDir + "/" + pDirent->d_name);
isyslog("CJoystickInterfaceLinux::Initialize: opening joystick %s", filename.c_str());
int fd = open(filename.c_str(), O_RDONLY);
if (fd < 0)
{
esyslog("%s: can't open %s (errno=%d)", __FUNCTION__, filename.c_str(), errno);
continue;
}
unsigned char axes = 0;
unsigned char buttons = 0;
int version = 0x000000;
char name[128] = { };
if (ioctl(fd, JSIOCGVERSION, &version) < 0 ||
ioctl(fd, JSIOCGAXES, &axes) < 0 ||
ioctl(fd, JSIOCGBUTTONS, &buttons) < 0 ||
ioctl(fd, JSIOCGNAME(128), name) < 0)
{
esyslog("%s: failed ioctl() (errno=%d)", __FUNCTION__, errno);
close(fd);
continue;
}
if (fcntl(fd, F_SETFL, O_NONBLOCK) < 0)
{
esyslog("%s: failed fcntl() (errno=%d)", __FUNCTION__, errno);
close(fd);
continue;
}
// We don't support the old (0.x) interface
if (version < 0x010000)
{
esyslog("%s: old (0.x) interface is not supported (version=%08x)", __FUNCTION__, version);
close(fd);
continue;
}
unsigned int index = (unsigned int)std::max(strtol(pDirent->d_name + strlen("js"), NULL, 10), 0L);
CJoystick* joystick = new CJoystickLinux(fd, filename, this);
joystick->SetName(name);
joystick->SetButtonCount(buttons);
joystick->SetAxisCount(axes);
joystick->SetRequestedPort(index);
joysticks.push_back(joystick);
}
}
closedir(pd);
return true;
}
bool CJoystickInterfaceSDL::PerformJoystickScan(std::vector<CJoystick*>& joysticks)
{
Deinitialize();
if (SDL_InitSubSystem(SDL_INIT_JOYSTICK) != 0)
{
esyslog("(Re)start joystick subsystem failed : %s", SDL_GetError());
return false;
}
// Any joysticks connected?
if (SDL_NumJoysticks() > 0)
{
// Load joystick names and open all connected joysticks
for (int i = 0 ; i < SDL_NumJoysticks(); i++)
{
SDL_Joystick *joy = SDL_JoystickOpen(i);
#if defined(TARGET_DARWIN)
// On OS X, the 360 controllers are handled externally, since the SDL code is
// really buggy and doesn't handle disconnects.
if (std::string(SDL_JoystickName(i)).find("360") != std::string::npos)
{
isyslog("Ignoring joystick: %s", SDL_JoystickName(i));
continue;
}
#endif
if (joy)
{
// Some (Microsoft) Keyboards are recognized as Joysticks by modern kernels
// Don't enumerate them
// https://bugs.launchpad.net/ubuntu/+source/linux/+bug/390959
// NOTICE: Enabled Joystick: Microsoft Wired Keyboard 600
// Details: Total Axis: 37 Total Hats: 0 Total Buttons: 57
// NOTICE: Enabled Joystick: Microsoft Microsoft® 2.4GHz Transceiver v6.0
// Details: Total Axis: 37 Total Hats: 0 Total Buttons: 57
int num_axis = SDL_JoystickNumAxes(joy);
int num_buttons = SDL_JoystickNumButtons(joy);
if (num_axis > 20 && num_buttons > 50)
{
isyslog("Your Joystick seems to be a Keyboard, ignoring it: %s Axis: %d Buttons: %d",
SDL_JoystickName(i), num_axis, num_buttons);
}
else
{
CJoystick* joystick = new CJoystickSDL(joy, this);
joystick->SetName(SDL_JoystickName(i));
joystick->SetButtonCount(SDL_JoystickNumButtons(joy));
joystick->SetHatCount(SDL_JoystickNumHats(joy));
joystick->SetAxisCount(SDL_JoystickNumAxes(joy));
joysticks.push_back(joystick);
/* TODO
isyslog("Enabled Joystick: \"%s\" (SDL)", joystick.m_configuration.Name().c_str());
isyslog("Details: Total Axes: %d Total Hats: %d Total Buttons: %d",
joystick.m_configuration.AxisIndexes().size(),
joystick.m_configuration.HatIndexes().size(),
joystick.m_configuration.ButtonIndexes().size());;
*/
}
}
}
}
// Disable joystick events, since we'll be polling them
SDL_JoystickEventState(SDL_DISABLE);
return true;
}
