From b92ec169625ad1b178e8c19a04114d8de024e95b Mon Sep 17 00:00:00 2001 From: yangning wu Date: Thu, 23 May 2024 11:30:21 +0800 Subject: [PATCH] add joystick support of cpp simulation --- readme.md | 12 ++ readme_zh.md | 4 + simulate/CMakeLists.txt | 2 + simulate/config.yaml | 7 +- simulate/src/joystick/LICENSE-2.0.txt | 202 ++++++++++++++++++ simulate/src/joystick/joystick.cc | 88 ++++++++ simulate/src/joystick/joystick.h | 164 ++++++++++++++ simulate/src/joystick/jstest.cc | 101 +++++++++ simulate/src/main.cc | 19 +- .../unitree_sdk2_bridge.cc | 104 ++++++++- .../unitree_sdk2_bridge/unitree_sdk2_bridge.h | 67 +++++- 11 files changed, 763 insertions(+), 7 deletions(-) create mode 100644 simulate/src/joystick/LICENSE-2.0.txt create mode 100755 simulate/src/joystick/joystick.cc create mode 100755 simulate/src/joystick/joystick.h create mode 100755 simulate/src/joystick/jstest.cc diff --git a/readme.md b/readme.md index 7c7c638..d35e12c 100644 --- a/readme.md +++ b/readme.md @@ -97,6 +97,12 @@ Refer to: https://github.com/unitreerobotics/unitree_sdk2_python ```bash pip3 install mujoco ``` + +#### joystick +```bash +pip3 install pygame +``` + ### 2. Test ```bash cd ./simulate_python @@ -123,6 +129,12 @@ robot: "go2" robot_scene: "scene.xml" # DDS domain id, it is recommended to distinguish from the real robot (default is 0 on the real robot) domain_id: 1 + +use_joystick: 1 # Simulate Unitree WirelessController using a gamepad +joystick_type: "xbox" # support "xbox" and "switch" gamepad layout +joystick_device: "/dev/input/js0" # Device path +joystick_bits: 16 # Some game controllers may only have 8-bit accuracy + # Network interface name, for simulation, it is recommended to use the local loopback "lo" interface: "lo" # Whether to output robot link, joint, sensor information, 1 for output diff --git a/readme_zh.md b/readme_zh.md index 0cea818..52b45cc 100644 --- a/readme_zh.md +++ b/readme_zh.md @@ -103,6 +103,10 @@ Could not locate cyclonedds. Try to set CYCLONEDDS_HOME or CMAKE_PREFIX_PATH ```bash pip3 install mujoco ``` +#### joystick +```bash +pip3 install pygame +``` ### 2. 测试 ```bash cd ./simulate_python diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 9001d4d..a20f61d 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -10,6 +10,7 @@ include_directories(/usr/local/include/ddscxx /usr/local/include/iceoryx/v2.0.2) link_libraries(unitree_sdk2 ddsc ddscxx rt pthread) FILE (GLOB SIM_SRC + src/joystick/joystick.cc src/mujoco/*.cc src/unitree_sdk2_bridge/*.cc) @@ -24,5 +25,6 @@ add_executable(unitree_mujoco ${SIM_SRC} src/main.cc) target_link_libraries(unitree_mujoco ${SIM_DEPENDENCIES}) add_executable(test test/test_unitree_sdk2.cpp) +add_executable(jstest src/joystick/jstest.cc src/joystick/joystick.cc) SET(CMAKE_BUILD_TYPE Release) \ No newline at end of file diff --git a/simulate/config.yaml b/simulate/config.yaml index 94d7a92..81774b8 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -4,6 +4,11 @@ robot_scene: "scene.xml" # Robot scene domain_id: 1 # Domain id interface: "lo" # Interface +use_joystick: 1 # Simulate Unitree WirelessController using a gamepad +joystick_type: "xbox" # support "xbox" and "switch" gamepad layout +joystick_device: "/dev/input/js0" # Device path +joystick_bits: 16 # Some game controllers may only have 8-bit accuracy + print_scene_information: 1 # Print link, joint and sensors information of robot -enable_elastic_band: 0 # Virtual spring band, used for lifting h1 \ No newline at end of file +enable_elastic_band: 0 # Virtual spring band, used for lifting h1 diff --git a/simulate/src/joystick/LICENSE-2.0.txt b/simulate/src/joystick/LICENSE-2.0.txt new file mode 100644 index 0000000..d319668 --- /dev/null +++ b/simulate/src/joystick/LICENSE-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2002-2016 Drew Noakes + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/simulate/src/joystick/joystick.cc b/simulate/src/joystick/joystick.cc new file mode 100755 index 0000000..7c904ca --- /dev/null +++ b/simulate/src/joystick/joystick.cc @@ -0,0 +1,88 @@ +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Copyright Drew Noakes 2013-2016 + +#include "joystick.h" + +Joystick::Joystick() +{ + openPath("/dev/input/js0"); +} + +Joystick::Joystick(int joystickNumber) +{ + std::stringstream sstm; + sstm << "/dev/input/js" << joystickNumber; + openPath(sstm.str()); +} + +Joystick::Joystick(std::string devicePath) +{ + openPath(devicePath); +} + +Joystick::Joystick(std::string devicePath, bool blocking) +{ + openPath(devicePath, blocking); +} + +void Joystick::openPath(std::string devicePath, bool blocking) +{ + // Open the device using either blocking or non-blocking + _fd = open(devicePath.c_str(), blocking ? O_RDONLY : O_RDONLY | O_NONBLOCK); +} + +bool Joystick::sample(JoystickEvent *event) +{ + int bytes = read(_fd, event, sizeof(*event)); + + if (bytes == -1) + return false; + + // NOTE if this condition is not met, we're probably out of sync and this + // Joystick instance is likely unusable + return bytes == sizeof(*event); +} + +bool Joystick::isFound() +{ + return _fd >= 0; +} + +void Joystick::getState() +{ + if (sample(&event_)) + { + if (event_.isButton()) + { + button_[event_.number] = event_.value; + } + else if (event_.isAxis()) + { + axis_[event_.number] = event_.value; + } + } +} + +Joystick::~Joystick() +{ + close(_fd); +} + +std::ostream &operator<<(std::ostream &os, const JoystickEvent &e) +{ + os << "type=" << static_cast(e.type) + << " number=" << static_cast(e.number) + << " value=" << static_cast(e.value); + return os; +} diff --git a/simulate/src/joystick/joystick.h b/simulate/src/joystick/joystick.h new file mode 100755 index 0000000..1cee5a1 --- /dev/null +++ b/simulate/src/joystick/joystick.h @@ -0,0 +1,164 @@ +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Copyright Drew Noakes 2013-2016 + +#ifndef __JOYSTICK_H__ +#define __JOYSTICK_H__ + +#include +#include +#include +#include +#include +#include +#include +#include "unistd.h" + + +#define JS_EVENT_BUTTON 0x01 // button pressed/released +#define JS_EVENT_AXIS 0x02 // joystick moved +#define JS_EVENT_INIT 0x80 // initial state of device + +class JoystickEvent +{ +public: + /** Minimum value of axes range */ + static const short MIN_AXES_VALUE = -32768; + + /** Maximum value of axes range */ + static const short MAX_AXES_VALUE = 32767; + + /** + * The timestamp of the event, in milliseconds. + */ + unsigned int time; + + /** + * The value associated with this joystick event. + * For buttons this will be either 1 (down) or 0 (up). + * For axes, this will range between MIN_AXES_VALUE and MAX_AXES_VALUE. + */ + short value; + + /** + * The event type. + */ + unsigned char type; + + /** + * The axis/button number. + */ + unsigned char number; + + /** + * Returns true if this event is the result of a button press. + */ + bool isButton() + { + return (type & JS_EVENT_BUTTON) != 0; + } + + /** + * Returns true if this event is the result of an axis movement. + */ + bool isAxis() + { + return (type & JS_EVENT_AXIS) != 0; + } + + /** + * Returns true if this event is part of the initial state obtained when + * the joystick is first connected to. + */ + bool isInitialState() + { + return (type & JS_EVENT_INIT) != 0; + } + + /** + * The ostream inserter needs to be a friend so it can access the + * internal data structures. + */ + friend std::ostream &operator<<(std::ostream &os, const JoystickEvent &e); +}; + +/** + * Stream insertion function so you can do this: + * cout << event << endl; + */ +std::ostream &operator<<(std::ostream &os, const JoystickEvent &e); + +/** + * Represents a joystick device. Allows data to be sampled from it. + */ +class Joystick +{ +private: + void openPath(std::string devicePath, bool blocking = false); + int _fd; + +public: + ~Joystick(); + + /** + * Initialises an instance for the first joystick: /dev/input/js0 + */ + Joystick(); + + /** + * Initialises an instance for the joystick with the specified, + * zero-indexed number. + */ + Joystick(int joystickNumber); + + /** + * Initialises an instance for the joystick device specified. + */ + Joystick(std::string devicePath); + + /** + * Joystick objects cannot be copied + */ + Joystick(Joystick const &) = delete; + + /** + * Joystick objects can be moved + */ + Joystick(Joystick &&) = default; + + /** + * Initialises an instance for the joystick device specified and provide + * the option of blocking I/O. + */ + Joystick(std::string devicePath, bool blocking); + + /** + * Returns true if the joystick was found and may be used, otherwise false. + */ + bool isFound(); + + /** + * Attempts to populate the provided JoystickEvent instance with data + * from the joystick. Returns true if data is available, otherwise false. + */ + + void getState(); + + JoystickEvent event_; + int button_[20] = {0}; + int axis_[10] = {0}; + + bool sample(JoystickEvent *event); +}; + +#endif diff --git a/simulate/src/joystick/jstest.cc b/simulate/src/joystick/jstest.cc new file mode 100755 index 0000000..47f8d1c --- /dev/null +++ b/simulate/src/joystick/jstest.cc @@ -0,0 +1,101 @@ +#include +#include +#include +#include "joystick.h" + +#define GAMEPAD_TYPE 1 // 1: XBOX, 0: SWITCH +#define MAX_AXES_VALUE 32768 +#define MIN_AXES_VALUE -32768 +using namespace std; + +typedef union +{ + struct + { + uint8_t R1 : 1; + uint8_t L1 : 1; + uint8_t start : 1; + uint8_t select : 1; + uint8_t R2 : 1; + uint8_t L2 : 1; + uint8_t F1 : 1; + uint8_t F2 : 1; + uint8_t A : 1; + uint8_t B : 1; + uint8_t X : 1; + uint8_t Y : 1; + uint8_t up : 1; + uint8_t right : 1; + uint8_t down : 1; + uint8_t left : 1; + } components; + uint16_t value; +} xKeySwitchUnion; + +int main(int argc, char **argv) +{ + // Create an instance of Joystick + Joystick joystick("/dev/input/js0"); + + // Ensure that it was found and that we can use it + if (!joystick.isFound()) + { + printf("open failed.\n"); + exit(1); + } + + xKeySwitchUnion unitree_key; + map AxisId = + { + {"LX", 0}, // Left stick axis x + {"LY", 1}, // Left stick axis y + {"RX", 3}, // Right stick axis x + {"RY", 4}, // Right stick axis y + {"LT", 2}, // Left trigger + {"RT", 5}, // Right trigger + {"DX", 6}, // Directional pad x + {"DY", 7}, // Directional pad y + }; + + map ButtonId = + { + {"X", 2}, + {"Y", 3}, + {"B", 1}, + {"A", 0}, + {"LB", 4}, + {"RB", 5}, + {"SELECT", 6}, + {"START", 7}, + }; + + while (true) + { + + // Attempt to sample an event from the joystick + joystick.getState(); + + unitree_key.components.R1 = joystick.button_[ButtonId["RB"]]; + unitree_key.components.L1 = joystick.button_[ButtonId["LB"]]; + unitree_key.components.start = joystick.button_[ButtonId["START"]]; + unitree_key.components.select = joystick.button_[ButtonId["SELECT"]]; + unitree_key.components.R2 = (joystick.axis_[AxisId["RT"]] > 0); + unitree_key.components.L2 = (joystick.axis_[AxisId["LT"]] > 0); + unitree_key.components.F1 = 0; + unitree_key.components.F2 = 0; + unitree_key.components.A = joystick.button_[ButtonId["A"]]; + unitree_key.components.B = joystick.button_[ButtonId["B"]]; + unitree_key.components.X = joystick.button_[ButtonId["X"]]; + unitree_key.components.Y = joystick.button_[ButtonId["Y"]]; + unitree_key.components.up = (joystick.axis_[AxisId["DY"]] < 0); + unitree_key.components.right = (joystick.axis_[AxisId["DX"]] > 0); + unitree_key.components.down = (joystick.axis_[AxisId["DY"]] > 0); + unitree_key.components.left = (joystick.axis_[AxisId["DX"]] < 0); + + cout << unitree_key.value << endl; + + // Restrict rate + usleep(10000); + } + return 0; +}; diff --git a/simulate/src/main.cc b/simulate/src/main.cc index 9d0b8fb..420d0b3 100755 --- a/simulate/src/main.cc +++ b/simulate/src/main.cc @@ -72,6 +72,11 @@ namespace int domain_id = 1; std::string interface = "lo"; + int use_joystick = 0; + std::string joystick_type = "xbox"; + std::string joystick_device = "/dev/input/js0"; + int joystick_bits = 16; + int print_scene_information = 1; int enable_elastic_band = 0; @@ -559,7 +564,8 @@ void *UnitreeSdk2BridgeThread(void *arg) } usleep(500000); } - if (config.robot=="h1") + + if (config.robot == "h1") { config.band_attached_link = 6 * mj_name2id(m, mjOBJ_BODY, "torso_link"); } @@ -567,9 +573,15 @@ void *UnitreeSdk2BridgeThread(void *arg) { config.band_attached_link = 6 * mj_name2id(m, mjOBJ_BODY, "base_link"); } + ChannelFactory::Instance()->Init(config.domain_id, config.interface); UnitreeSdk2Bridge unitree_interface(m, d); + if (config.use_joystick == 1) + { + unitree_interface.SetupJoystick(config.joystick_device, config.joystick_type, config.joystick_bits); + } + if (config.print_scene_information == 1) { unitree_interface.PrintSceneInformation(); @@ -636,6 +648,11 @@ int main(int argc, char **argv) config.interface = yaml_node["interface"].as(); config.print_scene_information = yaml_node["print_scene_information"].as(); config.enable_elastic_band = yaml_node["enable_elastic_band"].as(); + config.use_joystick = yaml_node["use_joystick"].as(); + config.joystick_type = yaml_node["joystick_type"].as(); + config.joystick_device = yaml_node["joystick_device"].as(); + config.joystick_bits = yaml_node["joystick_bits"].as(); + sim->use_elastic_band_ = config.enable_elastic_band; yaml_node.~Node(); diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc index 914c8b7..ec6058a 100644 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc @@ -8,16 +8,23 @@ UnitreeSdk2Bridge::UnitreeSdk2Bridge(mjModel *model, mjData *data) : mj_model_(m high_state_puber_.reset(new ChannelPublisher(TOPIC_HIGHSTATE)); high_state_puber_->InitChannel(); + wireless_controller_puber_.reset(new ChannelPublisher(TOPIC_WIRELESS_CONTROLLER)); + wireless_controller_puber_->InitChannel(); + low_cmd_suber_.reset(new ChannelSubscriber(TOPIC_LOWCMD)); low_cmd_suber_->InitChannel(bind(&UnitreeSdk2Bridge::LowCmdHandler, this, placeholders::_1), 1); lowStatePuberThreadPtr = CreateRecurrentThreadEx("lowstate", UT_CPU_ID_NONE, 2000, &UnitreeSdk2Bridge::PublishLowState, this); HighStatePuberThreadPtr = CreateRecurrentThreadEx("highstate", UT_CPU_ID_NONE, 2000, &UnitreeSdk2Bridge::PublishHighState, this); + WirelessControllerPuberThreadPtr = CreateRecurrentThreadEx("wirelesscontroller", UT_CPU_ID_NONE, 2000, &UnitreeSdk2Bridge::PublishWirelessController, this); CheckSensor(); } -UnitreeSdk2Bridge::~UnitreeSdk2Bridge(){}; +UnitreeSdk2Bridge::~UnitreeSdk2Bridge() +{ + delete js_; +} void UnitreeSdk2Bridge::LowCmdHandler(const void *msg) { @@ -79,7 +86,39 @@ void UnitreeSdk2Bridge::PublishHighState() high_state_puber_->Write(high_state); } -}; +} + +void UnitreeSdk2Bridge::PublishWirelessController() +{ + if (js_) + { + js_->getState(); + dds_keys_.components.R1 = js_->button_[js_id_.button["RB"]]; + dds_keys_.components.L1 = js_->button_[js_id_.button["LB"]]; + dds_keys_.components.start = js_->button_[js_id_.button["START"]]; + dds_keys_.components.select = js_->button_[js_id_.button["SELECT"]]; + dds_keys_.components.R2 = (js_->axis_[js_id_.axis["RT"]] > 0); + dds_keys_.components.L2 = (js_->axis_[js_id_.axis["LT"]] > 0); + dds_keys_.components.F1 = 0; + dds_keys_.components.F2 = 0; + dds_keys_.components.A = js_->button_[js_id_.button["A"]]; + dds_keys_.components.B = js_->button_[js_id_.button["B"]]; + dds_keys_.components.X = js_->button_[js_id_.button["X"]]; + dds_keys_.components.Y = js_->button_[js_id_.button["Y"]]; + dds_keys_.components.up = (js_->axis_[js_id_.axis["DY"]] < 0); + dds_keys_.components.right = (js_->axis_[js_id_.axis["DX"]] > 0); + dds_keys_.components.down = (js_->axis_[js_id_.axis["DY"]] > 0); + dds_keys_.components.left = (js_->axis_[js_id_.axis["DX"]] < 0); + + wireless_controller.lx() = double(js_->axis_[js_id_.axis["LX"]]) / max_value_; + wireless_controller.ly() = -double(js_->axis_[js_id_.axis["LY"]]) / max_value_; + wireless_controller.rx() = double(js_->axis_[js_id_.axis["RX"]]) / max_value_; + wireless_controller.ry() = -double(js_->axis_[js_id_.axis["RY"]]) / max_value_; + wireless_controller.keys() = dds_keys_.value; + + wireless_controller_puber_->Write(wireless_controller); + } +} void UnitreeSdk2Bridge::Run() { @@ -87,7 +126,64 @@ void UnitreeSdk2Bridge::Run() { sleep(2); } -}; +} + +void UnitreeSdk2Bridge::SetupJoystick(string device, string js_type, int bits) +{ + js_ = new Joystick(device); + if (!js_->isFound()) + { + cout << "Error: Joystick open failed." << endl; + exit(1); + } + + max_value_ = (1 << (bits - 1)); + + if (js_type == "xbox") + { + js_id_.axis["LX"] = 0; // Left stick axis x + js_id_.axis["LY"] = 1; // Left stick axis y + js_id_.axis["RX"] = 3; // Right stick axis x + js_id_.axis["RY"] = 4; // Right stick axis y + js_id_.axis["LT"] = 2; // Left trigger + js_id_.axis["RT"] = 5; // Right trigger + js_id_.axis["DX"] = 6; // Directional pad x + js_id_.axis["DY"] = 7; // Directional pad y + + js_id_.button["X"] = 2; + js_id_.button["Y"] = 3; + js_id_.button["B"] = 1; + js_id_.button["A"] = 0; + js_id_.button["LB"] = 4; + js_id_.button["RB"] = 5; + js_id_.button["SELECT"] = 6; + js_id_.button["START"] = 7; + } + else if (js_type == "switch") + { + js_id_.axis["LX"] = 0; // Left stick axis x + js_id_.axis["LY"] = 1; // Left stick axis y + js_id_.axis["RX"] = 2; // Right stick axis x + js_id_.axis["RY"] = 3; // Right stick axis y + js_id_.axis["LT"] = 5; // Left trigger + js_id_.axis["RT"] = 4; // Right trigger + js_id_.axis["DX"] = 6; // Directional pad x + js_id_.axis["DY"] = 7; // Directional pad y + + js_id_.button["X"] = 3; + js_id_.button["Y"] = 4; + js_id_.button["B"] = 1; + js_id_.button["A"] = 0; + js_id_.button["LB"] = 6; + js_id_.button["RB"] = 7; + js_id_.button["SELECT"] = 10; + js_id_.button["START"] = 11; + } + else + { + cout << "Unsupported gamepad." << endl; + } +} void UnitreeSdk2Bridge::PrintSceneInformation() { @@ -148,7 +244,7 @@ void UnitreeSdk2Bridge::PrintSceneInformation() index = index + mj_model_->sensor_dim[i]; } cout << endl; -}; +} void UnitreeSdk2Bridge::CheckSensor() { diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h index a1d1d4b..84352c0 100755 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h @@ -10,8 +10,9 @@ #include #include #include - +#include #include +#include "../joystick/joystick.h" using namespace unitree::common; using namespace unitree::robot; @@ -20,8 +21,62 @@ using namespace std; #define TOPIC_LOWSTATE "rt/lowstate" #define TOPIC_HIGHSTATE "rt/sportmodestate" #define TOPIC_LOWCMD "rt/lowcmd" +#define TOPIC_WIRELESS_CONTROLLER "rt/wirelesscontroller" #define MOTOR_SENSOR_NUM 3 +typedef union +{ + struct + { + uint8_t R1 : 1; + uint8_t L1 : 1; + uint8_t start : 1; + uint8_t select : 1; + uint8_t R2 : 1; + uint8_t L2 : 1; + uint8_t F1 : 1; + uint8_t F2 : 1; + uint8_t A : 1; + uint8_t B : 1; + uint8_t X : 1; + uint8_t Y : 1; + uint8_t up : 1; + uint8_t right : 1; + uint8_t down : 1; + uint8_t left : 1; + } components; + uint16_t value; +} xKeySwitchUnion; + +// Defaults to xbox gamepad +struct JoystickId +{ + + map axis = + { + {"LX", 0}, // Left stick axis x + {"LY", 1}, // Left stick axis y + {"RX", 3}, // Right stick axis x + {"RY", 4}, // Right stick axis y + {"LT", 2}, // Left trigger + {"RT", 5}, // Right trigger + {"DX", 6}, // Directional pad x + {"DY", 7}, // Directional pad y + }; + + map button = + { + {"X", 2}, + {"Y", 3}, + {"B", 1}, + {"A", 0}, + {"LB", 4}, + {"RB", 5}, + {"SELECT", 6}, + {"START", 7}, + }; +}; + class UnitreeSdk2Bridge { public: @@ -31,20 +86,30 @@ public: void LowCmdHandler(const void *msg); void PublishLowState(); void PublishHighState(); + void PublishWirelessController(); void Run(); void PrintSceneInformation(); void CheckSensor(); + void SetupJoystick(string device, string js_type, int bits); unitree_go::msg::dds_::LowState_ low_state{}; unitree_go::msg::dds_::SportModeState_ high_state{}; + unitree_go::msg::dds_::WirelessController_ wireless_controller{}; ChannelPublisherPtr low_state_puber_; ChannelPublisherPtr high_state_puber_; + ChannelPublisherPtr wireless_controller_puber_; ChannelSubscriberPtr low_cmd_suber_; ThreadPtr lowStatePuberThreadPtr; ThreadPtr HighStatePuberThreadPtr; + ThreadPtr WirelessControllerPuberThreadPtr; + + xKeySwitchUnion dds_keys_; + JoystickId js_id_; + Joystick *js_; + int max_value_ = (1 << 15); // 16 bits joystick mjData *mj_data_; mjModel *mj_model_;