Adding an example for high_level visual kick without ros using fastdds

CMakeLists.txt

@@ -4,20 +4,105 @@ project(
   VERSION ${SKBUILD_PROJECT_VERSION}
   LANGUAGES CXX)
 
-cmake_minimum_required(VERSION 3.15...3.27)
+cmake_minimum_required(VERSION 3.16...3.27)
 
 option(BUILD_PYTHON_BINDING "Build Python Binding" off)
 
 
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-link_libraries(booster_robotics_sdk.a fastrtps fastcdr libfoonathan_memory-0.7.3.a)
+# Detect target architecture to select the correct bundled library directory
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+    set(ARCH_DIR "aarch64")
+else()
+    set(ARCH_DIR "x86_64")
+endif()
+
+set(SDK_LIB_DIR      "${CMAKE_SOURCE_DIR}/lib/${ARCH_DIR}")
+set(THIRD_PARTY_INC  "${CMAKE_SOURCE_DIR}/third_party/include")
+set(THIRD_PARTY_LIB  "${CMAKE_SOURCE_DIR}/third_party/lib/${ARCH_DIR}")
+
+include_directories(BEFORE
+    "${CMAKE_SOURCE_DIR}/include"
+    "${THIRD_PARTY_INC}"
+)
+
+link_directories("${SDK_LIB_DIR}" "${THIRD_PARTY_LIB}" "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu")
+
+# tinyxml2 is a transitive dependency of fastrtps; link by full path as only
+# the versioned .so is present (dev package not installed on this host).
+find_library(TINYXML2_LIB
+    NAMES tinyxml2
+    HINTS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
+    NO_DEFAULT_PATH
+)
+if(NOT TINYXML2_LIB)
+    # Fall back to the versioned shared object directly
+    file(GLOB _tinyxml2_candidates
+        "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu/libtinyxml2.so*")
+    if(_tinyxml2_candidates)
+        list(GET _tinyxml2_candidates 0 TINYXML2_LIB)
+    else()
+        message(FATAL_ERROR
+            "libtinyxml2 not found. Install libtinyxml2-dev (or equivalent) "
+            "to satisfy the transitive dependency of libfastrtps.")
+    endif()
+endif()
+message(STATUS "tinyxml2: ${TINYXML2_LIB}")
+
+link_libraries(booster_robotics_sdk.a fastrtps fastcdr libfoonathan_memory-0.7.3.a "${TINYXML2_LIB}")
 
 add_executable(b1_loco_example_client example/high_level/b1_loco_example_client.cpp)
 add_executable(b1_arm_sdk_example_client example/high_level/b1_arm_sdk_example.cpp)
 add_executable(b1_7dof_arm_sdk_example_client example/high_level/b1_7dof_arm_sdk_example.cpp)
