47 lines
1.5 KiB
C++
47 lines
1.5 KiB
C++
//
|
|
// Created by tlab-uav on 24-9-13.
|
|
//
|
|
|
|
#include "joystick_input/JoystickInput.h"
|
|
|
|
using std::placeholders::_1;
|
|
|
|
JoystickInput::JoystickInput() : Node("joysick_input_node") {
|
|
publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
|
|
subscription_ = create_subscription<
|
|
sensor_msgs::msg::Joy>("joy", 10, std::bind(&JoystickInput::joy_callback, this, _1));
|
|
}
|
|
|
|
void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
|
|
if (msg->buttons[1] && msg->buttons[4]) {
|
|
inputs_.command = 1; // LB + B
|
|
} else if (msg->buttons[0] && msg->buttons[4]) {
|
|
inputs_.command = 2; // LB + A
|
|
} else if (msg->buttons[2] && msg->buttons[4]) {
|
|
inputs_.command = 3; // LB + X
|
|
} else if (msg->buttons[7]) {
|
|
inputs_.command = 4; // START
|
|
} else if (msg->axes[2] != 1 && msg->buttons[2]) {
|
|
inputs_.command = 10; // LT + X
|
|
} else if (msg->axes[2] != 1 && msg->buttons[0]) {
|
|
inputs_.command = 9; // LT + A
|
|
} else if (msg->axes[2] != 1 && msg->buttons[3]) {
|
|
inputs_.command = 8; // LT + Y
|
|
} else {
|
|
inputs_.command = 0;
|
|
inputs_.lx = -msg->axes[0];
|
|
inputs_.ly = msg->axes[1];
|
|
inputs_.rx = -msg->axes[3];
|
|
inputs_.ry = msg->axes[4];
|
|
}
|
|
publisher_->publish(inputs_);
|
|
}
|
|
|
|
int main(int argc, char *argv[]) {
|
|
rclcpp::init(argc, argv);
|
|
auto node = std::make_shared<JoystickInput>();
|
|
spin(node);
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|