98 lines
2.8 KiB
C++
98 lines
2.8 KiB
C++
//
|
|
// Created by tlab-uav on 24-9-13.
|
|
//
|
|
|
|
#include "unitree_joystick_input/JoystickInput.h"
|
|
#define TOPIC_JOYSTICK "rt/wirelesscontroller"
|
|
|
|
using std::placeholders::_1;
|
|
|
|
using namespace unitree::robot;
|
|
|
|
JoystickInput::JoystickInput() : Node("unitree_joysick_node")
|
|
{
|
|
publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
|
|
|
|
declare_parameter("network_interface", network_interface_);
|
|
declare_parameter("domain", domain_);
|
|
|
|
network_interface_ = get_parameter("network_interface").as_string();
|
|
domain_ = get_parameter("domain").as_int();
|
|
RCLCPP_INFO(get_logger(), " network_interface: %s, domain: %d", network_interface_.c_str(), domain_);
|
|
ChannelFactory::Instance()->Init(domain_, network_interface_);
|
|
|
|
subscriber_ = std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>>(
|
|
TOPIC_JOYSTICK);
|
|
subscriber_->InitChannel(
|
|
[this](auto&& PH1)
|
|
{
|
|
remoteWirelessHandle(std::forward<decltype(PH1)>(PH1));
|
|
},
|
|
1);
|
|
}
|
|
|
|
void JoystickInput::remoteWirelessHandle(const void* messages)
|
|
{
|
|
wireless_controller_ = *static_cast<const unitree_go::msg::dds_::WirelessController_*>(messages);
|
|
xKeySwitchUnion_.value = wireless_controller_.keys();
|
|
|
|
if (xKeySwitchUnion_.components.select)
|
|
{
|
|
inputs_.command = 1;
|
|
}
|
|
else if (xKeySwitchUnion_.components.start)
|
|
{
|
|
inputs_.command = 2;
|
|
}
|
|
else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.B)
|
|
{
|
|
inputs_.command = 3;
|
|
}
|
|
else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.A)
|
|
{
|
|
inputs_.command = 4;
|
|
}
|
|
else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.X)
|
|
{
|
|
inputs_.command = 5;
|
|
}
|
|
else if (xKeySwitchUnion_.components.right and xKeySwitchUnion_.components.Y)
|
|
{
|
|
inputs_.command = 6;
|
|
}
|
|
else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.B)
|
|
{
|
|
inputs_.command = 7;
|
|
}
|
|
else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.A)
|
|
{
|
|
inputs_.command = 8;
|
|
}
|
|
else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.X)
|
|
{
|
|
inputs_.command = 9;
|
|
}
|
|
else if (xKeySwitchUnion_.components.left and xKeySwitchUnion_.components.Y)
|
|
{
|
|
inputs_.command = 10;
|
|
}
|
|
else
|
|
{
|
|
inputs_.command = 0;
|
|
inputs_.lx = wireless_controller_.lx();
|
|
inputs_.ly = wireless_controller_.ly();
|
|
inputs_.rx = wireless_controller_.rx();
|
|
inputs_.ry = wireless_controller_.ry();
|
|
}
|
|
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;
|
|
}
|