+
+# visual_kick_no_ros – added per snippet request
+add_executable(visual_kick_no_ros example/high_level/visual_kick_no_ros.cpp)
+target_include_directories(visual_kick_no_ros PRIVATE "${CMAKE_SOURCE_DIR}/include" "${THIRD_PARTY_INC}")
+target_link_directories(visual_kick_no_ros PRIVATE "${SDK_LIB_DIR}" "${THIRD_PARTY_LIB}")
+target_link_libraries(visual_kick_no_ros booster_robotics_sdk.a fastrtps fastcdr libfoonathan_memory-0.7.3.a "${TINYXML2_LIB}")
+
+# visual_kick_ball_detect_dds_camera – reads RGB frames directly from DDS,
+# detects the ball with YOLO (optional) + colour-based HSV fallback, then kicks.
+find_package(OpenCV REQUIRED COMPONENTS core imgproc dnn highgui imgcodecs)
+add_executable(visual_kick_ball_detect_dds_camera
+    example/high_level/visual_kick_ball_detect_dds_camera.cpp)
+target_include_directories(visual_kick_ball_detect_dds_camera PRIVATE
+    "${CMAKE_SOURCE_DIR}/include"
+    "${THIRD_PARTY_INC}"
+    ${OpenCV_INCLUDE_DIRS})
+target_link_directories(visual_kick_ball_detect_dds_camera PRIVATE
+    "${SDK_LIB_DIR}"
+    "${THIRD_PARTY_LIB}")
+target_link_libraries(visual_kick_ball_detect_dds_camera
+    booster_robotics_sdk.a
+    fastrtps
+    fastcdr
+    libfoonathan_memory-0.7.3.a
+    "${TINYXML2_LIB}"
+    ${OpenCV_LIBS})
+
+# visual_pass_ball_to_person – head-sweep search, approach ball, sweep for
+# person, align body, then pass the ball toward the first person found.
+# Uses VisionClient (on-board detection, no ONNX model required).
+add_executable(visual_pass_ball_to_person
+    example/high_level/visual_pass_ball_to_person.cpp)
+target_include_directories(visual_pass_ball_to_person PRIVATE
+    "${CMAKE_SOURCE_DIR}/include"
+    "${THIRD_PARTY_INC}")
+target_link_directories(visual_pass_ball_to_person PRIVATE
+    "${SDK_LIB_DIR}"
+    "${THIRD_PARTY_LIB}")
+target_link_libraries(visual_pass_ball_to_person
+    booster_robotics_sdk.a
+    fastrtps
+    fastcdr
+    libfoonathan_memory-0.7.3.a
+    "${TINYXML2_LIB}")
+
 add_executable(b1_low_level_publisher example/low_level/low_level_publisher.cpp)
 add_executable(b1_low_level_subscriber example/low_level/low_level_subscriber.cpp)
 add_executable(low_level_hand_data_subscriber example/low_level/low_level_hand_data_subscriber.cpp)
