From 382c4edab4adc4804de34e4fd7ba4c643fa03768 Mon Sep 17 00:00:00 2001 From: Juancams Date: Fri, 20 Dec 2024 19:38:16 +0100 Subject: [PATCH 01/29] Changed license Signed-off-by: Juancams --- launch/go2_driver.launch.py | 42 ++++++++++++------------------------- 1 file changed, 13 insertions(+), 29 deletions(-) diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index fc0541e..fb2e547 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -1,32 +1,16 @@ -# 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 (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. from launch import LaunchDescription from launch_ros.actions import ComposableNodeContainer, Node From 8962aa98ea9010787d61f1e3794030edf0d6e0da Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 10:55:42 +0100 Subject: [PATCH 02/29] Create pull_request_template.md --- .github/pull_request_template.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/pull_request_template.md 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. From c88e334bc44ae76e0bcb1c1f43f6e8bc447483e8 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:22:05 +0100 Subject: [PATCH 03/29] Create humble-devel.yaml --- .github/workflows/humble-devel.yaml | 38 +++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 .github/workflows/humble-devel.yaml diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml new file mode 100644 index 0000000..e14f811 --- /dev/null +++ b/.github/workflows/humble-devel.yaml @@ -0,0 +1,38 @@ +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: Clone additional ROS 2 packages + run: | + mkdir -p src + cd src + git clone https://github.com/Unitree-Go2-Robot/unitree_go + git clone https://github.com/Unitree-Go2-Robot/unitree_api + git clone https://github.com/Unitree-Go2-Robot/go2_interfaces + shell: bash + + - name: Build ROS 2 package + uses: ros-tooling/action-ros-ci@0.3.13 + with: + package-name: go2_driver + target-ros2-distro: humble From d91237940f9a88191ae7ef59f68f2af7ec5d3a0a Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:26:52 +0100 Subject: [PATCH 04/29] Update humble-devel.yaml --- .github/workflows/humble-devel.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml index e14f811..34424a9 100644 --- a/.github/workflows/humble-devel.yaml +++ b/.github/workflows/humble-devel.yaml @@ -31,8 +31,8 @@ jobs: git clone https://github.com/Unitree-Go2-Robot/go2_interfaces shell: bash - - name: Build ROS 2 package + - name: Build ROS 2 package unitree_go unitree_api go2_interfaces uses: ros-tooling/action-ros-ci@0.3.13 with: - package-name: go2_driver + package-name: go2_driver target-ros2-distro: humble From b18ca0c11f5e6668c02eed8e4cca4db6ca64093e Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:40:09 +0100 Subject: [PATCH 05/29] Create dependencies.repos --- dependencies.repos | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 dependencies.repos 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 From 289b9df16a50e3fa512dc419a9e56d0d3043f2da Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:43:45 +0100 Subject: [PATCH 06/29] Update humble-devel.yaml --- .github/workflows/humble-devel.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml index 34424a9..b1740a5 100644 --- a/.github/workflows/humble-devel.yaml +++ b/.github/workflows/humble-devel.yaml @@ -31,8 +31,8 @@ jobs: git clone https://github.com/Unitree-Go2-Robot/go2_interfaces shell: bash - - name: Build ROS 2 package unitree_go unitree_api go2_interfaces + - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@0.3.13 with: - package-name: go2_driver + package-name: go2_driver unitree_go unitree_api go2_interfaces target-ros2-distro: humble From 879db276185d7f8c57d21e28f81b439fd002f9ac Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:49:05 +0100 Subject: [PATCH 07/29] Update humble-devel.yaml --- .github/workflows/humble-devel.yaml | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/.github/workflows/humble-devel.yaml b/.github/workflows/humble-devel.yaml index b1740a5..24805c8 100644 --- a/.github/workflows/humble-devel.yaml +++ b/.github/workflows/humble-devel.yaml @@ -22,17 +22,9 @@ jobs: with: ros-distro: humble - - name: Clone additional ROS 2 packages - run: | - mkdir -p src - cd src - git clone https://github.com/Unitree-Go2-Robot/unitree_go - git clone https://github.com/Unitree-Go2-Robot/unitree_api - git clone https://github.com/Unitree-Go2-Robot/go2_interfaces - shell: bash - - 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 From 8cc05e2ae880e5518be4bc23fa730ab24743fff0 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Fri, 14 Feb 2025 11:55:39 +0100 Subject: [PATCH 08/29] Create humble.yaml --- .github/workflows/humble.yaml | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 .github/workflows/humble.yaml 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 From 36db95c5964129f3782c94bd1de455f8ded6bab4 Mon Sep 17 00:00:00 2001 From: Juan Carlos Manzanares Serrano Date: Wed, 19 Feb 2025 17:31:28 +0100 Subject: [PATCH 09/29] Changed license Signed-off-by: Juan Carlos Manzanares Serrano --- include/go2_driver/go2_api_id.hpp | 42 ++++++++++--------------------- 1 file changed, 13 insertions(+), 29 deletions(-) diff --git a/include/go2_driver/go2_api_id.hpp b/include/go2_driver/go2_api_id.hpp index e9517f5..a03b566 100644 --- a/include/go2_driver/go2_api_id.hpp +++ b/include/go2_driver/go2_api_id.hpp @@ -1,32 +1,16 @@ -// 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_API_ID_HPP #define GO2_DRIVER__GO2_API_ID_HPP From f535f4e9c93ca257539192f72acc22601f9598cc Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 19 Feb 2025 17:42:05 +0100 Subject: [PATCH 10/29] Added camera support Signed-off-by: Juancams --- CMakeLists.txt | 1 + include/go2_driver/go2_driver.hpp | 60 ++++++------ src/go2_driver/go2_driver.cpp | 156 ++++++++++++++++++++++++------ 3 files changed, 158 insertions(+), 59 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cfb2ca..7887de6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale) rclcpp_components_register_nodes(${PROJECT_NAME} "go2_driver::Go2Driver") diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index aad7ff6..8aec3f0 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -1,32 +1,16 @@ -// 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_ @@ -34,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +26,7 @@ #include #include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" +#include "unitree_go/msg/go2_front_video_data.hpp" #include "tf2_ros/transform_broadcaster.h" #include "nlohmann/json.hpp" #include "unitree_api/msg/request.hpp" @@ -55,6 +41,13 @@ #include "go2_interfaces/srv/switch_gait.hpp" #include "go2_interfaces/srv/switch_joystick.hpp" +extern "C" { + #include + #include + #include + #include +} + namespace go2_driver { @@ -69,6 +62,7 @@ class Go2Driver : public rclcpp::Node 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 front_video_data_callback(const unitree_go::msg::Go2FrontVideoData::SharedPtr msg); void handleBodyHeight( const std::shared_ptr request_header, @@ -120,12 +114,14 @@ class Go2Driver : public rclcpp::Node rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Subscription::SharedPtr low_state_sub_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Subscription::SharedPtr front_video_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::Publisher::SharedPtr image_publisher_; rclcpp::Service::SharedPtr set_body_height_service_; rclcpp::Service::SharedPtr set_continuous_gait_service_; @@ -140,8 +136,14 @@ class Go2Driver : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_lidar_; tf2_ros::TransformBroadcaster tf_broadcaster_; + int camera_resolution_; sensor_msgs::msg::Joy joy_state_; + AVCodec * codec_; + AVCodecContext * p_codec_context_; + AVPacket * sps_packet_; + AVPacket * pps_packet_; + bool sps_sent_{false}; bool odom_published_{false}; }; diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index beb5804..f4ed4bb 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -1,51 +1,60 @@ -// 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) + tf_broadcaster_(this), + codec_(), + p_codec_context_(), + sps_packet_(), + pps_packet_() { rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + declare_parameter("camera_resolution", 720); + get_parameter("camera_resolution", camera_resolution_); + + codec_ = avcodec_find_decoder(AV_CODEC_ID_H264); + if (!codec_) { + RCLCPP_ERROR(get_logger(), "Failed to find codec."); + return; + } + p_codec_context_ = avcodec_alloc_context3(codec_); + if (p_codec_context_ == nullptr) { + RCLCPP_ERROR(get_logger(), "Failed to allocate codec context."); + return; + } + + if (avcodec_open2(p_codec_context_, codec_, nullptr) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to open codec."); + return; + } + 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); + image_publisher_ = create_publisher("/image_raw", 10); pointcloud_sub_ = create_subscription( "/utlidar/cloud", 10, @@ -65,6 +74,10 @@ Go2Driver::Go2Driver( cmd_vel_sub_ = create_subscription( "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); + front_video_sub_ = create_subscription( + "frontvideostream", 10, + std::bind(&Go2Driver::front_video_data_callback, this, std::placeholders::_1)); + set_body_height_service_ = this->create_service( "body_height", @@ -173,6 +186,89 @@ void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) joy_state_ = *msg; } +void Go2Driver::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_INFO(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_INFO(get_logger(), "PPS received."); + } + } + } + + if (!sps_packet_ || !pps_packet_) { + RCLCPP_WARN(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_ERROR(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_ERROR(get_logger(), "Failed to receive frame from decoder: %s", errbuf); + av_frame_free(&frame); + return; + } + + SwsContext * sws_ctx = sws_getContext( + frame->width, frame->height, AV_PIX_FMT_YUV420P, + frame->width, frame->height, AV_PIX_FMT_BGR24, + SWS_BICUBIC, NULL, NULL, NULL); + + int bgr_size = av_image_get_buffer_size(AV_PIX_FMT_BGR24, frame->width, frame->height, 1); + std::vector bgr_data(bgr_size); + + uint8_t * bgr_planes[3] = {bgr_data.data(), NULL, NULL}; + int bgr_strides[3] = {frame->width * 3, 0, 0}; + sws_scale(sws_ctx, frame->data, frame->linesize, 0, frame->height, bgr_planes, bgr_strides); + + auto image_msg = std::make_shared(); + image_msg->header.stamp = this->get_clock()->now(); + image_msg->header.frame_id = "camera_frame"; + image_msg->height = frame->height; + image_msg->width = frame->width; + image_msg->encoding = "bgr8"; + image_msg->step = frame->width * 3; + image_msg->data = bgr_data; + + image_publisher_->publish(*image_msg); +} + void Go2Driver::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) { sensor_msgs::msg::JointState joint_state; From 29274a386562bee688e7fed749f7d80a3600af65 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 20 Feb 2025 09:51:00 +0100 Subject: [PATCH 11/29] Changed to convert with opencv Signed-off-by: Juancams --- CMakeLists.txt | 4 ++++ include/go2_driver/go2_driver.hpp | 3 +++ src/go2_driver/go2_driver.cpp | 25 +++++++++---------------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7887de6..bdfe94a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,8 @@ 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) set(dependencies rclcpp @@ -28,6 +30,8 @@ set(dependencies unitree_api tf2_ros go2_interfaces + cv_bridge + OpenCV ) include_directories(include) diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index 8aec3f0..fc11922 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -48,6 +48,9 @@ extern "C" { #include } +#include +#include + namespace go2_driver { diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index f4ed4bb..0c3d4c4 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -245,28 +245,21 @@ void Go2Driver::front_video_data_callback(const unitree_go::msg::Go2FrontVideoDa return; } - SwsContext * sws_ctx = sws_getContext( - frame->width, frame->height, AV_PIX_FMT_YUV420P, - frame->width, frame->height, AV_PIX_FMT_BGR24, - SWS_BICUBIC, NULL, NULL, NULL); + 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); - int bgr_size = av_image_get_buffer_size(AV_PIX_FMT_BGR24, frame->width, frame->height, 1); - std::vector bgr_data(bgr_size); + cv::Mat bgr; + cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGBy); - uint8_t * bgr_planes[3] = {bgr_data.data(), NULL, NULL}; - int bgr_strides[3] = {frame->width * 3, 0, 0}; - sws_scale(sws_ctx, frame->data, frame->linesize, 0, frame->height, bgr_planes, bgr_strides); - - auto image_msg = std::make_shared(); + auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toImageMsg(); image_msg->header.stamp = this->get_clock()->now(); image_msg->header.frame_id = "camera_frame"; - image_msg->height = frame->height; - image_msg->width = frame->width; - image_msg->encoding = "bgr8"; - image_msg->step = frame->width * 3; - image_msg->data = bgr_data; image_publisher_->publish(*image_msg); + + av_frame_free(&frame); } void Go2Driver::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) From e83eed8e2e11b570e33c5167b0256eead0aa9c83 Mon Sep 17 00:00:00 2001 From: Juancams Date: Fri, 21 Feb 2025 12:15:24 +0100 Subject: [PATCH 12/29] Bug Signed-off-by: Juancams --- src/go2_driver/go2_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index 0c3d4c4..1eaddf5 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -251,7 +251,7 @@ void Go2Driver::front_video_data_callback(const unitree_go::msg::Go2FrontVideoDa 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_YUV420p2RGBy); + cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGB); auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toImageMsg(); image_msg->header.stamp = this->get_clock()->now(); From c9770a5ccd591424353e6c1ebc7ff6082316071b Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 09:53:21 +0100 Subject: [PATCH 13/29] Refactored all code into modules & added new modules/features Signed-off-by: Juancams --- CMakeLists.txt | 10 +- include/go2_driver/go2_driver.hpp | 144 +--- include/go2_driver/modules/go2_camera.hpp | 68 ++ .../modules/go2_handle_services.hpp | 150 ++++ .../go2_driver/modules/go2_joint_states.hpp | 57 ++ include/go2_driver/modules/go2_odometry.hpp | 59 ++ include/go2_driver/modules/go2_tts.hpp | 71 ++ include/go2_driver/modules/go2_vui.hpp | 67 ++ include/go2_driver/{ => utils}/go2_api_id.hpp | 25 + .../go2_driver/utils/go2_lifecycle_node.hpp | 40 ++ launch/go2_driver.launch.py | 2 +- src/go2_driver/go2_driver.cpp | 551 ++------------- src/go2_driver/modules/go2_camera.cpp | 163 +++++ .../modules/go2_handle_services.cpp | 659 ++++++++++++++++++ src/go2_driver/modules/go2_joint_states.cpp | 74 ++ src/go2_driver/modules/go2_odometry.cpp | 93 +++ src/go2_driver/modules/go2_tts.cpp | 153 ++++ src/go2_driver/modules/go2_vui.cpp | 141 ++++ 18 files changed, 1899 insertions(+), 628 deletions(-) create mode 100644 include/go2_driver/modules/go2_camera.hpp create mode 100644 include/go2_driver/modules/go2_handle_services.hpp create mode 100644 include/go2_driver/modules/go2_joint_states.hpp create mode 100644 include/go2_driver/modules/go2_odometry.hpp create mode 100644 include/go2_driver/modules/go2_tts.hpp create mode 100644 include/go2_driver/modules/go2_vui.hpp rename include/go2_driver/{ => utils}/go2_api_id.hpp (80%) create mode 100644 include/go2_driver/utils/go2_lifecycle_node.hpp create mode 100644 src/go2_driver/modules/go2_camera.cpp create mode 100644 src/go2_driver/modules/go2_handle_services.cpp create mode 100644 src/go2_driver/modules/go2_joint_states.cpp create mode 100644 src/go2_driver/modules/go2_odometry.cpp create mode 100644 src/go2_driver/modules/go2_tts.cpp create mode 100644 src/go2_driver/modules/go2_vui.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bdfe94a..fcc6532 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ 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 @@ -32,16 +33,23 @@ set(dependencies 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 ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale) +target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale OpenSSL::SSL OpenSSL::Crypto) rclcpp_components_register_nodes(${PROJECT_NAME} "go2_driver::Go2Driver") diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index fc11922..477fd11 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -16,138 +16,38 @@ #define GO2_DRIVER__GO2_DRIVER_HPP_ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include "unitree_go/msg/low_state.hpp" -#include "unitree_go/msg/imu_state.hpp" -#include "unitree_go/msg/go2_front_video_data.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" -extern "C" { - #include - #include - #include - #include -} - -#include -#include 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 front_video_data_callback(const unitree_go::msg::Go2FrontVideoData::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); + 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 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::Subscription::SharedPtr front_video_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::Publisher::SharedPtr image_publisher_; - - 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_; - int camera_resolution_; - sensor_msgs::msg::Joy joy_state_; - AVCodec * codec_; - AVCodecContext * p_codec_context_; - AVPacket * sps_packet_; - AVPacket * pps_packet_; - - bool sps_sent_{false}; - 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_; }; } // 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..d9754ba --- /dev/null +++ b/include/go2_driver/modules/go2_camera.hpp @@ -0,0 +1,68 @@ +// 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 "go2_driver/utils/go2_lifecycle_node.hpp" +#include +#include +#include + +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); + + rclcpp::Subscription::SharedPtr front_video_sub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr image_publisher_; + + std::shared_ptr node_; + + AVCodec * codec_; + AVCodecContext * p_codec_context_; + AVPacket * sps_packet_; + AVPacket * pps_packet_; + bool sps_sent_{false}; + int camera_resolution_; +}; + +} // 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..d6dc1b2 --- /dev/null +++ b/include/go2_driver/modules/go2_handle_services.hpp @@ -0,0 +1,150 @@ +// 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 "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 "geometry_msgs/msg/twist.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..e7c129e --- /dev/null +++ b/include/go2_driver/modules/go2_joint_states.hpp @@ -0,0 +1,57 @@ +// 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 "go2_driver/utils/go2_lifecycle_node.hpp" +#include "go2_driver/utils/go2_api_id.hpp" +#include "unitree_api/msg/request.hpp" +#include "nlohmann/json.hpp" +#include "unitree_go/msg/low_state.hpp" +#include + +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..19a8029 --- /dev/null +++ b/include/go2_driver/modules/go2_odometry.hpp @@ -0,0 +1,59 @@ +// 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 "go2_driver/utils/go2_lifecycle_node.hpp" +#include +#include "tf2_ros/transform_broadcaster.h" +#include + + +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_tts.hpp b/include/go2_driver/modules/go2_tts.hpp new file mode 100644 index 0000000..649a2aa --- /dev/null +++ b/include/go2_driver/modules/go2_tts.hpp @@ -0,0 +1,71 @@ +// 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 "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" +#include +#include +#include +#include +#include +#include + +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..d850259 --- /dev/null +++ b/include/go2_driver/modules/go2_vui.hpp @@ -0,0 +1,67 @@ +// 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/set_volume.hpp" +#include "go2_interfaces/srv/get_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 handleSetVolume( + 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); + + rclcpp::Service::SharedPtr set_volume_service_; + rclcpp::Service::SharedPtr get_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/go2_api_id.hpp b/include/go2_driver/utils/go2_api_id.hpp similarity index 80% rename from include/go2_driver/go2_api_id.hpp rename to include/go2_driver/utils/go2_api_id.hpp index a03b566..132ed2d 100644 --- a/include/go2_driver/go2_api_id.hpp +++ b/include/go2_driver/utils/go2_api_id.hpp @@ -56,6 +56,31 @@ enum class Mode 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 +{ + SetVolume = 1003, + GetVolume = 1004, }; } // namespace go2_driver 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..cec78e8 --- /dev/null +++ b/include/go2_driver/utils/go2_lifecycle_node.hpp @@ -0,0 +1,40 @@ +// 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 + + +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 fb2e547..050c656 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -54,6 +54,6 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(container) - ld.add_action(pointclod_to_laserscan_cmd) + # ld.add_action(pointclod_to_laserscan_cmd) return ld diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index 1eaddf5..f831d10 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -20,532 +20,75 @@ namespace go2_driver Go2Driver::Go2Driver( const rclcpp::NodeOptions & options) -: Node("go2_driver", options), - tf_broadcaster_(this), - codec_(), - p_codec_context_(), - sps_packet_(), - pps_packet_() +: LifecycleNode("go2_driver", options) { - rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - - declare_parameter("camera_resolution", 720); - get_parameter("camera_resolution", camera_resolution_); - - codec_ = avcodec_find_decoder(AV_CODEC_ID_H264); - if (!codec_) { - RCLCPP_ERROR(get_logger(), "Failed to find codec."); - return; - } - p_codec_context_ = avcodec_alloc_context3(codec_); - if (p_codec_context_ == nullptr) { - RCLCPP_ERROR(get_logger(), "Failed to allocate codec context."); - return; - } - - if (avcodec_open2(p_codec_context_, codec_, nullptr) < 0) { - RCLCPP_ERROR(get_logger(), "Failed to open codec."); - return; - } - - 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); - image_publisher_ = create_publisher("/image_raw", 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)); - - front_video_sub_ = create_subscription( - "frontvideostream", 10, - std::bind(&Go2Driver::front_video_data_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)); -} - -void Go2Driver::publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - 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; - } -} - -void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) -{ - joy_state_ = *msg; -} - -void Go2Driver::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_INFO(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_INFO(get_logger(), "PPS received."); - } - } - } - - if (!sps_packet_ || !pps_packet_) { - RCLCPP_WARN(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_ERROR(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_ERROR(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); - - auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toImageMsg(); - image_msg->header.stamp = this->get_clock()->now(); - image_msg->header.frame_id = "camera_frame"; - - image_publisher_->publish(*image_msg); - - av_frame_free(&frame); -} - -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; - - 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 Go2Driver::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.3 ~ 0.5]"; - 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; -} - -void Go2Driver::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 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; - } - - 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 Go2Driver::handleFootRaiseHeight( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) +CallbackReturnT Go2Driver::on_configure(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; - } + go2_services_handler_ = std::make_shared(shared_from_this()); + go2_camera_ = std::make_shared(shared_from_this()); + go2_tts_ = std::make_shared(shared_from_this()); + go2_vui_ = std::make_shared(shared_from_this()); + go2_odometry_ = std::make_shared(shared_from_this()); + go2_joint_states_ = std::make_shared(shared_from_this()); - nlohmann::json js; - js["data"] = request->height; + go2_services_handler_->on_configure(); + go2_camera_->on_configure(); + go2_tts_->on_configure(); + go2_vui_->on_configure(); + go2_odometry_->on_configure(); + go2_joint_states_->on_configure(); - 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; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleMode( - 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; - 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; - } + go2_services_handler_->on_activate(); + go2_camera_->on_activate(); + go2_tts_->on_activate(); + go2_vui_->on_activate(); + go2_odometry_->on_activate(); + go2_joint_states_->on_activate(); - request_pub_->publish(req); - response->success = true; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handlePose( - 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; + go2_services_handler_->on_deactivate(); + go2_camera_->on_deactivate(); + go2_tts_->on_deactivate(); + go2_vui_->on_deactivate(); + go2_odometry_->on_deactivate(); + go2_joint_states_->on_deactivate(); - 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; + 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; - } + go2_services_handler_->on_cleanup(); + go2_camera_->on_cleanup(); + go2_tts_->on_cleanup(); + go2_vui_->on_cleanup(); + go2_odometry_->on_cleanup(); + go2_joint_states_->on_cleanup(); - nlohmann::json js; - js["data"] = request->level; + go2_services_handler_.reset(); + go2_camera_.reset(); + go2_tts_.reset(); + go2_vui_.reset(); + go2_odometry_.reset(); + go2_joint_states_.reset(); - 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; + return CallbackReturnT::SUCCESS; } -void Go2Driver::handleSwitchGait( - 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; - - 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 Go2Driver::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; + 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..6c3493b --- /dev/null +++ b/src/go2_driver/modules/go2_camera.cpp @@ -0,0 +1,163 @@ +// 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_->get_parameter("camera_resolution", camera_resolution_); +} + +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); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_activate() +{ + image_publisher_->on_activate(); + + front_video_sub_ = node_->create_subscription( + "frontvideostream", 10, + std::bind(&Go2Camera::front_video_data_callback, this, std::placeholders::_1)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_deactivate() +{ + image_publisher_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Camera::on_cleanup() +{ + 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); + + 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); + + av_frame_free(&frame); +} + +} // 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..31404fe --- /dev/null +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -0,0 +1,659 @@ +// 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); + + cmd_vel_sub_ = node_->create_subscription( + "cmd_vel", 10, std::bind(&Go2HandleServices::cmd_vel_callback, this, std::placeholders::_1)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_activate() +{ + request_pub_->on_activate(); + + 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)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_deactivate() +{ + request_pub_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2HandleServices::on_cleanup() +{ + 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.3 ~ 0.5]"; + 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; +} + +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 [-1.5 ~ 1.5]"; + 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 || request->height > 0.1) { + 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; +} + +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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + 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::GetBodyHeight)) { + response_msg = msg; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + 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::GetFootRaiseHeight)) { + response_msg = msg; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + 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::GetSpeedLevel)) { + response_msg = msg; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + 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; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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..1972722 --- /dev/null +++ b/src/go2_driver/modules/go2_joint_states.cpp @@ -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. + + +#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); + + low_state_sub_ = node_->create_subscription( + "lowstate", 10, + std::bind(&Go2JointStates::publish_joint_states, this, std::placeholders::_1)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_activate() +{ + joint_state_pub_->on_activate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_deactivate() +{ + joint_state_pub_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2JointStates::on_cleanup() +{ + 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..4e6e567 --- /dev/null +++ b/src/go2_driver/modules/go2_odometry.cpp @@ -0,0 +1,93 @@ +// 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); + + odom_sub_ = node_->create_subscription( + "/utlidar/robot_odom", 10, + std::bind(&Go2Odometry::publish_odometry, this, std::placeholders::_1)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_activate() +{ + odom_pub_->on_activate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_deactivate() +{ + odom_pub_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2Odometry::on_cleanup() +{ + 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_tts.cpp b/src/go2_driver/modules/go2_tts.cpp new file mode 100644 index 0000000..e68e5b2 --- /dev/null +++ b/src/go2_driver/modules/go2_tts.cpp @@ -0,0 +1,153 @@ +// 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); + + 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)); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2TTS::on_deactivate() +{ + say_request_pub_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2TTS::on_cleanup() +{ + 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..40943b3 --- /dev/null +++ b/src/go2_driver/modules/go2_vui.cpp @@ -0,0 +1,141 @@ +// 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() +{ + set_volume_service_ = + node_->create_service( + "set_volume", + std::bind( + &Go2VUI::handleSetVolume, 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)); + + request_pub_ = node_->create_publisher("api/vui/request", 10); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_activate() +{ + request_pub_->on_activate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_deactivate() +{ + request_pub_->on_deactivate(); + + return CallbackReturnT::SUCCESS; +} + +CallbackReturnT Go2VUI::on_cleanup() +{ + return CallbackReturnT::SUCCESS; +} + +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; +} + +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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + auto response_sub_ = 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; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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["data"]; + response->success = true; + response->message = "Get volume successfully"; +} + +} // namespace go2_driver From 16e3bb651ba4fc048abca02379aa2cec471f5fda Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 10:17:51 +0100 Subject: [PATCH 14/29] Added packages dependencies Signed-off-by: Juancams --- package.xml | 3 +++ 1 file changed, 3 insertions(+) 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 From 683f59f3b6627a9dc46e867169739cca9f88e783 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 11:48:11 +0100 Subject: [PATCH 15/29] Added traces, reset variables and parameters to not activate modules Signed-off-by: Juancams --- include/go2_driver/go2_driver.hpp | 7 + launch/go2_driver.launch.py | 76 ++++++-- src/go2_driver/go2_driver.cpp | 163 ++++++++++++++---- src/go2_driver/modules/go2_camera.cpp | 14 ++ .../modules/go2_handle_services.cpp | 29 +++- src/go2_driver/modules/go2_joint_states.cpp | 18 +- src/go2_driver/modules/go2_odometry.cpp | 18 +- src/go2_driver/modules/go2_tts.cpp | 11 +- src/go2_driver/modules/go2_vui.cpp | 29 +++- 9 files changed, 294 insertions(+), 71 deletions(-) diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index 477fd11..2afd84e 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -48,6 +48,13 @@ class Go2Driver : public rclcpp_lifecycle::LifecycleNode std::shared_ptr go2_vui_; std::shared_ptr go2_odometry_; std::shared_ptr go2_joint_states_; + + bool use_camera_; + bool use_tts_; + bool use_vui_; + bool use_odometry_; + bool use_joint_states_; + bool use_services_; }; } // namespace go2_driver diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 050c656..0c7b0ee 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -13,12 +13,57 @@ # limitations under the License. from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration def generate_launch_description(): + 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') + + 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' + ) + composable_nodes = [] composable_node = ComposableNode( @@ -26,8 +71,15 @@ def generate_launch_description(): plugin='go2_driver::Go2Driver', name='go2_driver', namespace='', - + parameters=[{'use_camera': use_camera, + 'use_tts': use_tts, + 'use_vui': use_vui, + 'use_odometry': use_odometry, + 'use_joint_states': use_joint_states, + 'use_services': use_services, + }], ) + composable_nodes.append(composable_node) container = ComposableNodeContainer( @@ -39,21 +91,13 @@ 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, - }], - ) - 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(container) - # ld.add_action(pointclod_to_laserscan_cmd) return ld diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index f831d10..2c37367 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -22,66 +22,153 @@ Go2Driver::Go2Driver( const rclcpp::NodeOptions & options) : 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); + + 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_); } CallbackReturnT Go2Driver::on_configure(const rclcpp_lifecycle::State &) { - go2_services_handler_ = std::make_shared(shared_from_this()); - go2_camera_ = std::make_shared(shared_from_this()); - go2_tts_ = std::make_shared(shared_from_this()); - go2_vui_ = std::make_shared(shared_from_this()); - go2_odometry_ = std::make_shared(shared_from_this()); - go2_joint_states_ = std::make_shared(shared_from_this()); - - go2_services_handler_->on_configure(); - go2_camera_->on_configure(); - go2_tts_->on_configure(); - go2_vui_->on_configure(); - go2_odometry_->on_configure(); - go2_joint_states_->on_configure(); + if (use_camera_) { + go2_camera_ = std::make_shared(shared_from_this()); + go2_camera_->on_configure(); + } + + if (use_tts_) { + go2_tts_ = std::make_shared(shared_from_this()); + go2_tts_->on_configure(); + } + + if (use_vui_) { + go2_vui_ = std::make_shared(shared_from_this()); + go2_vui_->on_configure(); + } + + if (use_odometry_) { + go2_odometry_ = std::make_shared(shared_from_this()); + go2_odometry_->on_configure(); + } + + if (use_joint_states_) { + go2_joint_states_ = std::make_shared(shared_from_this()); + go2_joint_states_->on_configure(); + } + + if (use_services_) { + go2_services_handler_ = std::make_shared(shared_from_this()); + go2_services_handler_->on_configure(); + } + + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules configured\033[0m"); return CallbackReturnT::SUCCESS; } CallbackReturnT Go2Driver::on_activate(const rclcpp_lifecycle::State &) { - go2_services_handler_->on_activate(); - go2_camera_->on_activate(); - go2_tts_->on_activate(); - go2_vui_->on_activate(); - go2_odometry_->on_activate(); - go2_joint_states_->on_activate(); + if (use_camera_) { + go2_camera_->on_activate(); + } + + if (use_tts_) { + go2_tts_->on_activate(); + } + + if (use_vui_) { + go2_vui_->on_activate(); + } + + if (use_odometry_) { + go2_odometry_->on_activate(); + } + + if (use_joint_states_) { + go2_joint_states_->on_activate(); + } + + if (use_services_) { + go2_services_handler_->on_activate(); + } + + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules activated\033[0m"); return CallbackReturnT::SUCCESS; } CallbackReturnT Go2Driver::on_deactivate(const rclcpp_lifecycle::State &) { - go2_services_handler_->on_deactivate(); - go2_camera_->on_deactivate(); - go2_tts_->on_deactivate(); - go2_vui_->on_deactivate(); - go2_odometry_->on_deactivate(); - go2_joint_states_->on_deactivate(); + if (use_camera_) { + go2_camera_->on_deactivate(); + } + + if (use_tts_) { + go2_tts_->on_deactivate(); + } + + if (use_vui_) { + go2_vui_->on_deactivate(); + } + + if (use_odometry_) { + go2_odometry_->on_deactivate(); + } + + if (use_joint_states_) { + go2_joint_states_->on_deactivate(); + } + + if (use_services_) { + go2_services_handler_->on_deactivate(); + } + + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules deactivated\033[0m"); return CallbackReturnT::SUCCESS; } CallbackReturnT Go2Driver::on_cleanup(const rclcpp_lifecycle::State &) { - go2_services_handler_->on_cleanup(); - go2_camera_->on_cleanup(); - go2_tts_->on_cleanup(); - go2_vui_->on_cleanup(); - go2_odometry_->on_cleanup(); - go2_joint_states_->on_cleanup(); - - go2_services_handler_.reset(); - go2_camera_.reset(); - go2_tts_.reset(); - go2_vui_.reset(); - go2_odometry_.reset(); - go2_joint_states_.reset(); + if (use_camera_) { + go2_camera_->on_cleanup(); + go2_camera_.reset(); + } + + if (use_tts_) { + go2_tts_->on_cleanup(); + go2_tts_.reset(); + } + + if (use_vui_) { + go2_vui_->on_cleanup(); + go2_vui_.reset(); + } + + if (use_odometry_) { + go2_odometry_->on_cleanup(); + go2_odometry_.reset(); + } + + if (use_joint_states_) { + go2_joint_states_->on_cleanup(); + go2_joint_states_.reset(); + } + + if (use_services_) { + go2_services_handler_->on_cleanup(); + go2_services_handler_.reset(); + } + + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules cleaned up\033[0m"); return CallbackReturnT::SUCCESS; } diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp index 6c3493b..3e06355 100644 --- a/src/go2_driver/modules/go2_camera.cpp +++ b/src/go2_driver/modules/go2_camera.cpp @@ -51,6 +51,8 @@ CallbackReturnT Go2Camera::on_configure() image_publisher_ = node_->create_publisher("/image_raw", 10); + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module configured.\033[0m"); + return CallbackReturnT::SUCCESS; } @@ -62,6 +64,8 @@ CallbackReturnT Go2Camera::on_activate() "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; } @@ -69,11 +73,21 @@ CallbackReturnT Go2Camera::on_deactivate() { image_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(); + + avcodec_free_context(&p_codec_context_); + + RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module cleaned up.\033[0m"); + return CallbackReturnT::SUCCESS; } diff --git a/src/go2_driver/modules/go2_handle_services.cpp b/src/go2_driver/modules/go2_handle_services.cpp index 31404fe..3af0c86 100644 --- a/src/go2_driver/modules/go2_handle_services.cpp +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -28,8 +28,7 @@ CallbackReturnT Go2HandleServices::on_configure() { request_pub_ = node_->create_publisher("api/sport/request", 10); - cmd_vel_sub_ = node_->create_subscription( - "cmd_vel", 10, std::bind(&Go2HandleServices::cmd_vel_callback, this, std::placeholders::_1)); + RCLCPP_INFO(node_->get_logger(), "\033[1;34mHandle services module configured.\033[0m"); return CallbackReturnT::SUCCESS; } @@ -38,6 +37,9 @@ 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", @@ -129,6 +131,8 @@ CallbackReturnT Go2HandleServices::on_activate() &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; } @@ -136,11 +140,32 @@ 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; } diff --git a/src/go2_driver/modules/go2_joint_states.cpp b/src/go2_driver/modules/go2_joint_states.cpp index 1972722..ecc622a 100644 --- a/src/go2_driver/modules/go2_joint_states.cpp +++ b/src/go2_driver/modules/go2_joint_states.cpp @@ -28,9 +28,7 @@ CallbackReturnT Go2JointStates::on_configure() { joint_state_pub_ = node_->create_publisher("joint_states", 10); - 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 configured.\033[0m"); return CallbackReturnT::SUCCESS; } @@ -39,6 +37,12 @@ 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; } @@ -46,11 +50,19 @@ 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; } diff --git a/src/go2_driver/modules/go2_odometry.cpp b/src/go2_driver/modules/go2_odometry.cpp index 4e6e567..e48e351 100644 --- a/src/go2_driver/modules/go2_odometry.cpp +++ b/src/go2_driver/modules/go2_odometry.cpp @@ -32,9 +32,7 @@ CallbackReturnT Go2Odometry::on_configure() odom_pub_ = node_->create_publisher("odom", qos_profile); - 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 configured.\033[0m"); return CallbackReturnT::SUCCESS; } @@ -43,6 +41,12 @@ 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; } @@ -50,11 +54,19 @@ 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; } diff --git a/src/go2_driver/modules/go2_tts.cpp b/src/go2_driver/modules/go2_tts.cpp index e68e5b2..eaab245 100644 --- a/src/go2_driver/modules/go2_tts.cpp +++ b/src/go2_driver/modules/go2_tts.cpp @@ -27,6 +27,8 @@ 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; } @@ -41,6 +43,8 @@ CallbackReturnT Go2TTS::on_activate() &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; } @@ -48,11 +52,17 @@ 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; } @@ -81,7 +91,6 @@ void Go2TTS::handleSay( 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); diff --git a/src/go2_driver/modules/go2_vui.cpp b/src/go2_driver/modules/go2_vui.cpp index 40943b3..4c8363a 100644 --- a/src/go2_driver/modules/go2_vui.cpp +++ b/src/go2_driver/modules/go2_vui.cpp @@ -26,6 +26,17 @@ Go2VUI::Go2VUI(const std::shared_ptr & 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(); + set_volume_service_ = node_->create_service( "set_volume", @@ -40,14 +51,7 @@ CallbackReturnT Go2VUI::on_configure() &Go2VUI::handleGetVolume, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - request_pub_ = node_->create_publisher("api/vui/request", 10); - - return CallbackReturnT::SUCCESS; -} - -CallbackReturnT Go2VUI::on_activate() -{ - request_pub_->on_activate(); + RCLCPP_INFO(node_->get_logger(), "\033[1;34mVUI module activated.\033[0m"); return CallbackReturnT::SUCCESS; } @@ -56,11 +60,20 @@ CallbackReturnT Go2VUI::on_deactivate() { request_pub_->on_deactivate(); + set_volume_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; } From 3f90d1f8cabbad9d07c121fe6fe26c1042942d06 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 12:10:48 +0100 Subject: [PATCH 16/29] Test errors solved Signed-off-by: Juancams --- include/go2_driver/modules/go2_camera.hpp | 5 ++++- .../go2_driver/modules/go2_handle_services.hpp | 8 +++++++- include/go2_driver/modules/go2_joint_states.hpp | 11 ++++++++--- include/go2_driver/modules/go2_odometry.hpp | 7 +++++-- include/go2_driver/modules/go2_tts.hpp | 15 ++++++++++----- include/go2_driver/modules/go2_vui.hpp | 5 +++++ launch/go2_driver.launch.py | 7 +++---- 7 files changed, 42 insertions(+), 16 deletions(-) diff --git a/include/go2_driver/modules/go2_camera.hpp b/include/go2_driver/modules/go2_camera.hpp index d9754ba..4266db0 100644 --- a/include/go2_driver/modules/go2_camera.hpp +++ b/include/go2_driver/modules/go2_camera.hpp @@ -19,11 +19,13 @@ #include #include #include -#include "go2_driver/utils/go2_lifecycle_node.hpp" + #include #include #include +#include "go2_driver/utils/go2_lifecycle_node.hpp" + extern "C" { #include #include @@ -31,6 +33,7 @@ extern "C" { #include } + namespace go2_driver { diff --git a/include/go2_driver/modules/go2_handle_services.hpp b/include/go2_driver/modules/go2_handle_services.hpp index d6dc1b2..a3d7f93 100644 --- a/include/go2_driver/modules/go2_handle_services.hpp +++ b/include/go2_driver/modules/go2_handle_services.hpp @@ -19,7 +19,11 @@ #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" @@ -33,10 +37,12 @@ #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 "geometry_msgs/msg/twist.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" diff --git a/include/go2_driver/modules/go2_joint_states.hpp b/include/go2_driver/modules/go2_joint_states.hpp index e7c129e..2d52328 100644 --- a/include/go2_driver/modules/go2_joint_states.hpp +++ b/include/go2_driver/modules/go2_joint_states.hpp @@ -19,12 +19,17 @@ #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 "unitree_api/msg/request.hpp" + #include "nlohmann/json.hpp" -#include "unitree_go/msg/low_state.hpp" -#include + namespace go2_driver { diff --git a/include/go2_driver/modules/go2_odometry.hpp b/include/go2_driver/modules/go2_odometry.hpp index 19a8029..143d3ad 100644 --- a/include/go2_driver/modules/go2_odometry.hpp +++ b/include/go2_driver/modules/go2_odometry.hpp @@ -19,11 +19,14 @@ #include #include #include -#include "go2_driver/utils/go2_lifecycle_node.hpp" + #include -#include "tf2_ros/transform_broadcaster.h" #include +#include "tf2_ros/transform_broadcaster.h" + +#include "go2_driver/utils/go2_lifecycle_node.hpp" + namespace go2_driver { diff --git a/include/go2_driver/modules/go2_tts.hpp b/include/go2_driver/modules/go2_tts.hpp index 649a2aa..4f9325d 100644 --- a/include/go2_driver/modules/go2_tts.hpp +++ b/include/go2_driver/modules/go2_tts.hpp @@ -19,18 +19,23 @@ #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" + #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 { diff --git a/include/go2_driver/modules/go2_vui.hpp b/include/go2_driver/modules/go2_vui.hpp index d850259..d5ca071 100644 --- a/include/go2_driver/modules/go2_vui.hpp +++ b/include/go2_driver/modules/go2_vui.hpp @@ -19,14 +19,19 @@ #include #include #include + #include "go2_driver/utils/go2_lifecycle_node.hpp" #include "go2_driver/utils/go2_api_id.hpp" + #include "go2_interfaces/srv/set_volume.hpp" #include "go2_interfaces/srv/get_volume.hpp" + #include "unitree_api/msg/request.hpp" #include "unitree_api/msg/response.hpp" + #include "nlohmann/json.hpp" + namespace go2_driver { diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 0c7b0ee..1dc9bea 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -13,10 +13,10 @@ # limitations under the License. from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.substitutions import LaunchConfiguration def generate_launch_description(): @@ -76,8 +76,7 @@ def generate_launch_description(): 'use_vui': use_vui, 'use_odometry': use_odometry, 'use_joint_states': use_joint_states, - 'use_services': use_services, - }], + 'use_services': use_services}], ) composable_nodes.append(composable_node) From 150124bf628cafd0634d499facfc74e32ab82b37 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 14:26:52 +0100 Subject: [PATCH 17/29] Added Brightness & Switch services Signed-off-by: Juancams --- include/go2_driver/modules/go2_vui.hpp | 40 ++++- src/go2_driver/modules/go2_vui.cpp | 203 +++++++++++++++++++++++-- 2 files changed, 224 insertions(+), 19 deletions(-) diff --git a/include/go2_driver/modules/go2_vui.hpp b/include/go2_driver/modules/go2_vui.hpp index d5ca071..a0ad9b7 100644 --- a/include/go2_driver/modules/go2_vui.hpp +++ b/include/go2_driver/modules/go2_vui.hpp @@ -23,8 +23,12 @@ #include "go2_driver/utils/go2_lifecycle_node.hpp" #include "go2_driver/utils/go2_api_id.hpp" -#include "go2_interfaces/srv/set_volume.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" @@ -49,19 +53,43 @@ class Go2VUI : public go2_driver::Go2LifecycleNode CallbackReturnT on_cleanup() override; private: - void handleSetVolume( + void handleGetBrightness( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response); + 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); - rclcpp::Service::SharedPtr set_volume_service_; - rclcpp::Service::SharedPtr get_volume_service_; + 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_; diff --git a/src/go2_driver/modules/go2_vui.cpp b/src/go2_driver/modules/go2_vui.cpp index 4c8363a..52d97e4 100644 --- a/src/go2_driver/modules/go2_vui.cpp +++ b/src/go2_driver/modules/go2_vui.cpp @@ -37,11 +37,18 @@ CallbackReturnT Go2VUI::on_activate() { request_pub_->on_activate(); - set_volume_service_ = - node_->create_service( - "set_volume", + get_brightness_service_ = + node_->create_service( + "get_brightness", std::bind( - &Go2VUI::handleSetVolume, this, + &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_ = @@ -51,6 +58,27 @@ CallbackReturnT Go2VUI::on_activate() &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; @@ -60,7 +88,11 @@ 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"); @@ -77,28 +109,104 @@ CallbackReturnT Go2VUI::on_cleanup() return CallbackReturnT::SUCCESS; } -void Go2VUI::handleSetVolume( +void Go2VUI::handleGetBrightness( const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { (void)request_header; + (void)request; - if (request->volume < 0 || request->volume > 10) { + nlohmann::json js; + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Vui::GetBrightness); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + auto response_sub_ = 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; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + if (response_msg->header.status.code != 0) { response->success = false; - response->message = "Volume value is out of range [0 ~ 10]"; + response->message = "Failed to get brightness"; return; } - nlohmann::json js; - js["volume"] = request->volume; + 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::SetVolume); + req.header.identity.api_id = static_cast(go2_driver::Vui::GetSwitch); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + auto response_sub_ = 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; + } + }, sub_options); request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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( @@ -146,9 +254,78 @@ void Go2VUI::handleGetVolume( } auto data = js.parse(response_msg->data); - response->volume = data["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 From ca08b098d92082172a9b5dc9f5953966a6344a9c Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 4 Mar 2025 15:10:27 +0100 Subject: [PATCH 18/29] Added Switch Obstacles Avoidance service Signed-off-by: Juancams --- CMakeLists.txt | 1 + include/go2_driver/go2_driver.hpp | 3 + .../go2_switch_obstacles_avoidance.hpp | 74 ++++++++ include/go2_driver/modules/go2_vui.hpp | 2 +- include/go2_driver/utils/go2_api_id.hpp | 10 ++ launch/go2_driver.launch.py | 11 +- src/go2_driver/go2_driver.cpp | 21 +++ .../go2_switch_obstacles_avoidance.cpp | 159 ++++++++++++++++++ 8 files changed, 279 insertions(+), 2 deletions(-) create mode 100644 include/go2_driver/modules/go2_switch_obstacles_avoidance.hpp create mode 100644 src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fcc6532..b8f146c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(${PROJECT_NAME} SHARED 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}) diff --git a/include/go2_driver/go2_driver.hpp b/include/go2_driver/go2_driver.hpp index 2afd84e..52335b8 100644 --- a/include/go2_driver/go2_driver.hpp +++ b/include/go2_driver/go2_driver.hpp @@ -23,6 +23,7 @@ #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 @@ -48,6 +49,7 @@ class Go2Driver : public rclcpp_lifecycle::LifecycleNode 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_; @@ -55,6 +57,7 @@ class Go2Driver : public rclcpp_lifecycle::LifecycleNode 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_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_vui.hpp b/include/go2_driver/modules/go2_vui.hpp index a0ad9b7..9461fea 100644 --- a/include/go2_driver/modules/go2_vui.hpp +++ b/include/go2_driver/modules/go2_vui.hpp @@ -89,7 +89,7 @@ class Go2VUI : public go2_driver::Go2LifecycleNode 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_; diff --git a/include/go2_driver/utils/go2_api_id.hpp b/include/go2_driver/utils/go2_api_id.hpp index 132ed2d..09119d1 100644 --- a/include/go2_driver/utils/go2_api_id.hpp +++ b/include/go2_driver/utils/go2_api_id.hpp @@ -79,8 +79,18 @@ enum class Audio 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 diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 1dc9bea..8cfa33d 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): 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') declare_camera_cmd = DeclareLaunchArgument( 'use_camera', @@ -64,6 +65,12 @@ def generate_launch_description(): description='Use services' ) + declare_obtacles_avoidance_cmd = DeclareLaunchArgument( + 'use_switch_obtacles_avoidance', + default_value='True', + description='Use obtacles avoidance' + ) + composable_nodes = [] composable_node = ComposableNode( @@ -76,7 +83,8 @@ def generate_launch_description(): 'use_vui': use_vui, 'use_odometry': use_odometry, 'use_joint_states': use_joint_states, - 'use_services': use_services}], + 'use_services': use_services, + 'use_switch_obtacles_avoidance': use_switch_obtacles_avoidance}], ) composable_nodes.append(composable_node) @@ -97,6 +105,7 @@ def generate_launch_description(): 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(container) return ld diff --git a/src/go2_driver/go2_driver.cpp b/src/go2_driver/go2_driver.cpp index 2c37367..eb928f4 100644 --- a/src/go2_driver/go2_driver.cpp +++ b/src/go2_driver/go2_driver.cpp @@ -28,6 +28,7 @@ Go2Driver::Go2Driver( 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_); @@ -35,6 +36,7 @@ Go2Driver::Go2Driver( 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_); } CallbackReturnT Go2Driver::on_configure(const rclcpp_lifecycle::State &) @@ -69,6 +71,12 @@ CallbackReturnT Go2Driver::on_configure(const rclcpp_lifecycle::State &) go2_services_handler_->on_configure(); } + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_ = std::make_shared( + shared_from_this()); + go2_switch_obstacles_avoidance_->on_configure(); + } + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules configured\033[0m"); return CallbackReturnT::SUCCESS; @@ -100,6 +108,10 @@ CallbackReturnT Go2Driver::on_activate(const rclcpp_lifecycle::State &) go2_services_handler_->on_activate(); } + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_activate(); + } + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules activated\033[0m"); return CallbackReturnT::SUCCESS; @@ -131,6 +143,10 @@ CallbackReturnT Go2Driver::on_deactivate(const rclcpp_lifecycle::State &) go2_services_handler_->on_deactivate(); } + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_deactivate(); + } + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules deactivated\033[0m"); return CallbackReturnT::SUCCESS; @@ -168,6 +184,11 @@ CallbackReturnT Go2Driver::on_cleanup(const rclcpp_lifecycle::State &) go2_services_handler_.reset(); } + if (use_switch_obstacles_avoidance_) { + go2_switch_obstacles_avoidance_->on_cleanup(); + go2_switch_obstacles_avoidance_.reset(); + } + RCLCPP_INFO(get_logger(), "\033[1;32mAll modules cleaned up\033[0m"); return CallbackReturnT::SUCCESS; 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..310e24c --- /dev/null +++ b/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp @@ -0,0 +1,159 @@ +// 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); + + rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + + executor.add_callback_group(callback_group, node_->get_node_base_interface()); + + unitree_api::msg::Response::SharedPtr response_msg; + + auto response_sub_ = 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; + } + }, sub_options); + + request_pub_->publish(req); + + while (response_msg == nullptr) { + executor.spin_some(); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + + 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 From abb6640f49e11b2fe2bc48fd229862b6fb847bc7 Mon Sep 17 00:00:00 2001 From: Juancams Date: Thu, 6 Mar 2025 15:51:44 +0100 Subject: [PATCH 19/29] Added autostart Signed-off-by: Juancams --- launch/go2_driver.launch.py | 74 +++++++++++++++++++++++++++++-------- 1 file changed, 58 insertions(+), 16 deletions(-) diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 8cfa33d..f3e5487 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -13,21 +13,46 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import ( + DeclareLaunchArgument, ExecuteProcess, + RegisterEventHandler, OpaqueFunction +) from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import FindExecutable -def generate_launch_description(): +def autostart(context): + + 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 + ) + + activate_event = ExecuteProcess( + cmd=[[FindExecutable( + name='ros2'), ' lifecycle', ' set', ' /go2_driver', ' activate']], + shell=True + ) + + event_handler = RegisterEventHandler( + OnProcessExit( + target_action=configure_event, + on_exit=[activate_event], + ) + ) + + return [configure_event, event_handler] + + return [] - 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') + +def generate_launch_description(): declare_camera_cmd = DeclareLaunchArgument( 'use_camera', @@ -71,6 +96,12 @@ def generate_launch_description(): description='Use obtacles avoidance' ) + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', + default_value='True', + description='Automatically start the components' + ) + composable_nodes = [] composable_node = ComposableNode( @@ -78,13 +109,15 @@ def generate_launch_description(): plugin='go2_driver::Go2Driver', name='go2_driver', namespace='', - parameters=[{'use_camera': use_camera, - 'use_tts': use_tts, - 'use_vui': use_vui, - 'use_odometry': use_odometry, - 'use_joint_states': use_joint_states, - 'use_services': use_services, - 'use_switch_obtacles_avoidance': use_switch_obtacles_avoidance}], + parameters=[{ + '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) @@ -98,6 +131,13 @@ def generate_launch_description(): output='screen', ) + 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) @@ -106,6 +146,8 @@ def generate_launch_description(): 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(event_handler) return ld From 07e34189aa6554a867340075fa605969d2ae6dbb Mon Sep 17 00:00:00 2001 From: Juancams Date: Fri, 7 Mar 2025 09:22:44 +0100 Subject: [PATCH 20/29] Flake8 Signed-off-by: Juancams --- launch/go2_driver.launch.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index f3e5487..918f1b1 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -14,14 +14,15 @@ from launch import LaunchDescription from launch.actions import ( - DeclareLaunchArgument, ExecuteProcess, - RegisterEventHandler, OpaqueFunction + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, + RegisterEventHandler, ) -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, FindExecutable +from launch.event_handlers import OnProcessExit, OnProcessStart from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import FindExecutable def autostart(context): From 983fda8ae1f4ba78268c1bc1a516bfdf22882885 Mon Sep 17 00:00:00 2001 From: Juancams Date: Fri, 7 Mar 2025 09:29:09 +0100 Subject: [PATCH 21/29] Flake8 Signed-off-by: Juancams --- launch/go2_driver.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 918f1b1..08117c7 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -19,8 +19,8 @@ OpaqueFunction, RegisterEventHandler, ) -from launch.substitutions import LaunchConfiguration, FindExecutable from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import LaunchConfiguration, FindExecutable from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode From ce75c0d6405128c30d3e8ce4f24cef073ceb29a7 Mon Sep 17 00:00:00 2001 From: Juancams Date: Fri, 7 Mar 2025 09:37:32 +0100 Subject: [PATCH 22/29] Flake8 Signed-off-by: Juancams --- launch/go2_driver.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 08117c7..14378df 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -20,7 +20,7 @@ RegisterEventHandler, ) from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import LaunchConfiguration, FindExecutable +from launch.substitutions import FindExecutable, LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode From bec77479eb46676a2ba651384951ad6f1bc3222e Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 10 Mar 2025 16:33:50 +0100 Subject: [PATCH 23/29] Camera calibration & publish camera_info Signed-off-by: Juancams --- CMakeLists.txt | 4 +- config/camera_calibration.yaml | 19 ++++++ include/go2_driver/modules/go2_camera.hpp | 21 +++++++ launch/go2_driver.launch.py | 11 +++- src/go2_driver/modules/go2_camera.cpp | 71 ++++++++++++++++++++++- 5 files changed, 121 insertions(+), 5 deletions(-) create mode 100644 config/camera_calibration.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index b8f146c..714c304 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,7 +50,7 @@ add_library(${PROJECT_NAME} SHARED ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale OpenSSL::SSL OpenSSL::Crypto) +target_link_libraries(${PROJECT_NAME} avcodec avformat avutil swscale OpenSSL::SSL OpenSSL::Crypto ${OpenCV_LIBS}) rclcpp_components_register_nodes(${PROJECT_NAME} "go2_driver::Go2Driver") @@ -67,7 +67,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/include/go2_driver/modules/go2_camera.hpp b/include/go2_driver/modules/go2_camera.hpp index 4266db0..5de59de 100644 --- a/include/go2_driver/modules/go2_camera.hpp +++ b/include/go2_driver/modules/go2_camera.hpp @@ -21,8 +21,11 @@ #include #include +#include +#include #include #include +#include #include "go2_driver/utils/go2_lifecycle_node.hpp" @@ -52,9 +55,11 @@ class Go2Camera : public go2_driver::Go2LifecycleNode 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_; @@ -64,6 +69,22 @@ class Go2Camera : public go2_driver::Go2LifecycleNode 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_; }; } // namespace go2_driver diff --git a/launch/go2_driver.launch.py b/launch/go2_driver.launch.py index 14378df..215caac 100644 --- a/launch/go2_driver.launch.py +++ b/launch/go2_driver.launch.py @@ -12,6 +12,9 @@ # 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, @@ -55,6 +58,12 @@ def autostart(context): 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', @@ -110,7 +119,7 @@ def generate_launch_description(): plugin='go2_driver::Go2Driver', name='go2_driver', namespace='', - parameters=[{ + parameters=[camera_calibration_file, { 'use_camera': LaunchConfiguration('use_camera'), 'use_tts': LaunchConfiguration('use_tts'), 'use_vui': LaunchConfiguration('use_vui'), diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp index 3e06355..08c5cdb 100644 --- a/src/go2_driver/modules/go2_camera.cpp +++ b/src/go2_driver/modules/go2_camera.cpp @@ -15,7 +15,6 @@ #include - namespace go2_driver { @@ -27,7 +26,38 @@ Go2Camera::Go2Camera(const std::shared_ptr & no 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() @@ -50,6 +80,7 @@ CallbackReturnT Go2Camera::on_configure() } 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"); @@ -59,6 +90,7 @@ CallbackReturnT Go2Camera::on_configure() CallbackReturnT Go2Camera::on_activate() { image_publisher_->on_activate(); + camera_info_publisher_->on_activate(); front_video_sub_ = node_->create_subscription( "frontvideostream", 10, @@ -72,6 +104,7 @@ CallbackReturnT Go2Camera::on_activate() CallbackReturnT Go2Camera::on_deactivate() { image_publisher_->on_deactivate(); + camera_info_publisher_->on_deactivate(); front_video_sub_.reset(); @@ -83,6 +116,7 @@ CallbackReturnT Go2Camera::on_deactivate() CallbackReturnT Go2Camera::on_cleanup() { image_publisher_.reset(); + camera_info_publisher_.reset(); avcodec_free_context(&p_codec_context_); @@ -165,13 +199,46 @@ void Go2Camera::front_video_data_callback(const unitree_go::msg::Go2FrontVideoDa cv::Mat bgr; cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGB); - auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toImageMsg(); + cv::Mat K = cv::Mat(3, 3, CV_64F, k_.data()); + cv::Mat D = cv::Mat(1, d_.size(), CV_64F, d_.data()); + cv::Mat dst; + + cv::undistort(bgr, dst, K, D); + + auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", dst).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 From 7034eef1dba028991cd9027747d2ec94fc604d70 Mon Sep 17 00:00:00 2001 From: Juancams Date: Mon, 10 Mar 2025 16:49:00 +0100 Subject: [PATCH 24/29] Uncrustify Signed-off-by: Juancams --- include/go2_driver/modules/go2_camera.hpp | 3 ++- src/go2_driver/modules/go2_camera.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/go2_driver/modules/go2_camera.hpp b/include/go2_driver/modules/go2_camera.hpp index 5de59de..5b9d823 100644 --- a/include/go2_driver/modules/go2_camera.hpp +++ b/include/go2_driver/modules/go2_camera.hpp @@ -59,7 +59,8 @@ class Go2Camera : public go2_driver::Go2LifecycleNode rclcpp::Subscription::SharedPtr front_video_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr image_publisher_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr camera_info_publisher_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + camera_info_publisher_; std::shared_ptr node_; diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp index 08c5cdb..57b7c7d 100644 --- a/src/go2_driver/modules/go2_camera.cpp +++ b/src/go2_driver/modules/go2_camera.cpp @@ -80,7 +80,8 @@ CallbackReturnT Go2Camera::on_configure() } image_publisher_ = node_->create_publisher("/image_raw", 10); - camera_info_publisher_ = node_->create_publisher("/camera_info", 10); + camera_info_publisher_ = + node_->create_publisher("/camera_info", 10); RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module configured.\033[0m"); From 79fb13cd1a39d1df69997228cf02e534f0330776 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 11 Mar 2025 16:45:03 +0100 Subject: [PATCH 25/29] Solved some bugs Signed-off-by: Juancams --- src/go2_driver/modules/go2_handle_services.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/go2_driver/modules/go2_handle_services.cpp b/src/go2_driver/modules/go2_handle_services.cpp index 3af0c86..fc95304 100644 --- a/src/go2_driver/modules/go2_handle_services.cpp +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -192,7 +192,7 @@ void Go2HandleServices::handleBodyHeight( if (request->height < -0.18 || request->height > 0.03) { response->success = false; - response->message = "Height value is out of range [0.3 ~ 0.5]"; + response->message = "Height value is out of range [-0.18 ~ 0.03]"; return; } @@ -243,7 +243,7 @@ void Go2HandleServices::handleEuler( 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]"; + response->message = "Yaw value is out of range [-0.6 ~ 0.6]"; return; } @@ -266,7 +266,7 @@ void Go2HandleServices::handleFootRaiseHeight( { (void)request_header; - if (request->height < 0 || request->height > 0.1) { + 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; @@ -281,6 +281,7 @@ void Go2HandleServices::handleFootRaiseHeight( request_pub_->publish(req); response->success = true; + response->message = "Foot raise height changed"; } void Go2HandleServices::handleMode( From dea2ddbfe328d1ec3d26e385d476bf8901c6feaa Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 11 Mar 2025 16:48:09 +0100 Subject: [PATCH 26/29] Added response message to BodyHeight service Signed-off-by: Juancams --- src/go2_driver/modules/go2_handle_services.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/go2_driver/modules/go2_handle_services.cpp b/src/go2_driver/modules/go2_handle_services.cpp index fc95304..bf0ebc7 100644 --- a/src/go2_driver/modules/go2_handle_services.cpp +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -205,6 +205,7 @@ void Go2HandleServices::handleBodyHeight( request_pub_->publish(req); response->success = true; + response->message = "Body height changed"; } void Go2HandleServices::handleContinuousGait( From 403250194770639a953b6e85c1e34d9da9ee0bd2 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 18 Mar 2025 16:07:57 +0100 Subject: [PATCH 27/29] Improve lens correction for RT Signed-off-by: Jose Miguel --- include/go2_driver/modules/go2_camera.hpp | 3 +++ src/go2_driver/modules/go2_camera.cpp | 9 +++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/go2_driver/modules/go2_camera.hpp b/include/go2_driver/modules/go2_camera.hpp index 5b9d823..beaf03a 100644 --- a/include/go2_driver/modules/go2_camera.hpp +++ b/include/go2_driver/modules/go2_camera.hpp @@ -86,6 +86,9 @@ class Go2Camera : public go2_driver::Go2LifecycleNode int roi_height_; int roi_width_; bool roi_do_rectify_; + + // Precompute lens correction interpolation + cv::Mat mapX_, mapY_; }; } // namespace go2_driver diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp index 57b7c7d..00414b5 100644 --- a/src/go2_driver/modules/go2_camera.cpp +++ b/src/go2_driver/modules/go2_camera.cpp @@ -85,6 +85,9 @@ CallbackReturnT Go2Camera::on_configure() RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module configured.\033[0m"); + // Precompute undistort map for lens correction + cv::initUndistortRectifyMap(k_, d_, cv::Matx33f::eye(), k_, cv::SIZE(width_, height_), CV_32FC1, mapX_, mapY_); + return CallbackReturnT::SUCCESS; } @@ -200,11 +203,9 @@ void Go2Camera::front_video_data_callback(const unitree_go::msg::Go2FrontVideoDa cv::Mat bgr; cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGB); - cv::Mat K = cv::Mat(3, 3, CV_64F, k_.data()); - cv::Mat D = cv::Mat(1, d_.size(), CV_64F, d_.data()); cv::Mat dst; - - cv::undistort(bgr, dst, K, D); + // 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", dst).toImageMsg(); image_msg->header.stamp = node_->get_clock()->now(); From 79f1f7ae29d4225a8536e9ae17f7e972c70e6fe0 Mon Sep 17 00:00:00 2001 From: Juancams Date: Tue, 18 Mar 2025 16:35:23 +0100 Subject: [PATCH 28/29] Solved errors Signed-off-by: Juancams --- src/go2_driver/modules/go2_camera.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/go2_driver/modules/go2_camera.cpp b/src/go2_driver/modules/go2_camera.cpp index 00414b5..392ef9a 100644 --- a/src/go2_driver/modules/go2_camera.cpp +++ b/src/go2_driver/modules/go2_camera.cpp @@ -86,7 +86,9 @@ CallbackReturnT Go2Camera::on_configure() RCLCPP_INFO(node_->get_logger(), "\033[1;34mCamera module configured.\033[0m"); // Precompute undistort map for lens correction - cv::initUndistortRectifyMap(k_, d_, cv::Matx33f::eye(), k_, cv::SIZE(width_, height_), CV_32FC1, mapX_, mapY_); + 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; } @@ -203,11 +205,11 @@ void Go2Camera::front_video_data_callback(const unitree_go::msg::Go2FrontVideoDa cv::Mat bgr; cv::cvtColor(yuv420p, bgr, cv::COLOR_YUV420p2RGB); - cv::Mat dst; + // cv::Mat dst; // Undistort image using remap to improve performance - cv::remap(bgr, dst, mapX_, mapY_, cv::INTER_LINEAR); + // cv::remap(bgr, dst, mapX_, mapY_, cv::INTER_LINEAR); - auto image_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", dst).toImageMsg(); + 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"; From c44d07ee93a74f84cfae315eb872d07ffe40f715 Mon Sep 17 00:00:00 2001 From: Juancams Date: Wed, 4 Jun 2025 15:50:00 +0200 Subject: [PATCH 29/29] Removed callback_groups Signed-off-by: Juancams --- CMakeLists.txt | 2 + .../go2_driver/utils/go2_lifecycle_node.hpp | 14 +++ .../modules/go2_handle_services.cpp | 114 ++++++++++-------- .../go2_switch_obstacles_avoidance.cpp | 29 +++-- src/go2_driver/modules/go2_vui.cpp | 87 +++++++------ 5 files changed, 143 insertions(+), 103 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 714c304..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) diff --git a/include/go2_driver/utils/go2_lifecycle_node.hpp b/include/go2_driver/utils/go2_lifecycle_node.hpp index cec78e8..257354d 100644 --- a/include/go2_driver/utils/go2_lifecycle_node.hpp +++ b/include/go2_driver/utils/go2_lifecycle_node.hpp @@ -18,6 +18,20 @@ #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 { diff --git a/src/go2_driver/modules/go2_handle_services.cpp b/src/go2_driver/modules/go2_handle_services.cpp index bf0ebc7..fd8aa9e 100644 --- a/src/go2_driver/modules/go2_handle_services.cpp +++ b/src/go2_driver/modules/go2_handle_services.cpp @@ -495,29 +495,32 @@ void Go2HandleServices::handleGetBodyHeight( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Mode::GetBodyHeight); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { @@ -546,29 +549,32 @@ void Go2HandleServices::handleGetFootRaiseHeight( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Mode::GetFootRaiseHeight); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { @@ -596,29 +602,32 @@ void Go2HandleServices::handleGetSpeedLevel( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Mode::GetSpeedLevel); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { @@ -646,15 +655,8 @@ void Go2HandleServices::handleGetState( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Mode::GetState); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; - - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; + 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, @@ -662,13 +664,23 @@ void Go2HandleServices::handleGetState( if (msg->header.identity.api_id == static_cast(go2_driver::Mode::GetState)) { response_msg = msg; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { diff --git a/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp b/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp index 310e24c..6ad8ce7 100644 --- a/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp +++ b/src/go2_driver/modules/go2_switch_obstacles_avoidance.cpp @@ -93,30 +93,33 @@ void Go2SwitchObstaclesAvoidance::handleGetObstaclesAvoidance( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::ObstaclesAvoidance::GetSwitch); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { diff --git a/src/go2_driver/modules/go2_vui.cpp b/src/go2_driver/modules/go2_vui.cpp index 52d97e4..3583805 100644 --- a/src/go2_driver/modules/go2_vui.cpp +++ b/src/go2_driver/modules/go2_vui.cpp @@ -122,29 +122,32 @@ void Go2VUI::handleGetBrightness( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Vui::GetBrightness); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { @@ -172,29 +175,32 @@ void Go2VUI::handleGetSwitch( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Vui::GetSwitch); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; - - executor.add_callback_group(callback_group, node_->get_node_base_interface()); - - unitree_api::msg::Response::SharedPtr response_msg; + 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( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) { @@ -222,29 +228,32 @@ void Go2VUI::handleGetVolume( req.parameter = js.dump(); req.header.identity.api_id = static_cast(go2_driver::Vui::GetVolume); - rclcpp::executors::SingleThreadedExecutor executor; - rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = callback_group; - - executor.add_callback_group(callback_group, node_->get_node_base_interface()); + unitree_api::msg::Response::SharedPtr response_msg = nullptr; + rclcpp::Node::SharedPtr aux_node = rclcpp::Node::make_shared("aux_node"); - unitree_api::msg::Response::SharedPtr response_msg; - - auto response_sub_ = node_->create_subscription( + 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; } - }, sub_options); + }); request_pub_->publish(req); + auto start_time = std::chrono::steady_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (response_msg == nullptr) { - executor.spin_some(); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + 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) {