반응형
안녕하세요 여러분.
크리에이트메이커 입니다.
무인로봇 조종 의뢰 막바지로 조이스틱을 C++에서 연결하고 ROS2통신하는 기능을 구현하였습니다.
logitech Exteme 3d pro 조이스틱을 연동하는 코드인데요.
원낙 자료가 없어서, 여기저기 문서보고 chatGPT검색하면서 구현했습니다.
복붙해서 사용하시면 되구요.
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joy.hpp"
#include <windows.h>
#include <dinput.h>
#include <iostream>
#pragma comment(lib, "dinput8.lib")
#pragma comment(lib, "dxguid.lib")
class JoystickNode : public rclcpp::Node
{
public:
JoystickNode()
: Node("joystick_node")
{
joy_publisher_ = this->create_publisher<sensor_msgs::msg::Joy>("/joy", 100);
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), [this]()
{
sensor_msgs::msg::Joy joy_msg;
if (readJoystickState(joy_msg)) {
joy_publisher_->publish(joy_msg);
} });
}
private:
static BOOL CALLBACK EnumJoysticksCallback(const DIDEVICEINSTANCE *pdidInstance, VOID *pContext)
{
auto self = static_cast<JoystickNode *>(pContext);
if (FAILED(self->direct_input->CreateDevice(pdidInstance->guidInstance, &self->joystick, NULL))) // Modified
{
RCLCPP_ERROR(rclcpp::get_logger("logger_name"), "Failed to create joystick device");
self->direct_input->Release(); // Modified
return false;
}
if (FAILED(self->joystick->SetCooperativeLevel(NULL, DISCL_BACKGROUND | DISCL_NONEXCLUSIVE))) // Modified
{
RCLCPP_ERROR(rclcpp::get_logger("logger_name"), "Failed to set cooperative level for joystick");
self->joystick->Release(); // Modifie
self->direct_input->Release();
return false;
}
if (FAILED(self->joystick->SetDataFormat(&c_dfDIJoystick2))) // Modified
{
RCLCPP_ERROR(rclcpp::get_logger("logger_name"), "Failed to set data format for joystick");
self->joystick->Release(); // Modified
self->direct_input->Release();
return false;
}
if (FAILED(self->joystick->Acquire()))
{
RCLCPP_ERROR(rclcpp::get_logger("logger_name"), "Failed to acquire joystick");
self->joystick->Release();
self->direct_input->Release();
return false;
}
if (pdidInstance->tszProductName == "Logitech Extreme 3D")
{
return DIENUM_STOP;
}
else
{
return DIENUM_CONTINUE;
}
}
bool readJoystickState(sensor_msgs::msg::Joy &joy_msg)
{
// DirectInput 초기화
if (FAILED(DirectInput8Create(GetModuleHandle(NULL), DIRECTINPUT_VERSION, IID_IDirectInput8, (void **)&direct_input, NULL)))
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize DirectInput");
return false;
}
if (FAILED(direct_input->EnumDevices(DI8DEVCLASS_GAMECTRL, EnumJoysticksCallback, this, DIEDFL_ATTACHEDONLY)))
{
std::cerr << "Failed to enumerate joysticks" << std::endl;
direct_input->Release();
return 1;
}
DIJOYSTATE2 joy_state;
HRESULT hr = joystick->GetDeviceState(sizeof(DIJOYSTATE2), &joy_state);
if (FAILED(hr))
{
switch (hr)
{
case DIERR_INPUTLOST:
std::cerr << "DirectInput lost access to the device" << std::endl;
break;
case DIERR_NOTACQUIRED:
std::cerr << "Device is not acquired" << std::endl;
break;
case DIERR_NOTINITIALIZED:
std::cerr << "DirectInput has not been initialized" << std::endl;
break;
case DIERR_OTHERAPPHASPRIO:
std::cerr << "Another application has a higher priority level, preventing access to the device" << std::endl;
break;
case DIERR_INVALIDPARAM:
std::cerr << "Invalid parameter passed to the function" << std::endl;
break;
default:
std::cerr << "Unknown error occurred: " << std::hex << hr << std::endl;
break;
}
return false;
}
// 조이스틱 입력 메시지 구성
joy_msg.header.stamp = this->now();
joy_msg.header.frame_id = "joystick";
// std::cout << "DIJOYSTATE2 struct contents:" << std::endl;
// std::cout << "lX: " << joy_state.lX << std::endl;
// std::cout << "lY: " << joy_state.lY << std::endl;
// std::cout << "lZ: " << joy_state.lZ << std::endl;
// std::cout << "lRx: " << joy_state.lRx << std::endl;
// std::cout << "lRy: " << joy_state.lRy << std::endl;
// std::cout << "lRz: " << joy_state.lRz << std::endl;
//std::cout << "rglSlider[0]: " << joy_state.rglSlider[0] << std::endl;
// std::cout << "rglSlider[1]: " << joy_state.rglSlider[1] << std::endl;
// std::cout << "rgdwPOV[0]: " << joy_state.rgdwPOV[0] << std::endl;
// std::cout << "rgdwPOV[1]: " << joy_state.rgdwPOV[1] << std::endl;
// std::cout << "rgdwPOV[2]: " << joy_state.rgdwPOV[2] << std::endl;
// std::cout << "rgdwPOV[3]: " << joy_state.rgdwPOV[3] << std::endl;
joystick->Unacquire();
joystick->Release();
direct_input->Release();
return true;
}
rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
IDirectInput8 *direct_input;
IDirectInputDevice8 *joystick;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<JoystickNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
연동 과정중 가장 중요한점은 저 연결하는데 필요한 함수들입니다.
자세히 보시면 순서가 있어요. 저 순서대로 하셔야 마지막에 Aquire가 실행되고 가장중요한
조이스틱의 실시간 정보를 받고 출력하고 반영하는 getdevicestate가 실행이 됩니다.
ros2 통신을 하는데 필요한 msg기능은 해당 코드를 기반으로 알아서 짜서 쓰시면 될것 같습니다 ~그리고 혹시
저 lib파일이 foxy에 없어서 진행이 안되시면 그냥 directx최신거 받아서 lib파일 찾아서 foxy에 넣으세용.
전 저거 연구하느라 2일 꼬박 고생했네용.. 함수 순서가 중요합니다. 순서가..
알고나면 리눅스 보다 사용하기 훨씬 편합니다. 윈도우가요. 그럼 즐거운 개발되세용~
반응형