diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..43cc8c8 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,13 @@ +### Description + +Please explain the changes you made, including a reference to the related issue if applicable. + +### Checklist +- [ ] I have formatted the code to pass the CI. +- [ ] Extend the test. + +### Screenshots / Images / Videos +Please add here all the media that shows the system in operation. + +### People involved +Please use @mentions to name people who have contributed to this PR. diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml new file mode 100644 index 0000000..24805c8 --- /dev/null +++ b/.github/workflows/humble-devel.yaml @@ -0,0 +1,30 @@ +name: humble + +on: + push: + branches: + - humble-devel + pull_request: + branches: + - humble-devel + schedule: + - cron: "0 7 * * 1" + +jobs: + ROS2: + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Setup ROS 2 environment + uses: ros-tooling/setup-ros@0.7.9 + with: + ros-distro: humble + + - name: Build ROS 2 package + uses: ros-tooling/action-ros-ci@0.3.13 + with: + package-name: go2_driver unitree_go unitree_api go2_interfaces + vcs-repo-file-url: https://raw.githubusercontent.com/Unitree-Go2-Robot/go2_driver/refs/heads/humble-devel/dependencies.repos + target-ros2-distro: humble diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml new file mode 100644 index 0000000..a199efe --- /dev/null +++ b/.github/workflows/humble.yaml @@ -0,0 +1,30 @@ +name: humble + +on: + push: + branches: + - humble + pull_request: + branches: + - humble + schedule: + - cron: "0 7 * * 1" + +jobs: + ROS2: + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Setup ROS 2 environment + uses: ros-tooling/setup-ros@0.7.9 + with: + ros-distro: humble + + - name: Build ROS 2 package + uses: ros-tooling/action-ros-ci@0.3.13 + with: + package-name: go2_driver unitree_go unitree_api go2_interfaces + vcs-repo-file-url: https://raw.githubusercontent.com/Unitree-Go2-Robot/go2_driver/refs/heads/humble/dependencies.repos + target-ros2-distro: humble diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cfb2ca..1334682 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +add_definitions(-DRCLCPP_ENABLE_TOPIC_STATISTICS=0) + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -16,6 +18,9 @@ find_package(unitree_go REQUIRED) find_package(unitree_api REQUIRED) find_package(tf2_ros REQUIRED) find_package(go2_interfaces REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) +find_package(OpenSSL REQUIRED) set(dependencies rclcpp @@ -28,15 +33,26 @@ set(dependencies unitree_api tf2_ros go2_interfaces + cv_bridge + OpenCV + OpenSSL ) include_directories(include) add_library(${PROJECT_NAME} SHARED src/go2_driver/go2_driver.cpp + src/go2_driver/modules/go2_camera.cpp + src/go2_driver/modules/go2_handle_services.cpp + src/go2_driver/modules/go2_tts.cpp + src/go2_driver/modules/go2_vui.cpp + src/go2_driver/modules/go2_odometry.cpp + src/go2_driver/modules/go2_joint_states.cpp + src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale OpenSSL::SSL OpenSSL::Crypto ${OpenCV_LIBS}) rclcpp_components_register_nodes(${PROJECT_NAME} "go2_driver::Go2Driver") @@ -53,7 +69,7 @@ install(DIRECTORY include/ DESTINATION include ) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/config/camera_calibration.yaml b/config/camera_calibration.yaml new file mode 100644 index 0000000..b08106d --- /dev/null +++ b/config/camera_calibration.yaml @@ -0,0 +1,19 @@ +go2_driver: + ros__parameters: + frame_id: "camera_frame" + distortion_model: "plumb_bob" + width: 1280 + height: 720 + d: [ -0.3899037066336244, 0.1788088900468458, 0.0002001278572207758, -0.000684749815310552, -0.044162166777284 ] + k: [ 806.0577552762542, 0, 632.4742958402456, 0, 805.5558008931949, 346.8794920158575, 0, 0, 1 ] + r: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ] + p: [ 806.0577552762542, 0, 632.4742958402456, 0, 0, 805.5558008931949, 346.8794920158575, 0, 0, 0, 1, 0 ] + binning_x: 0 + binning_y: 0 + roi: + x_offset: 0 + y_offset: 0 + height: 0 + width: 0 + do_rectify: false + diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..fcd25d7 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,13 @@ +repositories: + go2_interfaces: + type: git + url: https://github.com/Unitree-Go2-Robot/go2_interfaces.git + version: humble + unitree_api: + type: git + url: https://github.com/Unitree-Go2-Robot/unitree_api.git + version: main + unitree_go: + type: git + url: https://github.com/Unitree-Go2-Robot/unitree_go.git + version: main diff --git a/include/go2_driver/go2_api_id.hpp b/include/go2_driver/go2_api_id.hpp deleted file mode 100644 index e9517f5..0000000 --- a/include/go2_driver/go2_api_id.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// BSD 3-Clause License - -// Copyright (c) 2024, Intelligent Robotics Lab -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: - -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. - -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. - -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#ifndef GO2_DRIVER__GO2_API_ID_HPP -#define GO2_DRIVER__GO2_API_ID_HPP - -namespace go2_driver -{ - -enum class Mode -{ - Damp = 1001, - BalanceStand = 1002, - StopMove = 1003, - StandUp = 1004, - StandDown = 1005, - RecoveryStand = 1006, - Euler = 1007, - Move = 1008, - Sit = 1009, - RiseSit = 1010, - SwitchGait = 1011, - Trigger = 1012, - BodyHeight = 1013, - FootRaiseHeight = 1014, - SpeedLevel = 1015, - Hello = 1016, - Stretch = 1017, - TrajectoryFollow = 1018, - ContinuousGait = 1019, - Content = 1020, - Wallow = 1021, - Dance1 = 1022, - Dance2 = 1023, - GetBodyHeight = 1024, - GetFootRaiseHeight = 1025, - GetSpeedLevel = 1026, - SwitchJoystick = 1027, - Pose = 1028, - Scrape = 1029, - FrontFlip = 1030, - FrontJump = 1031, - FrontPounce = 1032, - WiggleHips = 1033, - GetState = 1034, - EconomicGait = 1035, - FingerHeart = 1036, -}; - -} // namespace go2_driver - -#endif // GO2_DRIVER__GO2_API_ID_HPP diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index aad7ff6..52335b8 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -1,148 +1,63 @@ -// BSD 3-Clause License - -// Copyright (c) 2024, Intelligent Robotics Lab -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: - -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. - -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. - -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// Copyright 2024 Intelligent Robotics Lab +// +// 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. #ifndef GO2_DRIVER__GO2_DRIVER_HPP_ #define GO2_DRIVER__GO2_DRIVER_HPP_ #include -#include -#include -#include -#include -#include -#include -#include -#include "unitree_go/msg/low_state.hpp" -#include "unitree_go/msg/imu_state.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "nlohmann/json.hpp" -#include "unitree_api/msg/request.hpp" -#include "go2_driver/go2_api_id.hpp" -#include "go2_interfaces/srv/body_height.hpp" -#include "go2_interfaces/srv/continuous_gait.hpp" -#include "go2_interfaces/srv/euler.hpp" -#include "go2_interfaces/srv/foot_raise_height.hpp" -#include "go2_interfaces/srv/mode.hpp" -#include "go2_interfaces/srv/pose.hpp" -#include "go2_interfaces/srv/speed_level.hpp" -#include "go2_interfaces/srv/switch_gait.hpp" -#include "go2_interfaces/srv/switch_joystick.hpp" +#include +#include "go2_driver/modules/go2_camera.hpp" +#include "go2_driver/modules/go2_handle_services.hpp" +#include "go2_driver/modules/go2_tts.hpp" +#include "go2_driver/modules/go2_vui.hpp" +#include "go2_driver/modules/go2_odometry.hpp" +#include "go2_driver/modules/go2_joint_states.hpp" +#include "go2_driver/modules/go2_switch_obstacles_avoidance.hpp" + namespace go2_driver { -class Go2Driver : public rclcpp::Node +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2Driver : public rclcpp_lifecycle::LifecycleNode { public: Go2Driver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); -private: - void publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - void publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg); - void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); - - void handleBodyHeight( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleContinuousGait( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleEuler( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); + CallbackReturnT on_configure(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_cleanup(const rclcpp_lifecycle::State &) override; + CallbackReturnT on_shutdown(const rclcpp_lifecycle::State &) override; - void handleFootRaiseHeight( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleMode( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handlePose( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleSpeedLevel( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleSwitchGait( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - void handleSwitchJoystick( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); - - rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Subscription::SharedPtr robot_pose_sub_; - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Subscription::SharedPtr low_state_sub_; - rclcpp::Subscription::SharedPtr cmd_vel_sub_; - - rclcpp::Publisher::SharedPtr pointcloud_pub_; - rclcpp::Publisher::SharedPtr joint_state_pub_; - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr imu_pub_; - rclcpp::Publisher::SharedPtr request_pub_; - - rclcpp::Service::SharedPtr set_body_height_service_; - rclcpp::Service::SharedPtr set_continuous_gait_service_; - rclcpp::Service::SharedPtr set_euler_service_; - rclcpp::Service::SharedPtr set_foot_raise_height_service_; - rclcpp::Service::SharedPtr set_mode_service_; - rclcpp::Service::SharedPtr set_pose_service_; - rclcpp::Service::SharedPtr set_speed_level_service_; - rclcpp::Service::SharedPtr set_switch_gait_service_; - rclcpp::Service::SharedPtr set_switch_joystick_service_; - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr timer_lidar_; - tf2_ros::TransformBroadcaster tf_broadcaster_; - sensor_msgs::msg::Joy joy_state_; - - bool odom_published_{false}; +private: + std::shared_ptr go2_camera_; + std::shared_ptr go2_services_handler_; + std::shared_ptr go2_tts_; + std::shared_ptr go2_vui_; + std::shared_ptr go2_odometry_; + std::shared_ptr go2_joint_states_; + std::shared_ptr go2_switch_obstacles_avoidance_; + + bool use_camera_; + bool use_tts_; + bool use_vui_; + bool use_odometry_; + bool use_joint_states_; + bool use_services_; + bool use_switch_obstacles_avoidance_; }; } // namespace go2_driver diff --git a/include/go2_driver/modules/go2_camera.hpp b/include/go2_driver/modules/go2_camera.hpp new file mode 100644 index 0000000..beaf03a --- /dev/null +++ b/include/go2_driver/modules/go2_camera.hpp @@ -0,0 +1,96 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_CAMERA_HPP +#define GO2_DRIVER__GO2_CAMERA_HPP + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "go2_driver/utils/go2_lifecycle_node.hpp" + +extern "C" { + #include + #include + #include + #include +} + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2Camera : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2Camera(const std::shared_ptr & node); + ~Go2Camera() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void front_video_data_callback(const unitree_go::msg::Go2FrontVideoData::SharedPtr msg); + void publishCameraInfo(); + + rclcpp::Subscription::SharedPtr front_video_sub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr image_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + camera_info_publisher_; + + std::shared_ptr node_; + + AVCodec * codec_; + AVCodecContext * p_codec_context_; + AVPacket * sps_packet_; + AVPacket * pps_packet_; + bool sps_sent_{false}; + int camera_resolution_; + + std::string frame_id_; + std::string distorsion_model_; + int height_; + int width_; + std::vector d_; + std::vector k_; + std::vector r_; + std::vector p_; + int binning_x_; + int binning_y_; + int roi_x_offset_; + int roi_y_offset_; + int roi_height_; + int roi_width_; + bool roi_do_rectify_; + + // Precompute lens correction interpolation + cv::Mat mapX_, mapY_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_CAMERA_HPP diff --git a/include/go2_driver/modules/go2_handle_services.hpp b/include/go2_driver/modules/go2_handle_services.hpp new file mode 100644 index 0000000..a3d7f93 --- /dev/null +++ b/include/go2_driver/modules/go2_handle_services.hpp @@ -0,0 +1,156 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_HANDLE_SERVICES_HPP +#define GO2_DRIVER__GO2_HANDLE_SERVICES_HPP + +#include +#include +#include + +#include + +#include "go2_driver/utils/go2_lifecycle_node.hpp" + +#include "go2_interfaces/srv/body_height.hpp" +#include "go2_interfaces/srv/continuous_gait.hpp" +#include "go2_interfaces/srv/euler.hpp" +#include "go2_interfaces/srv/foot_raise_height.hpp" +#include "go2_interfaces/srv/mode.hpp" +#include "go2_interfaces/srv/pose.hpp" +#include "go2_interfaces/srv/speed_level.hpp" +#include "go2_interfaces/srv/switch_gait.hpp" +#include "go2_interfaces/srv/switch_joystick.hpp" +#include "go2_interfaces/srv/get_body_height.hpp" +#include "go2_interfaces/srv/get_foot_raise_height.hpp" +#include "go2_interfaces/srv/get_speed_level.hpp" +#include "go2_interfaces/srv/get_state.hpp" + +#include "unitree_api/msg/request.hpp" +#include "unitree_api/msg/response.hpp" + +#include "go2_driver/utils/go2_api_id.hpp" + +#include "nlohmann/json.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2HandleServices : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2HandleServices(const std::shared_ptr & node); + ~Go2HandleServices() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); + + void handleBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetState( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr request_pub_; + + rclcpp::Service::SharedPtr set_body_height_service_; + rclcpp::Service::SharedPtr set_continuous_gait_service_; + rclcpp::Service::SharedPtr set_euler_service_; + rclcpp::Service::SharedPtr set_foot_raise_height_service_; + rclcpp::Service::SharedPtr set_mode_service_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_speed_level_service_; + rclcpp::Service::SharedPtr set_switch_gait_service_; + rclcpp::Service::SharedPtr set_switch_joystick_service_; + rclcpp::Service::SharedPtr get_body_height_service_; + rclcpp::Service::SharedPtr get_foot_raise_height_service_; + rclcpp::Service::SharedPtr get_speed_level_service_; + rclcpp::Service::SharedPtr get_state_service_; + + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + std::shared_ptr node_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_HANDLE_SERVICES_HPP diff --git a/include/go2_driver/modules/go2_joint_states.hpp b/include/go2_driver/modules/go2_joint_states.hpp new file mode 100644 index 0000000..2d52328 --- /dev/null +++ b/include/go2_driver/modules/go2_joint_states.hpp @@ -0,0 +1,62 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_JOINT_STATES_HPP +#define GO2_DRIVER__GO2_JOINT_STATES_HPP + +#include +#include +#include + +#include + +#include "unitree_go/msg/low_state.hpp" +#include "unitree_api/msg/request.hpp" + +#include "go2_driver/utils/go2_lifecycle_node.hpp" +#include "go2_driver/utils/go2_api_id.hpp" + +#include "nlohmann/json.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2JointStates : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2JointStates(const std::shared_ptr & node); + ~Go2JointStates() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg); + + rclcpp::Subscription::SharedPtr low_state_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr joint_state_pub_; + + std::shared_ptr node_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_JOINT_STATES_HPP diff --git a/include/go2_driver/modules/go2_odometry.hpp b/include/go2_driver/modules/go2_odometry.hpp new file mode 100644 index 0000000..143d3ad --- /dev/null +++ b/include/go2_driver/modules/go2_odometry.hpp @@ -0,0 +1,62 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_ODOMETRY_HPP +#define GO2_DRIVER__GO2_ODOMETRY_HPP + +#include +#include +#include + +#include +#include + +#include "tf2_ros/transform_broadcaster.h" + +#include "go2_driver/utils/go2_lifecycle_node.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2Odometry : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2Odometry(const std::shared_ptr & node); + ~Go2Odometry() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void publish_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); + + rclcpp::Subscription::SharedPtr odom_sub_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr odom_pub_; + + std::shared_ptr node_; + tf2_ros::TransformBroadcaster tf_broadcaster_; + + bool odom_published_{false}; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_ODOMETRY_HPP diff --git a/include/go2_driver/modules/go2_switch_obstacles_avoidance.hpp b/include/go2_driver/modules/go2_switch_obstacles_avoidance.hpp new file mode 100644 index 0000000..faa0d54 --- /dev/null +++ b/include/go2_driver/modules/go2_switch_obstacles_avoidance.hpp @@ -0,0 +1,74 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_SWITCH_OBSTACLES_AVOIDANCE_HPP +#define GO2_DRIVER__GO2_SWITCH_OBSTACLES_AVOIDANCE_HPP + +#include +#include +#include + +#include "go2_interfaces/srv/get_switch_obstacles_avoidance.hpp" +#include "go2_interfaces/srv/set_switch_obstacles_avoidance.hpp" + +#include "unitree_api/msg/request.hpp" +#include "unitree_api/msg/response.hpp" + +#include "go2_driver/utils/go2_lifecycle_node.hpp" +#include "go2_driver/utils/go2_api_id.hpp" + +#include "nlohmann/json.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2SwitchObstaclesAvoidance : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2SwitchObstaclesAvoidance(const std::shared_ptr & node); + ~Go2SwitchObstaclesAvoidance() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void handleGetObstaclesAvoidance( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSetObstaclesAvoidance( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp::Service::SharedPtr + get_obstacles_avoidance_service_; + rclcpp::Service::SharedPtr + set_obstacles_avoidance_service_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr request_pub_; + + std::shared_ptr node_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_SWITCH_OBSTACLES_AVOIDANCE_HPP diff --git a/include/go2_driver/modules/go2_tts.hpp b/include/go2_driver/modules/go2_tts.hpp new file mode 100644 index 0000000..4f9325d --- /dev/null +++ b/include/go2_driver/modules/go2_tts.hpp @@ -0,0 +1,76 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_TTS_HPP +#define GO2_DRIVER__GO2_TTS_HPP + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "go2_driver/utils/go2_lifecycle_node.hpp" +#include "go2_driver/utils/go2_api_id.hpp" +#include "unitree_api/msg/request.hpp" +#include "go2_interfaces/srv/say.hpp" + +#include "nlohmann/json.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2TTS : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2TTS(const std::shared_ptr & node); + ~Go2TTS() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void handleSay( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp::Service::SharedPtr say_service_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr say_request_pub_; + + std::shared_ptr node_; + + std::vector> calculate_chunks( + const std::vector & data, + size_t chunk_size); + std::vector read_file(const std::string & filename); + std::string base64_encode(const std::vector & data); +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_TTS_HPP diff --git a/include/go2_driver/modules/go2_vui.hpp b/include/go2_driver/modules/go2_vui.hpp new file mode 100644 index 0000000..9461fea --- /dev/null +++ b/include/go2_driver/modules/go2_vui.hpp @@ -0,0 +1,100 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#ifndef GO2_DRIVER__GO2_VUI_HPP +#define GO2_DRIVER__GO2_VUI_HPP + +#include +#include +#include + +#include "go2_driver/utils/go2_lifecycle_node.hpp" +#include "go2_driver/utils/go2_api_id.hpp" + +#include "go2_interfaces/srv/get_brightness.hpp" +#include "go2_interfaces/srv/get_switch.hpp" +#include "go2_interfaces/srv/get_volume.hpp" +#include "go2_interfaces/srv/set_brightness.hpp" +#include "go2_interfaces/srv/set_switch.hpp" +#include "go2_interfaces/srv/set_volume.hpp" + +#include "unitree_api/msg/request.hpp" +#include "unitree_api/msg/response.hpp" + +#include "nlohmann/json.hpp" + + +namespace go2_driver +{ + +using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class Go2VUI : public go2_driver::Go2LifecycleNode +{ +public: + explicit Go2VUI(const std::shared_ptr & node); + ~Go2VUI() override = default; + + CallbackReturnT on_configure() override; + CallbackReturnT on_activate() override; + CallbackReturnT on_deactivate() override; + CallbackReturnT on_cleanup() override; + +private: + void handleGetBrightness( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetSwitch( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleGetVolume( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSetBrightness( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSetSwitch( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSetVolume( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp::Service::SharedPtr get_brightness_service_; + rclcpp::Service::SharedPtr get_switch_service_; + rclcpp::Service::SharedPtr get_volume_service_; + rclcpp::Service::SharedPtr set_brightness_service_; + rclcpp::Service::SharedPtr set_switch_service_; + rclcpp::Service::SharedPtr set_volume_service_; + + rclcpp_lifecycle::LifecyclePublisher::SharedPtr request_pub_; + + std::shared_ptr node_; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_VUI_HPP diff --git a/include/go2_driver/utils/go2_api_id.hpp b/include/go2_driver/utils/go2_api_id.hpp new file mode 100644 index 0000000..09119d1 --- /dev/null +++ b/include/go2_driver/utils/go2_api_id.hpp @@ -0,0 +1,98 @@ +// Copyright 2024 Intelligent Robotics Lab +// +// 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. + +#ifndef GO2_DRIVER__GO2_API_ID_HPP +#define GO2_DRIVER__GO2_API_ID_HPP + +namespace go2_driver +{ + +enum class Mode +{ + Damp = 1001, + BalanceStand = 1002, + StopMove = 1003, + StandUp = 1004, + StandDown = 1005, + RecoveryStand = 1006, + Euler = 1007, + Move = 1008, + Sit = 1009, + RiseSit = 1010, + SwitchGait = 1011, + Trigger = 1012, + BodyHeight = 1013, + FootRaiseHeight = 1014, + SpeedLevel = 1015, + Hello = 1016, + Stretch = 1017, + TrajectoryFollow = 1018, + ContinuousGait = 1019, + Content = 1020, + Wallow = 1021, + Dance1 = 1022, + Dance2 = 1023, + GetBodyHeight = 1024, + GetFootRaiseHeight = 1025, + GetSpeedLevel = 1026, + SwitchJoystick = 1027, + Pose = 1028, + Scrape = 1029, + FrontFlip = 1030, + FrontJump = 1031, + FrontPounce = 1032, + WiggleHips = 1033, + GetState = 1034, + EconomicGait = 1035, + FingerHeart = 1036, + Dance3 = 1037, + Dance4 = 1038, + HopSpinLeft = 1039, + HopSpinRight = 1040, + LeftFlip = 1042, + BackFlip = 1044, + FreeWalk = 1045, + FreeBound = 1046, + FreeJump = 1047, + FreeAvoid = 1048, + WalkStair = 1049, + WalkUpRight = 1050, + CrossStep = 1051, +}; + +enum class Audio +{ + StartAudio = 4001, + TTS = 4003, +}; + +enum class Vui +{ + SetSwitch = 1001, + GetSwitch = 1002, + SetVolume = 1003, + GetVolume = 1004, + SetBrightness = 1005, + GetBrightness = 1006, +}; + +enum class ObstaclesAvoidance +{ + SetSwitch = 1001, + GetSwitch = 1002, +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_API_ID_HPP diff --git a/include/go2_driver/utils/go2_lifecycle_node.hpp b/include/go2_driver/utils/go2_lifecycle_node.hpp new file mode 100644 index 0000000..257354d --- /dev/null +++ b/include/go2_driver/utils/go2_lifecycle_node.hpp @@ -0,0 +1,54 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + +#ifndef GO2_DRIVER__GO2_LIFECYCLE_NODE_HPP +#define GO2_DRIVER__GO2_LIFECYCLE_NODE_HPP + +#include +#include + +#define RCLCPP_DISABLE_TOPIC_STATISTICS + +#include "unitree_api/msg/response.hpp" +#include "rosidl_runtime_cpp/traits.hpp" + +#include + +namespace libstatistics_collector { +namespace topic_statistics_collector { + +template<> +struct HasHeader : public std::false_type {}; +} // namespace topic_statistics_collector +} // namespace libstatistics_collector + +namespace go2_driver +{ + +class Go2LifecycleNode +{ +public: + virtual ~Go2LifecycleNode() = default; + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure() = + 0; + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate() = + 0; + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate() + = 0; + virtual rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup() = 0; +}; + +} // namespace go2_driver + +#endif // GO2_DRIVER__GO2_LIFECYCLE_NODE_HPP diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index fc0541e..215caac 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -1,40 +1,117 @@ -# BSD 3-Clause License +# Copyright (c) 2024 Intelligent Robotics Lab (URJC) +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import FindExecutable, LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode -# Copyright (c) 2024, Intelligent Robotics Lab -# All rights reserved. -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +def autostart(context): -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. + if (LaunchConfiguration('autostart').perform(context) == 'True' or + LaunchConfiguration('autostart').perform(context) == 'true'): + configure_event = ExecuteProcess( + cmd=[[FindExecutable( + name='ros2'), ' lifecycle', ' set', ' /go2_driver', ' configure']], + shell=True + ) -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. + activate_event = ExecuteProcess( + cmd=[[FindExecutable( + name='ros2'), ' lifecycle', ' set', ' /go2_driver', ' activate']], + shell=True + ) -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. + event_handler = RegisterEventHandler( + OnProcessExit( + target_action=configure_event, + on_exit=[activate_event], + ) + ) -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + return [configure_event, event_handler] -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer, Node -from launch_ros.descriptions import ComposableNode + return [] def generate_launch_description(): + camera_calibration_file = os.path.join( + get_package_share_directory('go2_driver'), + 'config', + 'camera_calibration.yaml' + ) + + declare_camera_cmd = DeclareLaunchArgument( + 'use_camera', + default_value='True', + description='Use camera' + ) + + declare_tts_cmd = DeclareLaunchArgument( + 'use_tts', + default_value='True', + description='Use text to speech' + ) + + declare_vui_cmd = DeclareLaunchArgument( + 'use_vui', + default_value='True', + description='Use voice user interface' + ) + + declare_odometry_cmd = DeclareLaunchArgument( + 'use_odometry', + default_value='True', + description='Use odometry' + ) + + declare_joint_states_cmd = DeclareLaunchArgument( + 'use_joint_states', + default_value='True', + description='Use joint states' + ) + + declare_services_cmd = DeclareLaunchArgument( + 'use_services', + default_value='True', + description='Use services' + ) + + declare_obtacles_avoidance_cmd = DeclareLaunchArgument( + 'use_switch_obtacles_avoidance', + default_value='True', + description='Use obtacles avoidance' + ) + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='True', + description='Automatically start the components' + ) + composable_nodes = [] composable_node = ComposableNode( @@ -42,8 +119,17 @@ def generate_launch_description(): plugin='go2_driver::Go2Driver', name='go2_driver', namespace='', - + parameters=[camera_calibration_file, { + 'use_camera': LaunchConfiguration('use_camera'), + 'use_tts': LaunchConfiguration('use_tts'), + 'use_vui': LaunchConfiguration('use_vui'), + 'use_odometry': LaunchConfiguration('use_odometry'), + 'use_joint_states': LaunchConfiguration('use_joint_states'), + 'use_services': LaunchConfiguration('use_services'), + 'use_switch_obtacles_avoidance': LaunchConfiguration('use_switch_obtacles_avoidance'), + }], ) + composable_nodes.append(composable_node) container = ComposableNodeContainer( @@ -55,21 +141,23 @@ def generate_launch_description(): output='screen', ) - pointclod_to_laserscan_cmd = Node( - package='pointcloud_to_laserscan', - executable='pointcloud_to_laserscan_node', - name='pointcloud_to_laserscan', - namespace='', - output='screen', - remappings=[('/cloud_in', '/pointcloud')], - parameters=[{ - 'target_frame': 'radar', - 'transform_tolerance': 0.01, - }], - ) + event_handler = RegisterEventHandler( + OnProcessStart( + target_action=container, + on_start=[OpaqueFunction(function=autostart)], + ) + ) ld = LaunchDescription() + ld.add_action(declare_camera_cmd) + ld.add_action(declare_tts_cmd) + ld.add_action(declare_vui_cmd) + ld.add_action(declare_odometry_cmd) + ld.add_action(declare_joint_states_cmd) + ld.add_action(declare_services_cmd) + ld.add_action(declare_obtacles_avoidance_cmd) + ld.add_action(declare_autostart_cmd) ld.add_action(container) - ld.add_action(pointclod_to_laserscan_cmd) + ld.add_action(event_handler) return ld diff --git a/package.xml b/package.xml index 35a368d..bb7ba90 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,9 @@ tf2_ros go2_interfaces nlohmann-json-dev + cv_bridge + OpenCV + OpenSSL ament_lint_auto ament_lint_common diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index beb5804..eb928f4 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -1,462 +1,202 @@ -// BSD 3-Clause License - -// Copyright (c) 2024, Intelligent Robotics Lab -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: - -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. - -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. - -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// Copyright 2024 Intelligent Robotics Lab +// +// 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. #include + namespace go2_driver { Go2Driver::Go2Driver( const rclcpp::NodeOptions & options) -: Node("go2_driver", options), - tf_broadcaster_(this) -{ - rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - - pointcloud_pub_ = create_publisher("pointcloud", 10); - joint_state_pub_ = create_publisher("joint_states", 10); - odom_pub_ = create_publisher("odom", qos_profile); - imu_pub_ = create_publisher("imu", 10); - request_pub_ = create_publisher("api/sport/request", 10); - - pointcloud_sub_ = create_subscription( - "/utlidar/cloud", 10, - std::bind(&Go2Driver::publish_lidar, this, std::placeholders::_1)); - - robot_pose_sub_ = create_subscription( - "/utlidar/robot_pose", 10, - std::bind(&Go2Driver::publish_pose_stamped, this, std::placeholders::_1)); - - joy_sub_ = create_subscription( - "joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1)); - - low_state_sub_ = create_subscription( - "lowstate", 10, - std::bind(&Go2Driver::publish_joint_states, this, std::placeholders::_1)); - - cmd_vel_sub_ = create_subscription( - "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); - - set_body_height_service_ = - this->create_service( - "body_height", - std::bind( - &Go2Driver::handleBodyHeight, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_continuous_gait_service_ = - this->create_service( - "continuous_gait", - std::bind( - &Go2Driver::handleContinuousGait, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_euler_service_ = - this->create_service( - "euler", - std::bind( - &Go2Driver::handleEuler, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_foot_raise_height_service_ = - this->create_service( - "foot_raise_height", - std::bind( - &Go2Driver::handleFootRaiseHeight, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_mode_service_ = - this->create_service( - "mode", - std::bind( - &Go2Driver::handleMode, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_pose_service_ = - this->create_service( - "pose", - std::bind( - &Go2Driver::handlePose, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_speed_level_service_ = - this->create_service( - "speed_level", - std::bind( - &Go2Driver::handleSpeedLevel, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_switch_gait_service_ = - this->create_service( - "switch_gait", - std::bind( - &Go2Driver::handleSwitchGait, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - - set_switch_joystick_service_ = - this->create_service( - "switch_joystick", - std::bind( - &Go2Driver::handleSwitchJoystick, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); +: LifecycleNode("go2_driver", options) +{ + declare_parameter("use_camera", true); + declare_parameter("use_tts", true); + declare_parameter("use_vui", true); + declare_parameter("use_odometry", true); + declare_parameter("use_joint_states", true); + declare_parameter("use_services", true); + declare_parameter("use_switch_obstacles_avoidance", true); + + get_parameter("use_camera", use_camera_); + get_parameter("use_tts", use_tts_); + get_parameter("use_vui", use_vui_); + get_parameter("use_odometry", use_odometry_); + get_parameter("use_joint_states", use_joint_states_); + get_parameter("use_services", use_services_); + get_parameter("use_switch_obstacles_avoidance", use_switch_obstacles_avoidance_); } -void Go2Driver::publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +CallbackReturnT Go2Driver::on_configure(const rclcpp_lifecycle::State &) { - msg->header.stamp = now(); - msg->header.frame_id = "radar"; - pointcloud_pub_->publish(*msg); -} - -void Go2Driver::publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header.stamp = now(); - transform.header.frame_id = "odom"; - transform.child_frame_id = "base_link"; - transform.transform.translation.x = msg->pose.position.x; - transform.transform.translation.y = msg->pose.position.y; - transform.transform.translation.z = msg->pose.position.z + 0.07; - transform.transform.rotation.x = msg->pose.orientation.x; - transform.transform.rotation.y = msg->pose.orientation.y; - transform.transform.rotation.z = msg->pose.orientation.z; - transform.transform.rotation.w = msg->pose.orientation.w; - tf_broadcaster_.sendTransform(transform); - - if (!odom_published_) { - nav_msgs::msg::Odometry odom; - odom.header.stamp = now(); - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_link"; - odom.pose.pose.position.x = msg->pose.position.x; - odom.pose.pose.position.y = msg->pose.position.y; - odom.pose.pose.position.z = msg->pose.position.z + 0.07; - odom.pose.pose.orientation.x = msg->pose.orientation.x; - odom.pose.pose.orientation.y = msg->pose.orientation.y; - odom.pose.pose.orientation.z = msg->pose.orientation.z; - odom.pose.pose.orientation.w = msg->pose.orientation.w; - odom_pub_->publish(odom); - odom_published_ = true; + if (use_camera_) { + go2_camera_ = std::make_shared(shared_from_this()); + go2_camera_->on_configure(); } -} - -void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) -{ - joy_state_ = *msg; -} -void Go2Driver::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) -{ - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = now(); - joint_state.name = {"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", - "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", - "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", - "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"}; - - joint_state.position = {msg->motor_state[3].q, msg->motor_state[4].q, msg->motor_state[5].q, - msg->motor_state[0].q, msg->motor_state[1].q, msg->motor_state[2].q, - msg->motor_state[9].q, msg->motor_state[10].q, msg->motor_state[11].q, - msg->motor_state[6].q, msg->motor_state[7].q, msg->motor_state[8].q}; - - joint_state_pub_->publish(joint_state); -} - -void Go2Driver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) -{ - nlohmann::json js; - js["x"] = msg->linear.x; - js["y"] = msg->linear.y; - js["z"] = msg->angular.z; + if (use_tts_) { + go2_tts_ = std::make_shared(shared_from_this()); + go2_tts_->on_configure(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::Move); + if (use_vui_) { + go2_vui_ = std::make_shared(shared_from_this()); + go2_vui_->on_configure(); + } - request_pub_->publish(req); -} + if (use_odometry_) { + go2_odometry_ = std::make_shared(shared_from_this()); + go2_odometry_->on_configure(); + } -void Go2Driver::handleBodyHeight( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; + if (use_joint_states_) { + go2_joint_states_ = std::make_shared(shared_from_this()); + go2_joint_states_->on_configure(); + } - if (request->height < -0.18 || request->height > 0.03) { - response->success = false; - response->message = "Height value is out of range [0.3 ~ 0.5]"; - return; + if (use_services_) { + go2_services_handler_ = std::make_shared(shared_from_this()); + go2_services_handler_->on_configure(); } - nlohmann::json js; - js["data"] = request->height; + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_ = std::make_shared( + shared_from_this()); + go2_switch_obstacles_avoidance_->on_configure(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::BodyHeight); + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules configured\033[0m"); - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleContinuousGait( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) +CallbackReturnT Go2Driver::on_activate(const rclcpp_lifecycle::State &) { - (void)request_header; + if (use_camera_) { + go2_camera_->on_activate(); + } - nlohmann::json js; - js["data"] = request->flag; + if (use_tts_) { + go2_tts_->on_activate(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::ContinuousGait); + if (use_vui_) { + go2_vui_->on_activate(); + } - request_pub_->publish(req); - response->success = true; -} + if (use_odometry_) { + go2_odometry_->on_activate(); + } -void Go2Driver::handleEuler( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - - nlohmann::json js; - if (request->roll < -0.75 || request->roll > 0.75) { - response->success = false; - response->message = "Roll value is out of range [-0.75 ~ 0.75]"; - return; - } else if (request->pitch < -0.75 || request->pitch > 0.75) { - response->success = false; - response->message = "Pitch value is out of range [-0.75 ~ 0.75]"; - return; - } else if (request->yaw < -0.6 || request->yaw > 0.6) { - response->success = false; - response->message = "Yaw value is out of range [-1.5 ~ 1.5]"; - return; + if (use_joint_states_) { + go2_joint_states_->on_activate(); } - js["x"] = request->roll; - js["y"] = request->pitch; - js["z"] = request->yaw; + if (use_services_) { + go2_services_handler_->on_activate(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::Euler); + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_activate(); + } + + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules activated\033[0m"); - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleFootRaiseHeight( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) +CallbackReturnT Go2Driver::on_deactivate(const rclcpp_lifecycle::State &) { - (void)request_header; - - if (request->height < 0 || request->height > 0.1) { - response->success = false; - response->message = "Height value is out of range [-0.06 ~ 0.03]"; - return; + if (use_camera_) { + go2_camera_->on_deactivate(); } - nlohmann::json js; - js["data"] = request->height; - - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::FootRaiseHeight); + if (use_tts_) { + go2_tts_->on_deactivate(); + } - request_pub_->publish(req); - response->success = true; -} + if (use_vui_) { + go2_vui_->on_deactivate(); + } -void Go2Driver::handleMode( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - std::string mode = request->mode; - - unitree_api::msg::Request req; - - if (mode == "damp") { - response->message = "Change the mode to Damp"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Damp); - } else if (mode == "balance_stand") { - response->message = "Change the mode to BalanceStand"; - req.header.identity.api_id = static_cast(go2_driver::Mode::BalanceStand); - } else if (mode == "stop_move") { - response->message = "Change the mode to StopMove"; - req.header.identity.api_id = static_cast(go2_driver::Mode::StopMove); - } else if (mode == "stand_up") { - response->message = "Change the mode to StandUp"; - req.header.identity.api_id = static_cast(go2_driver::Mode::StandUp); - } else if (mode == "stand_down") { - response->message = "Change the mode to StandDown"; - req.header.identity.api_id = static_cast(go2_driver::Mode::StandDown); - } else if (mode == "sit") { - response->message = "Change the mode to Sit"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Sit); - } else if (mode == "rise_sit") { - response->message = "Change the mode to RiseSit"; - req.header.identity.api_id = static_cast(go2_driver::Mode::RiseSit); - } else if (mode == "hello") { - response->message = "Change the mode to Hello. Say hello to your robot!"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Hello); - } else if (mode == "stretch") { - response->message = "Change the mode to Stretch"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Stretch); - } else if (mode == "wallow") { - response->message = "Change the mode to Wallow"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Wallow); - } else if (mode == "scrape") { - response->message = "Change the mode to Scrape"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Scrape); - } else if (mode == "front_flip") { - response->message = "Front flip??? Really? You want to break your robot? Crazy!"; - // req.header.identity.api_id = static_cast(go2_driver::Mode::FrontFlip); - } else if (mode == "front_jump") { - response->message = "Change the mode to Front Jump"; - req.header.identity.api_id = static_cast(go2_driver::Mode::FrontJump); - } else if (mode == "front_pounce") { - response->message = "Change the mode to Front Pounce"; - req.header.identity.api_id = static_cast(go2_driver::Mode::FrontPounce); - } else if (mode == "dance1") { - response->message = "Change the mode to Dance 1. Let's dance!"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Dance1); - } else if (mode == "dance2") { - response->message = "Change the mode to Dance 2. Let's dance!"; - req.header.identity.api_id = static_cast(go2_driver::Mode::Dance2); - } else if (mode == "finger_heart") { - response->message = "Change the mode to Finger Heart"; - req.header.identity.api_id = static_cast(go2_driver::Mode::FingerHeart); - } else { - response->success = false; - response->message = "Invalid mode"; - return; + if (use_odometry_) { + go2_odometry_->on_deactivate(); } - request_pub_->publish(req); - response->success = true; -} + if (use_joint_states_) { + go2_joint_states_->on_deactivate(); + } -void Go2Driver::handlePose( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; + if (use_services_) { + go2_services_handler_->on_deactivate(); + } - nlohmann::json js; - js["data"] = request->flag; + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_deactivate(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::Pose); + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules deactivated\033[0m"); - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleSpeedLevel( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) +CallbackReturnT Go2Driver::on_cleanup(const rclcpp_lifecycle::State &) { - (void)request_header; - - if (request->level < -1 || request->level > 1) { - response->success = false; - response->message = "Speed level is out of range [-1 ~ 1]"; - return; + if (use_camera_) { + go2_camera_->on_cleanup(); + go2_camera_.reset(); } - nlohmann::json js; - js["data"] = request->level; + if (use_tts_) { + go2_tts_->on_cleanup(); + go2_tts_.reset(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::SpeedLevel); + if (use_vui_) { + go2_vui_->on_cleanup(); + go2_vui_.reset(); + } - request_pub_->publish(req); - response->success = true; -} + if (use_odometry_) { + go2_odometry_->on_cleanup(); + go2_odometry_.reset(); + } -void Go2Driver::handleSwitchGait( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; + if (use_joint_states_) { + go2_joint_states_->on_cleanup(); + go2_joint_states_.reset(); + } - if (request->d < 0 || request->d > 4) { - response->success = false; - response->message = "Invalid gait type [0 - 4]"; - return; + if (use_services_) { + go2_services_handler_->on_cleanup(); + go2_services_handler_.reset(); } - nlohmann::json js; - js["data"] = request->d; + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_cleanup(); + go2_switch_obstacles_avoidance_.reset(); + } - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchGait); + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules cleaned up\033[0m"); - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleSwitchJoystick( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) +CallbackReturnT Go2Driver::on_shutdown(const rclcpp_lifecycle::State &) { - (void)request_header; - - nlohmann::json js; - js["data"] = request->flag; - - unitree_api::msg::Request req; - req.parameter = js.dump(); - req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchJoystick); - - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } } // namespace go2_driver diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp new file mode 100644 index 0000000..392ef9a --- /dev/null +++ b/src/go2_driver/modules/go2_camera.cpp @@ -0,0 +1,248 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + +namespace go2_driver +{ + +Go2Camera::Go2Camera(const std::shared_ptr & node) +: node_(node), + codec_(), + p_codec_context_(), + sps_packet_(), + pps_packet_() +{ + node_->declare_parameter("camera_resolution", 720); + node_->declare_parameter("frame_id", "camera_frame"); + node_->declare_parameter("height", 720); + node_->declare_parameter("width", 1280); + node_->declare_parameter("distorsion_model", "plumb_bob"); + node_->declare_parameter("d", std::vector()); + node_->declare_parameter("k", std::vector()); + node_->declare_parameter("r", std::vector()); + node_->declare_parameter("p", std::vector()); + node_->declare_parameter("binning_x", 0); + node_->declare_parameter("binning_y", 0); + node_->declare_parameter("roi.x_offset", 0); + node_->declare_parameter("roi.y_offset", 0); + node_->declare_parameter("roi.height", 0); + node_->declare_parameter("roi.width", 0); + node_->declare_parameter("roi.do_rectify", false); + + node_->get_parameter("camera_resolution", camera_resolution_); + node_->get_parameter("frame_id", frame_id_); + node_->get_parameter("height", height_); + node_->get_parameter("width", width_); + node_->get_parameter("distorsion_model", distorsion_model_); + node_->get_parameter("d", d_); + node_->get_parameter("k", k_); + node_->get_parameter("r", r_); + node_->get_parameter("p", p_); + node_->get_parameter("binning_x", binning_x_); + node_->get_parameter("binning_y", binning_y_); + node_->get_parameter("roi.x_offset", roi_x_offset_); + node_->get_parameter("roi.y_offset", roi_y_offset_); + node_->get_parameter("roi.height", roi_height_); + node_->get_parameter("roi.width", roi_width_); + node_->get_parameter("roi.do_rectify", roi_do_rectify_); +} + +CallbackReturnT Go2Camera::on_configure() +{ + codec_ = avcodec_find_decoder(AV_CODEC_ID_H264); + if (!codec_) { + RCLCPP_ERROR(node_->get_logger(), "Failed to find codec."); + return CallbackReturnT::FAILURE; + } + + p_codec_context_ = avcodec_alloc_context3(codec_); + if (p_codec_context_ == nullptr) { + RCLCPP_ERROR(node_->get_logger(), "Failed to allocate codec context."); + return CallbackReturnT::FAILURE; + } + + if (avcodec_open2(p_codec_context_, codec_, nullptr) < 0) { + RCLCPP_ERROR(node_->get_logger(), "Failed to open codec."); + return CallbackReturnT::FAILURE; + } + + image_publisher_ = node_->create_publisher("/image_raw", 10); + camera_info_publisher_ = + node_->create_publisher("/camera_info", 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module configured.\033[0m"); + + // Precompute undistort map for lens correction + cv::Mat K = cv::Mat(3, 3, CV_64F, k_.data()); + cv::Mat D = cv::Mat(1, 5, CV_64F, d_.data()); + cv::initUndistortRectifyMap(K, D, cv::Matx33f::eye(), K, cv::Size(width_, height_), CV_32FC1, mapX_, mapY_); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_activate() +{ + image_publisher_->on_activate(); + camera_info_publisher_->on_activate(); + + front_video_sub_ = node_->create_subscription( + "frontvideostream", 10, + std::bind(&Go2Camera::front_video_data_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_deactivate() +{ + image_publisher_->on_deactivate(); + camera_info_publisher_->on_deactivate(); + + front_video_sub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_cleanup() +{ + image_publisher_.reset(); + camera_info_publisher_.reset(); + + avcodec_free_context(&p_codec_context_); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2Camera::front_video_data_callback(const unitree_go::msg::Go2FrontVideoData::SharedPtr msg) +{ + if (msg->resolution != camera_resolution_) {return;} + AVPacket * packet = av_packet_alloc(); + + av_new_packet(packet, msg->data.size()); + memcpy(packet->data, msg->data.data(), msg->data.size()); + + if (packet->size >= 4 && packet->data[0] == 0x00 && packet->data[1] == 0x00 && + packet->data[2] == 0x00 && packet->data[3] == 0x01) + { + + uint8_t nal_unit_type = packet->data[4] & 0x1F; + + if (nal_unit_type == 7) { + if (!sps_packet_) { + sps_packet_ = av_packet_alloc(); + av_packet_ref(sps_packet_, packet); + RCLCPP_DEBUG(node_->get_logger(), "SPS received."); + } + } else if (nal_unit_type == 1) { + if (!pps_packet_) { + pps_packet_ = av_packet_alloc(); + av_packet_ref(pps_packet_, packet); + RCLCPP_DEBUG(node_->get_logger(), "PPS received."); + } + } + } + + if (!sps_packet_ || !pps_packet_) { + RCLCPP_DEBUG(node_->get_logger(), "Waiting for SPS/PPS before decoding."); + return; + } + + if (!sps_sent_) { + avcodec_send_packet(p_codec_context_, sps_packet_); + avcodec_send_packet(p_codec_context_, pps_packet_); + sps_sent_ = true; + } + + int ret = avcodec_send_packet(p_codec_context_, packet); + + if (ret < 0) { + char errbuf[AV_ERROR_MAX_STRING_SIZE]; + av_strerror(ret, errbuf, sizeof(errbuf)); + RCLCPP_DEBUG(node_->get_logger(), "Failed to send packet to decoder: %s", errbuf); + } + + AVFrame * frame = av_frame_alloc(); + ret = avcodec_receive_frame(p_codec_context_, frame); + + if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) { + av_frame_free(&frame); + return; + } else if (ret < 0) { + char errbuf[AV_ERROR_MAX_STRING_SIZE]; + av_strerror(ret, errbuf, sizeof(errbuf)); + RCLCPP_DEBUG(node_->get_logger(), "Failed to receive frame from decoder: %s", errbuf); + av_frame_free(&frame); + return; + } + + cv::Mat yuv420p(frame->height * 3 / 2, frame->width, CV_8UC1); + memcpy(yuv420p.data, frame->data[0], frame->linesize[0] * frame->height); + memcpy( + yuv420p.data + frame->linesize[0] * frame->height, frame->data[1], + frame->linesize[1] * frame->height / 2); + memcpy( + yuv420p.data + frame->linesize[0] * frame->height + frame->linesize[1] * frame->height / 2, + frame->data[2], frame->linesize[2] * frame->height / 2); + + cv::Mat bgr; + cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGB); + + // cv::Mat dst; + // Undistort image using remap to improve performance + // cv::remap(bgr, dst, mapX_, mapY_, cv::INTER_LINEAR); + + auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toImageMsg(); + image_msg->header.stamp = node_->get_clock()->now(); + image_msg->header.frame_id = "camera_frame"; + + image_publisher_->publish(*image_msg); + + if (camera_info_publisher_->get_subscription_count() > 0) { + publishCameraInfo(); + } + + av_frame_free(&frame); +} + +void Go2Camera::publishCameraInfo() +{ + sensor_msgs::msg::CameraInfo camera_info_msg; + camera_info_msg.header.stamp = node_->get_clock()->now(); + camera_info_msg.header.frame_id = frame_id_; + camera_info_msg.height = height_; + camera_info_msg.width = width_; + camera_info_msg.distortion_model = distorsion_model_; + camera_info_msg.d = std::vector(d_.begin(), d_.end()); + std::copy(k_.begin(), k_.end(), camera_info_msg.k.begin()); + std::copy(r_.begin(), r_.end(), camera_info_msg.r.begin()); + std::copy(p_.begin(), p_.end(), camera_info_msg.p.begin()); + camera_info_msg.binning_x = binning_x_; + camera_info_msg.binning_y = binning_y_; + camera_info_msg.roi.x_offset = roi_x_offset_; + camera_info_msg.roi.y_offset = roi_y_offset_; + camera_info_msg.roi.height = roi_height_; + camera_info_msg.roi.width = roi_width_; + camera_info_msg.roi.do_rectify = roi_do_rectify_; + + camera_info_publisher_->publish(camera_info_msg); +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_handle_services.cpp b/src/go2_driver/modules/go2_handle_services.cpp new file mode 100644 index 0000000..fd8aa9e --- /dev/null +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -0,0 +1,698 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + + +namespace go2_driver +{ + +Go2HandleServices::Go2HandleServices(const std::shared_ptr & node) +: node_(node) +{ +} + +CallbackReturnT Go2HandleServices::on_configure() +{ + request_pub_ = node_->create_publisher("api/sport/request", 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mHandle services module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_activate() +{ + request_pub_->on_activate(); + + cmd_vel_sub_ = node_->create_subscription( + "cmd_vel", 10, std::bind(&Go2HandleServices::cmd_vel_callback, this, std::placeholders::_1)); + + set_body_height_service_ = + node_->create_service( + "body_height", + std::bind( + &Go2HandleServices::handleBodyHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_continuous_gait_service_ = + node_->create_service( + "continuous_gait", + std::bind( + &Go2HandleServices::handleContinuousGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_euler_service_ = + node_->create_service( + "euler", + std::bind( + &Go2HandleServices::handleEuler, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_foot_raise_height_service_ = + node_->create_service( + "foot_raise_height", + std::bind( + &Go2HandleServices::handleFootRaiseHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_mode_service_ = + node_->create_service( + "mode", + std::bind( + &Go2HandleServices::handleMode, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_pose_service_ = + node_->create_service( + "pose", + std::bind( + &Go2HandleServices::handlePose, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_speed_level_service_ = + node_->create_service( + "speed_level", + std::bind( + &Go2HandleServices::handleSpeedLevel, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_gait_service_ = + node_->create_service( + "switch_gait", + std::bind( + &Go2HandleServices::handleSwitchGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_joystick_service_ = + node_->create_service( + "switch_joystick", + std::bind( + &Go2HandleServices::handleSwitchJoystick, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_body_height_service_ = + node_->create_service( + "get_body_height", + std::bind( + &Go2HandleServices::handleGetBodyHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_foot_raise_height_service_ = + node_->create_service( + "get_foot_raise_height", + std::bind( + &Go2HandleServices::handleGetFootRaiseHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_speed_level_service_ = + node_->create_service( + "get_speed_level", + std::bind( + &Go2HandleServices::handleGetSpeedLevel, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_state_service_ = + node_->create_service( + "get_state", + std::bind( + &Go2HandleServices::handleGetState, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mHandle services module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_deactivate() +{ + request_pub_->on_deactivate(); + + cmd_vel_sub_.reset(); + set_body_height_service_.reset(); + set_continuous_gait_service_.reset(); + set_euler_service_.reset(); + set_foot_raise_height_service_.reset(); + set_mode_service_.reset(); + set_pose_service_.reset(); + set_speed_level_service_.reset(); + set_switch_gait_service_.reset(); + set_switch_joystick_service_.reset(); + get_body_height_service_.reset(); + get_foot_raise_height_service_.reset(); + get_speed_level_service_.reset(); + get_state_service_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mHandle services module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_cleanup() +{ + request_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mHandle services module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2HandleServices::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + nlohmann::json js; + js["x"] = msg->linear.x; + js["y"] = msg->linear.y; + js["z"] = msg->angular.z; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Move); + + request_pub_->publish(req); +} + +void Go2HandleServices::handleBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < -0.18 || request->height > 0.03) { + response->success = false; + response->message = "Height value is out of range [-0.18 ~ 0.03]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::BodyHeight); + + request_pub_->publish(req); + response->success = true; + response->message = "Body height changed"; +} + +void Go2HandleServices::handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::ContinuousGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + if (request->roll < -0.75 || request->roll > 0.75) { + response->success = false; + response->message = "Roll value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->pitch < -0.75 || request->pitch > 0.75) { + response->success = false; + response->message = "Pitch value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->yaw < -0.6 || request->yaw > 0.6) { + response->success = false; + response->message = "Yaw value is out of range [-0.6 ~ 0.6]"; + return; + } + + js["x"] = request->roll; + js["y"] = request->pitch; + js["z"] = request->yaw; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Euler); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < -0.06 || request->height > 0.03) { + response->success = false; + response->message = "Height value is out of range [-0.06 ~ 0.03]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::FootRaiseHeight); + + request_pub_->publish(req); + response->success = true; + response->message = "Foot raise height changed"; +} + +void Go2HandleServices::handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + std::string mode = request->mode; + + unitree_api::msg::Request req; + + if (mode == "damp") { + response->message = "Change the mode to Damp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Damp); + } else if (mode == "balance_stand") { + response->message = "Change the mode to BalanceStand"; + req.header.identity.api_id = static_cast(go2_driver::Mode::BalanceStand); + } else if (mode == "stop_move") { + response->message = "Change the mode to StopMove"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StopMove); + } else if (mode == "stand_up") { + response->message = "Change the mode to StandUp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandUp); + } else if (mode == "stand_down") { + response->message = "Change the mode to StandDown"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandDown); + } else if (mode == "sit") { + response->message = "Change the mode to Sit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Sit); + } else if (mode == "rise_sit") { + response->message = "Change the mode to RiseSit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::RiseSit); + } else if (mode == "hello") { + response->message = "Change the mode to Hello. Say hello to your robot!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Hello); + } else if (mode == "stretch") { + response->message = "Change the mode to Stretch"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Stretch); + } else if (mode == "wallow") { + response->message = "Change the mode to Wallow"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Wallow); + } else if (mode == "scrape") { + response->message = "Change the mode to Scrape"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Scrape); + } else if (mode == "front_flip") { + response->message = "Front flip??? Really? You want to break your robot? Crazy!"; + // req.header.identity.api_id = static_cast(go2_driver::Mode::FrontFlip); + } else if (mode == "front_jump") { + response->message = "Change the mode to Front Jump"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontJump); + } else if (mode == "front_pounce") { + response->message = "Change the mode to Front Pounce"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontPounce); + } else if (mode == "dance1") { + response->message = "Change the mode to Dance 1. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance1); + } else if (mode == "dance2") { + response->message = "Change the mode to Dance 2. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance2); + } else if (mode == "finger_heart") { + response->message = "Change the mode to Finger Heart"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FingerHeart); + } else if (mode == "finger_heart") { + response->message = "Change the mode to Finger Heart"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FingerHeart); + } else if (mode == "dance3") { + response->message = "Change the mode to Dance 3. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance3); + } else if (mode == "dance4") { + response->message = "Change the mode to Dance 4. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance4); + } else if (mode == "hop_spin_left") { + response->message = "Change the mode to Hop Spin Left"; + req.header.identity.api_id = static_cast(go2_driver::Mode::HopSpinLeft); + } else if (mode == "hop_spin_right") { + response->message = "Change the mode to Hop Spin Right"; + req.header.identity.api_id = static_cast(go2_driver::Mode::HopSpinRight); + } else if (mode == "left_flip") { + response->message = "Left flip??? Really? You want to break your robot? Crazy!"; + // req.header.identity.api_id = static_cast(go2_driver::Mode::LeftFlip); + } else if (mode == "back_flip") { + response->message = "Back flip??? Really? You want to break your robot? Crazy!"; + // req.header.identity.api_id = static_cast(go2_driver::Mode::BackFlip); + } else if (mode == "free_walk") { + response->message = "Change the mode to Free Walk"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FreeWalk); + } else if (mode == "free_bound") { + response->message = "Change the mode to Free Bound"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FreeBound); + } else if (mode == "free_jump") { + response->message = "Change the mode to Free Jump"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FreeJump); + } else if (mode == "free_avoid") { + response->message = "Change the mode to Free Avoid"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FreeAvoid); + } else if (mode == "walk_stair") { + response->message = "Change the mode to Walk Stair"; + req.header.identity.api_id = static_cast(go2_driver::Mode::WalkStair); + } else if (mode == "walk_up_right") { + response->message = "Change the mode to Walk Up Right"; + req.header.identity.api_id = static_cast(go2_driver::Mode::WalkUpRight); + } else if (mode == "cross_step") { + response->message = "Change the mode to Cross Step"; + req.header.identity.api_id = static_cast(go2_driver::Mode::CrossStep); + } else { + response->success = false; + response->message = "Invalid mode"; + return; + } + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Pose); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->level < -1 || request->level > 1) { + response->success = false; + response->message = "Speed level is out of range [-1 ~ 1]"; + return; + } + + nlohmann::json js; + js["data"] = request->level; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SpeedLevel); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->d < 0 || request->d > 4) { + response->success = false; + response->message = "Invalid gait type [0 - 4]"; + return; + } + + nlohmann::json js; + js["data"] = request->d; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchJoystick); + + request_pub_->publish(req); + response->success = true; +} + +void Go2HandleServices::handleGetBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::GetBodyHeight); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/sport/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Mode::GetBodyHeight)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetBodyHeight response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get body height"; + return; + } + + auto data = js.parse(response_msg->data); + // response->height = data["data"]; + response->success = true; + response->message = "Get body height successfully"; +} + + +void Go2HandleServices::handleGetFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::GetFootRaiseHeight); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/sport/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Mode::GetFootRaiseHeight)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetFootRaiseHeight response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get foot raise height"; + return; + } + + auto data = js.parse(response_msg->data); + response->height = data["data"]; + response->success = true; + response->message = "Get foot raise height successfully"; +} + +void Go2HandleServices::handleGetSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::GetSpeedLevel); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/sport/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Mode::GetSpeedLevel)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetSpeedLevel response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get speed level"; + return; + } + + auto data = js.parse(response_msg->data); + response->level = data["data"]; + response->success = true; + response->message = "Get speed level successfully"; +} + +void Go2HandleServices::handleGetState( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::GetState); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = node_->create_subscription( + "/api/sport/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Mode::GetState)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetState response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get state"; + return; + } + + auto data = js.parse(response_msg->data); + // response->state = data["data"]; // TODO(Juancams): Look at what field it is + response->success = true; + response->message = "Get state successfully"; +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_joint_states.cpp b/src/go2_driver/modules/go2_joint_states.cpp new file mode 100644 index 0000000..ecc622a --- /dev/null +++ b/src/go2_driver/modules/go2_joint_states.cpp @@ -0,0 +1,86 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + + +namespace go2_driver +{ + +Go2JointStates::Go2JointStates(const std::shared_ptr & node) +: node_(node) +{ +} + +CallbackReturnT Go2JointStates::on_configure() +{ + joint_state_pub_ = node_->create_publisher("joint_states", 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mJoint states module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_activate() +{ + joint_state_pub_->on_activate(); + + low_state_sub_ = node_->create_subscription( + "lowstate", 10, + std::bind(&Go2JointStates::publish_joint_states, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mJoint states module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_deactivate() +{ + joint_state_pub_->on_deactivate(); + + low_state_sub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mJoint states module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_cleanup() +{ + joint_state_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mJoint states module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2JointStates::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) +{ + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->now(); + joint_state.name = {"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"}; + + joint_state.position = {msg->motor_state[3].q, msg->motor_state[4].q, msg->motor_state[5].q, + msg->motor_state[0].q, msg->motor_state[1].q, msg->motor_state[2].q, + msg->motor_state[9].q, msg->motor_state[10].q, msg->motor_state[11].q, + msg->motor_state[6].q, msg->motor_state[7].q, msg->motor_state[8].q}; + + joint_state_pub_->publish(joint_state); +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_odometry.cpp b/src/go2_driver/modules/go2_odometry.cpp new file mode 100644 index 0000000..e48e351 --- /dev/null +++ b/src/go2_driver/modules/go2_odometry.cpp @@ -0,0 +1,105 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + + +namespace go2_driver +{ + +Go2Odometry::Go2Odometry(const std::shared_ptr & node) +: node_(node), + tf_broadcaster_(node) +{ +} + +CallbackReturnT Go2Odometry::on_configure() +{ + rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + + odom_pub_ = node_->create_publisher("odom", qos_profile); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mOdometry module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_activate() +{ + odom_pub_->on_activate(); + + odom_sub_ = node_->create_subscription( + "/utlidar/robot_odom", 10, + std::bind(&Go2Odometry::publish_odometry, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mOdometry module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_deactivate() +{ + odom_pub_->on_deactivate(); + + odom_sub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mOdometry module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_cleanup() +{ + odom_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mOdometry module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2Odometry::publish_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = msg->header.stamp; + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = msg->pose.pose.position.x; + transform.transform.translation.y = msg->pose.pose.position.y; + transform.transform.translation.z = msg->pose.pose.position.z; + transform.transform.rotation.x = msg->pose.pose.orientation.x; + transform.transform.rotation.y = msg->pose.pose.orientation.y; + transform.transform.rotation.z = msg->pose.pose.orientation.z; + transform.transform.rotation.w = msg->pose.pose.orientation.w; + tf_broadcaster_.sendTransform(transform); + + if (!odom_published_) { + nav_msgs::msg::Odometry odom; + odom.header.stamp = node_->now(); + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + odom.pose.pose.position.x = msg->pose.pose.position.x; + odom.pose.pose.position.y = msg->pose.pose.position.y; + odom.pose.pose.position.z = msg->pose.pose.position.z; + odom.pose.pose.orientation.x = msg->pose.pose.orientation.x; + odom.pose.pose.orientation.y = msg->pose.pose.orientation.y; + odom.pose.pose.orientation.z = msg->pose.pose.orientation.z; + odom.pose.pose.orientation.w = msg->pose.pose.orientation.w; + odom_pub_->publish(odom); + odom_published_ = true; + } +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp b/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp new file mode 100644 index 0000000..6ad8ce7 --- /dev/null +++ b/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp @@ -0,0 +1,162 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + + +namespace go2_driver +{ + +Go2SwitchObstaclesAvoidance::Go2SwitchObstaclesAvoidance( + const std::shared_ptr & node) +: node_(node) +{ +} + +CallbackReturnT Go2SwitchObstaclesAvoidance::on_configure() +{ + request_pub_ = node_->create_publisher( + "api/obstacles_avoid/request", + 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mObstacles Avoidance module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2SwitchObstaclesAvoidance::on_activate() +{ + request_pub_->on_activate(); + + get_obstacles_avoidance_service_ = + node_->create_service( + "get_obstacles_avoidance", + std::bind( + &Go2SwitchObstaclesAvoidance::handleGetObstaclesAvoidance, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_obstacles_avoidance_service_ = + node_->create_service( + "set_obstacles_avoidance", + std::bind( + &Go2SwitchObstaclesAvoidance::handleSetObstaclesAvoidance, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mObstacles Avoidance module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2SwitchObstaclesAvoidance::on_deactivate() +{ + request_pub_->on_deactivate(); + + get_obstacles_avoidance_service_.reset(); + set_obstacles_avoidance_service_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mObstacles Avoidance module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2SwitchObstaclesAvoidance::on_cleanup() +{ + request_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mObstacles Avoidance module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2SwitchObstaclesAvoidance::handleGetObstaclesAvoidance( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::ObstaclesAvoidance::GetSwitch); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/obstacles_avoid/response", 10, + [this, &response_msg]( + const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::ObstaclesAvoidance::GetSwitch)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetObstaclesAvoidance response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get switch obstacles avoidance"; + return; + } + + auto data = js.parse(response_msg->data); + response->enable = data["enable"]; + response->success = true; + response->message = "Get switch obstacles avoidance successfully"; +} + +void Go2SwitchObstaclesAvoidance::handleSetObstaclesAvoidance( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->enable < 0 || request->enable > 1) { + response->success = false; + response->message = "Switch value is out of range [0 ~ 1]"; + return; + } + + nlohmann::json js; + js["enable"] = request->enable; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::ObstaclesAvoidance::SetSwitch); + + request_pub_->publish(req); + response->success = true; + response->message = "Set switch obstacles avoidance successfully"; +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_tts.cpp b/src/go2_driver/modules/go2_tts.cpp new file mode 100644 index 0000000..eaab245 --- /dev/null +++ b/src/go2_driver/modules/go2_tts.cpp @@ -0,0 +1,162 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + +#include + + +namespace go2_driver +{ + +Go2TTS::Go2TTS(const std::shared_ptr & node) +: node_(node) +{ +} + +CallbackReturnT Go2TTS::on_configure() +{ + say_request_pub_ = node_->create_publisher("api/audiohub/request", 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mTTS module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2TTS::on_activate() +{ + say_request_pub_->on_activate(); + + say_service_ = + node_->create_service( + "say", + std::bind( + &Go2TTS::handleSay, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mTTS module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2TTS::on_deactivate() +{ + say_request_pub_->on_deactivate(); + + say_service_.reset(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2TTS::on_cleanup() +{ + say_request_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mTTS module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2TTS::handleSay( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + std::string text = request->text; + std::string command = "echo \"" + text + "\" | text2wave -o /tmp/output.wav"; + + system(command.c_str()); + + std::vector audio_data = read_file("/tmp/output.wav"); + if (audio_data.empty()) { + response->success = false; + return; + } + + std::vector> chunks = calculate_chunks(audio_data, 256 * 1024); + + size_t total_chunks = chunks.size(); + + unitree_api::msg::Request start_req; + start_req.header.identity.api_id = static_cast(go2_driver::Audio::StartAudio); + + // We waited a while for the open to be published + say_request_pub_->publish(start_req); + + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + for (size_t chunk_idx = 0; chunk_idx < total_chunks; ++chunk_idx) { + nlohmann::json j; + j["current_block_index"] = chunk_idx + 1; + j["total_block_number"] = total_chunks; + j["block_content"] = base64_encode(chunks[chunk_idx]); + + unitree_api::msg::Request req; + req.header.identity.api_id = static_cast(go2_driver::Audio::TTS); + + req.parameter = j.dump(); + + say_request_pub_->publish(req); + } + + response->success = true; +} + +std::vector Go2TTS::read_file(const std::string & filename) +{ + std::ifstream file(filename, std::ios::binary); + if (!file) { + std::cerr << "Error open: " << filename << std::endl; + return {}; + } + + return std::vector(std::istreambuf_iterator(file), {}); +} + +std::vector> Go2TTS::calculate_chunks( + const std::vector & data, + size_t chunk_size) +{ + std::vector> chunks; + + for (size_t i = 0; i < data.size(); i += chunk_size) { + size_t end = std::min(i + chunk_size, data.size()); + std::vector chunk(data.begin() + i, data.begin() + end); + chunks.push_back(chunk); + } + + return chunks; +} + +std::string Go2TTS::base64_encode(const std::vector & data) +{ + BIO * bio, * b64; + BUF_MEM * bufferPtr; + + b64 = BIO_new(BIO_f_base64()); + bio = BIO_new(BIO_s_mem()); + bio = BIO_push(b64, bio); + BIO_set_flags(bio, BIO_FLAGS_BASE64_NO_NL); + + BIO_write(bio, data.data(), data.size()); + BIO_flush(bio); + BIO_get_mem_ptr(bio, &bufferPtr); + + std::string encoded(bufferPtr->data, bufferPtr->length); + BIO_free_all(bio); + + return encoded; +} + +} // namespace go2_driver diff --git a/src/go2_driver/modules/go2_vui.cpp b/src/go2_driver/modules/go2_vui.cpp new file mode 100644 index 0000000..3583805 --- /dev/null +++ b/src/go2_driver/modules/go2_vui.cpp @@ -0,0 +1,340 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// 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. + + +#include + + +namespace go2_driver +{ + +Go2VUI::Go2VUI(const std::shared_ptr & node) +: node_(node) +{ +} + +CallbackReturnT Go2VUI::on_configure() +{ + request_pub_ = node_->create_publisher("api/vui/request", 10); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mVUI module configured.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_activate() +{ + request_pub_->on_activate(); + + get_brightness_service_ = + node_->create_service( + "get_brightness", + std::bind( + &Go2VUI::handleGetBrightness, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_switch_service_ = + node_->create_service( + "get_switch", + std::bind( + &Go2VUI::handleGetSwitch, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_volume_service_ = + node_->create_service( + "get_volume", + std::bind( + &Go2VUI::handleGetVolume, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_brightness_service_ = + node_->create_service( + "set_brightness", + std::bind( + &Go2VUI::handleSetBrightness, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_service_ = + node_->create_service( + "set_switch", + std::bind( + &Go2VUI::handleSetSwitch, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_volume_service_ = + node_->create_service( + "set_volume", + std::bind( + &Go2VUI::handleSetVolume, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mVUI module activated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_deactivate() +{ + request_pub_->on_deactivate(); + + get_brightness_service_.reset(); + get_switch_service_.reset(); + set_volume_service_.reset(); + set_brightness_service_.reset(); + set_switch_service_.reset(); + get_volume_service_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mVUI module deactivated.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_cleanup() +{ + request_pub_.reset(); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mVUI module cleaned up.\033[0m"); + + return CallbackReturnT::SUCCESS; +} + +void Go2VUI::handleGetBrightness( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::GetBrightness); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/vui/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Vui::GetBrightness)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetBrightness response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get brightness"; + return; + } + + auto data = js.parse(response_msg->data); + response->brightness = data["brightness"]; + response->success = true; + response->message = "Get brightness successfully"; +} + +void Go2VUI::handleGetSwitch( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::GetSwitch); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/vui/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Vui::GetSwitch)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetSwitch response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get switch"; + return; + } + + auto data = js.parse(response_msg->data); + response->enable = data["enable"]; + response->success = true; + response->message = "Get switch successfully"; +} + +void Go2VUI::handleGetVolume( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + (void)request; + + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::GetVolume); + + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); + + auto response_sub_ = aux_node->create_subscription( + "/api/vui/response", 10, + [this, &response_msg](const unitree_api::msg::Response::SharedPtr msg) { + if (msg->header.identity.api_id == static_cast(go2_driver::Vui::GetVolume)) { + response_msg = msg; + } + }); + + request_pub_->publish(req); + + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + + while (response_msg == nullptr) { + rclcpp::spin_some(aux_node); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto now = std::chrono::steady_clock::now(); + if (now - start_time > timeout) { + response->success = false; + response->message = "Timeout waiting for GetVolume response"; + return; + } + } + + if (response_msg->header.status.code != 0) { + response->success = false; + response->message = "Failed to get volume"; + return; + } + + auto data = js.parse(response_msg->data); + response->volume = data["volume"]; + response->success = true; + response->message = "Get volume successfully"; +} + +void Go2VUI::handleSetBrightness( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->brightness < 0 || request->brightness > 10) { + response->success = false; + response->message = "Brightness value is out of range [0 ~ 10]"; + return; + } + + nlohmann::json js; + js["brightness"] = request->brightness; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::SetBrightness); + + request_pub_->publish(req); + response->success = true; + response->message = "Set brightness successfully"; +} + +void Go2VUI::handleSetSwitch( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["enable"] = request->enable; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::SetSwitch); + + request_pub_->publish(req); + response->success = true; + response->message = "Set switch successfully"; +} + +void Go2VUI::handleSetVolume( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->volume < 0 || request->volume > 10) { + response->success = false; + response->message = "Volume value is out of range [0 ~ 10]"; + return; + } + + nlohmann::json js; + js["volume"] = request->volume; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::SetVolume); + + request_pub_->publish(req); + response->success = true; + response->message = "Set volume successfully"; +} + +} // namespace go2_driver