multithread.cpp
////////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2001-2023 Force Dimension, Switzerland.
// All Rights Reserved.
//
// Force Dimension SDK 3.17.1
//
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///
/// This example illustrates how to write a multithreaded application that runs
/// one haptic control loop for each available Force Dimension haptic device,
/// each in its own thread.
///
/// It starts by opening a connection to all available haptic devices, and then
/// starts a gravity compensation loop in a separate thread for each of them.
/// All threads execute independently until the user requests the application
/// to exit.
///
////////////////////////////////////////////////////////////////////////////////
// C++ library headers
#include <atomic>
#include <iomanip>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
// Force Dimension SDK library header
#include "dhdc.h"
/// Global running flag shared by all threads
std::atomic<bool> s_running { true };
/// Global running threads count
std::atomic<size_t> s_runningThreadsCount { 0 };
////////////////////////////////////////////////////////////////////////////////
///
/// This function implements a simple haptic thread that puts a given haptic
/// device, identified by then handle passed as argument, into gravity
/// compensation.
///
/// \param a_arg
/// A pointer to the device ID that identifies the haptic device
///
////////////////////////////////////////////////////////////////////////////////
void* gravityCompensationThread(void* a_arg)
{
// Increment the global running thread counter.
s_runningThreadsCount += 1;
// Retrieve the device index as argument.
int deviceId = *(reinterpret_cast<int*>(a_arg));
// Enable force rendering on the haptic device.
if (dhdEnableForce(DHD_ON, deviceId) < 0)
{
std::cout << "error: failed to enable force rendering (" << dhdErrorGetLastStr() << ")" << std::endl;
s_running = false;
}
// Run haptic loop.
while (s_running)
{
// Apply null force.
if (dhdSetForceAndTorqueAndGripperForce (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, deviceId) < DHD_NO_ERROR)
{
std::cout << "error: failed to set force on device id " << deviceId << " (" << dhdErrorGetLastStr() << ")" << std::endl;
s_running = false;
break;
}
}
// Close the connection to the haptic device.
if (dhdClose(deviceId) < 0)
{
std::cout << "warning: failed to close the connection on device id " << deviceId << "(" << dhdErrorGetLastStr() << ")" << std::endl;
}
// Decrement the global running thread counter.
s_runningThreadsCount -= 1;
return nullptr;
}
////////////////////////////////////////////////////////////////////////////////
int main(int argc,
char* argv[])
{
// Optimize console output performance.
std::cout.rdbuf()->pubsetbuf(nullptr, 512);
std::cout << std::nounitbuf;
std::ios_base::sync_with_stdio(false);
// Display version information.
std::cout << "Multithreaded Gravity Compensation Example " << dhdGetSDKVersionStr() << std::endl;
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
// check for devices (limit to 10 devices)
int deviceCount = dhdGetDeviceCount();
if (deviceCount < 1)
{
std::cout << "error: no device found (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Display the device type.
std::cout << deviceCount << " device(s) detected" << std::endl;
// Allocate resources and start threads.
std::vector<int> deviceIdsList;
for (int index = 0; index < deviceCount; index++ )
{
int deviceId = dhdOpenID(index);
if (deviceId >= 0)
{
uint16_t serialNumber = 0;
std::string serialNumberString;
if (dhdGetSerialNumber(&serialNumber, deviceId) == DHD_NO_ERROR)
{
std::ostringstream serialNumberStringStream;
serialNumberStringStream << "S/N " << serialNumber;
serialNumberString = serialNumberStringStream.str();
}
std::cout << "starting gravity compensation thread for " << dhdGetSystemName() << " " << serialNumberString << std::endl;
deviceIdsList.push_back(deviceId);
if (dhdStartThread(gravityCompensationThread, &(deviceIdsList.back()), DHD_THREAD_PRIORITY_HIGH) < 0)
{
std::cout << "error: failed to start thread for " << dhdGetSystemName() << " (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
}
}
// Wait for all threads to start.
while (s_runningThreadsCount < deviceIdsList.size())
{
dhdSleep(0.1);
// Detect a failure in any of the threads.
if (!s_running)
{
std::cout << "error: at least one thread quit with an error" << std::endl;
dhdSleep(2.0);
return -1;
}
}
// Display user instructions.
std::cout << "press 'q' to quit" << std::endl << std::endl;
// UI thread (default priority)
while (s_running)
{
// Do not hog a CPU core.
dhdSleep(0.1);
// Process user input.
if (dhdKbHit())
{
switch (dhdKbGet())
{
// Quit the application.
case 'q':
{
s_running = false;
break;
}
}
}
}
// Wait for all threads to finish.
while (s_runningThreadsCount > 0)
{
dhdSleep(0.1);
}
// Report success.
std::cout << "all threads finished" << std::endl;
return 0;
}
DHD header file.
char __SDK dhdKbGet()
Definition: dhdc.cpp:4743
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID=-1)
Definition: dhdc.cpp:2040
int __SDK dhdOpenID(char ID)
Definition: dhdc.cpp:411
int __SDK dhdEnableForce(uchar val, char ID=-1)
Definition: dhdc.cpp:4378
int __SDK dhdGetSerialNumber(ushort *sn, char ID=-1)
Definition: dhdc.cpp:540
int __SDK dhdClose(char ID=-1)
Definition: dhdc.cpp:443
int __SDK dhdStartThread(void *func(void *), void *arg, int priority)
Definition: dhdc.cpp:4778
void __SDK dhdSleep(double sec)
Definition: dhdc.cpp:4754
int __SDK dhdGetDeviceCount()
Definition: dhdc.cpp:226
const char *__SDK dhdGetSDKVersionStr()
Definition: dhdc.cpp:925
@ DHD_NO_ERROR
Definition: dhdc.h:52
const char *__SDK dhdErrorGetLastStr()
Definition: dhdError.cpp:197
#define DHD_THREAD_PRIORITY_HIGH
Definition: dhdc.h:210
const char *__SDK dhdGetSystemName(char ID=-1)
Definition: dhdc.cpp:814
bool __SDK dhdKbHit()
Definition: dhdc.cpp:4731
#define DHD_ON
Definition: dhdc.h:128