@@ -26,10 +111,6 @@ add_executable(b1_7dof_arm_low_sdk_example example/low_level/b1_7dof_arm_low_sdk
 add_executable(odometer_example example/low_level/odometer_example.cpp)
 add_executable(battery_state_subscriber example/low_level/battery_state_subscriber.cpp)
 
-
-
-include_directories(BEFORE $(PROJECT_SOURCE_DIR)/include)
-
 if(BUILD_PYTHON_BINDING)
     find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
     message(STATUS "Python3 executable: ${Python3_EXECUTABLE}")
@@ -61,4 +142,3 @@ if(BUILD_PYTHON_BINDING)
     install(TARGETS booster_robotics_sdk_python LIBRARY DESTINATION ${PYTHON_SITE_PACKAGES})
     install(FILES ${CMAKE_SOURCE_DIR}/build/booster_robotics_sdk_python.pyi DESTINATION ${PYTHON_SITE_PACKAGES})    
 endif()
-

example/high_level/visual_kick_no_ros.cpp

@@ -0,0 +1,150 @@
+#include <chrono>
+#include <cmath>
+#include <csignal>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <atomic>
+
+#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
+#include <fastdds/dds/domain/DomainParticipant.hpp>
+#include <fastdds/dds/publisher/Publisher.hpp>
+#include <fastdds/dds/publisher/DataWriter.hpp>
+#include <fastdds/dds/topic/Topic.hpp>
+#include <fastdds/dds/topic/TypeSupport.hpp>
+
+#include <booster/robot/b1/b1_loco_client.hpp>
+#include <booster/robot/b1/b1_api_const.hpp>
+#include <booster/robot/channel/channel_factory.hpp>
+#include <booster/idl/b1/Kick.h>
+
+using namespace eprosima::fastdds::dds;
+using booster::robot::b1::B1LocoClient;
+using booster::robot::b1::VisualKickVersion;
+
+static std::atomic<bool> g_run{true};
+
+void sigint_handler(int) { g_run = false; }
+
+int main(int argc, char* argv[]) {
+    if (argc < 2) {
+        std::cout << "Usage: " << argv[0] << " networkInterface\n";
+        return 1;
+    }
+
+    std::signal(SIGINT, sigint_handler);
+    // ---- FastDDS setup ----
+    DomainParticipantQos pqos;
+    pqos.name("visual_kick_no_ros_participant");
+    DomainParticipant* participant =
+        DomainParticipantFactory::get_instance()->create_participant(0, pqos);
+    if (!participant) {
+        std::cerr << "Failed to create participant\n";
+        return 1;
+    }
+
+    TypeSupport kick_type(new brain::msg::Kick());
+    kick_type.register_type(participant);
+
+    Topic* topic = participant->create_topic(
+        booster::robot::b1::kTopicKickReference, // "rt/kick_ball"
+        kick_type.get_type_name(),
+        TOPIC_QOS_DEFAULT);
+    if (!topic) {
+        std::cerr << "Failed to create topic\n";
+        return 1;
+    }
+
+    Publisher* publisher = participant->create_publisher(PUBLISHER_QOS_DEFAULT, nullptr);
+    if (!publisher) {
+        std::cerr << "Failed to create publisher\n";
+        return 1;
+    }
+
+    DataWriter* writer = publisher->create_datawriter(topic, DATAWRITER_QOS_DEFAULT, nullptr);
+    if (!writer) {
+        std::cerr << "Failed to create writer\n";
+        return 1;
+    }
+
+    // ---- Booster loco client ----
+    booster::robot::ChannelFactory::Instance()->Init(0, "lo");
+    B1LocoClient loco;
+    loco.Init();
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    booster::robot::b1::GetModeResponse mode;
+    int mode_rc = loco.GetMode(mode);
+    std::cout << "[INFO] GetMode rc=" << mode_rc
+              << "  mode=" << static_cast<int>(mode.ToJson()["mode"]) << "\n";
+
+    // VisualKick is documented as a kSoccer-mode action - transition there
+    // first (matches the sequence that returned rc=0 in visual_kick_version8.cpp).
+    int rc_walk = loco.ChangeMode(booster::robot::RobotMode::kWalking);
+    std::cout << "[INFO] ChangeMode(kWalking) rc=" << rc_walk << "\n";
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    int rc_soccer = loco.ChangeMode(booster::robot::RobotMode::kSoccer);
+    if (rc_soccer == 0) {
+        std::cout << "[OK  ] Robot mode set to kSoccer successfully.\n";
+    } else {
+        std::cerr << "[FAIL] Could not set kSoccer mode, rc=" << rc_soccer << ". Aborting.\n";
+        return rc_soccer;
+    }
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    int ret = loco.VisualKick(true, VisualKickVersion::kV2); // enable policy
+    if (ret != 0) {
+        std::cerr << "VisualKick(true) failed: " << ret << "\n";
+        return ret;
+    }
+
+    std::cout << "VisualKick enabled. Publishing rt/kick_ball ... Ctrl+C to stop.\n";
+
+    // ---- publish loop ----
+    auto t0 = std::chrono::steady_clock::now();
+    const double goal_x = 4.5;
+    const double goal_y = 0.0;
+    double power = 0.8; // start low; calibrate gradually
+
+    while (g_run.load()) {
+        auto now = std::chrono::steady_clock::now();
+        double t = std::chrono::duration<double>(now - t0).count();
+
+        // Example ball estimate in robot coordinates:
+        double ball_x = 0.35;
+        double ball_y = 0.03 * std::sin(2.0 * M_PI * 0.4 * t);
+
+        brain::msg::Kick msg;
+        msg.x(ball_x);
+        msg.y(ball_y);
+        msg.goal_x(goal_x);
+        msg.goal_y(goal_y);
+        msg.power(power);
+
+        // Optional if needed by your controller:
+        // msg.dir(0.0);
+        // msg.robot_theta_to_field(0.0);
+
+        if (writer->write(&msg) != ReturnCode_t::RETCODE_OK) {
+            std::cerr << "Failed to publish kick message\n";
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(33)); // ~30Hz
+    }
+
+    // ---- disable policy ----
+    ret = loco.VisualKick(false, VisualKickVersion::kV2);
+    if (ret != 0) {
+        std::cerr << "VisualKick(false) failed: " << ret << "\n";
+    }
+
+    // ---- cleanup ----
+    publisher->delete_datawriter(writer);
+    participant->delete_publisher(publisher);
+    participant->delete_topic(topic);
+    DomainParticipantFactory::get_instance()->delete_participant(participant);
+
+    std::cout << "Stopped.\n";
+    return 0;
+}