From 7410790c65547ef49f4872fdc44a9ad58e2d559d Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Sat, 13 Jan 2024 10:05:03 +0900 Subject: [PATCH] add hako master for sample program --- CMakeLists.txt | 1 + examples/hello_world/src/hello_world.c | 4 +- src/assets/src/hako_asset.cpp | 12 ++++++ src/assets/src/hako_asset_impl.cpp | 53 ++++++++++++++++++++++++++ src/assets/src/hako_asset_impl.hpp | 6 +++ src/include/hako_asset.h | 2 + 6 files changed, 77 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9baadc6..fbc060b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ set(HAKO_CMD_SOURCE_DIR "${PROJECT_SOURCE_DIR}/src/cmd") set(HAKO_LIB_SOURCE_DIR "${PROJECT_SOURCE_DIR}/src/lib") set(HAKO_PROXY_SOURCE_DIR "${PROJECT_SOURCE_DIR}/src/proxy") set(HAKO_INC_DIR "${PROJECT_SOURCE_DIR}/src/include") +set(HAKOC_SOURCE_DIR "${PROJECT_SOURCE_DIR}/src/hakoc/src") if (DEFINED HAKO_CLIENT_OPTION_FILEPATH) include(${HAKO_CLIENT_OPTION_FILEPATH}) diff --git a/examples/hello_world/src/hello_world.c b/examples/hello_world/src/hello_world.c index 0432721..82e343e 100644 --- a/examples/hello_world/src/hello_world.c +++ b/examples/hello_world/src/hello_world.c @@ -61,7 +61,7 @@ int main(int argc, const char* argv[]) else { my_callback.on_manual_timing_control = my_on_manual_timing_control; } - + hako_master_start(delta_time_usec, delta_time_usec); int ret = hako_asset_register(asset_name, config_path, &my_callback, delta_time_usec); if (ret != 0) { printf("ERORR: hako_asset_register() returns %d.", ret); @@ -69,5 +69,7 @@ int main(int argc, const char* argv[]) } ret = hako_asset_start(); printf("INFO: hako_asset_start() returns %d\n", ret); + + hako_master_stop(); return 0; } diff --git a/src/assets/src/hako_asset.cpp b/src/assets/src/hako_asset.cpp index ef93869..91a9003 100644 --- a/src/assets/src/hako_asset.cpp +++ b/src/assets/src/hako_asset.cpp @@ -127,3 +127,15 @@ int hako_asset_usleep(hako_time_t sleep_time_usec) { } return EINTR; } + +int hako_master_start(hako_time_t delta_usec, hako_time_t max_delay_usec) +{ + if (hako_master_impl_start(delta_usec, max_delay_usec)) { + return 0; + } + return EIO; +} +void hako_master_stop(void) +{ + hako_master_impl_stop(); +} \ No newline at end of file diff --git a/src/assets/src/hako_asset_impl.cpp b/src/assets/src/hako_asset_impl.cpp index 8a23af4..12c93bc 100644 --- a/src/assets/src/hako_asset_impl.cpp +++ b/src/assets/src/hako_asset_impl.cpp @@ -353,3 +353,56 @@ hako_time_t hako_asset_impl_get_world_time() { return hako_asset_instance.hako_asset->get_worldtime(); } +std::shared_ptr hako_master = nullptr; +static bool hako_master_cmd_stop = false; +static void* hako_master_impl_thread_run(void* arg) +{ + std::cout << "INFO: hako_master thread start" << std::endl; + if (arg) { + //nothing to do + } + while (hako_master_cmd_stop == false) { + try { + hako_master->execute(); + } catch (std::exception *e) { + std::cerr << "ERROR: hako_master.execute() Failed" << std::endl; + return nullptr; + } + } + hako_master = nullptr; + return nullptr; +} + +bool hako_master_impl_start(hako_time_t delta_usec, hako_time_t max_delay_usec) +{ + if (hako_master != nullptr) { + return false; + } + pthread_t thread; + try { + hako::init(); + hako_master = hako::create_master(); + if (hako_master == nullptr) { + std::cout << "INFO: hako::create_master() Failed" << std::endl; + return false; + } + hako_master->set_config_simtime(max_delay_usec, delta_usec); + } catch (std::exception *e) { + std::cout << "INFO: hako::create_master() Failed" << std::endl; + return false; + } + + if (pthread_create(&thread, NULL, hako_master_impl_thread_run, nullptr) != 0) { + std::cerr << "ERROR: Failed to create hako_master_impl_thread_run thread!" << std::endl; + return false; + } + return true; +} +void hako_master_impl_stop(void) +{ + hako_master_cmd_stop = true; + while (hako_master != nullptr) { + usleep(WAIT_TIME_USEC); + } + return; +} \ No newline at end of file diff --git a/src/assets/src/hako_asset_impl.hpp b/src/assets/src/hako_asset_impl.hpp index 22ddd15..93d5fc5 100644 --- a/src/assets/src/hako_asset_impl.hpp +++ b/src/assets/src/hako_asset_impl.hpp @@ -67,4 +67,10 @@ extern hako_time_t hako_asset_impl_get_world_time(); extern bool hako_asset_impl_pdu_read(const char* robo_name, HakoPduChannelIdType lchannel, char* buffer, size_t buffer_len); extern bool hako_asset_impl_pdu_write(const char* robo_name, HakoPduChannelIdType lchannel, const char* buffer, size_t buffer_len); +/* + * for master api for test + */ +extern bool hako_master_impl_start(hako_time_t delta_usec, hako_time_t max_delay_usec); +extern void hako_master_impl_stop(void); + #endif /* _HAKO_ASSET_IMPL_HPP_ */ diff --git a/src/include/hako_asset.h b/src/include/hako_asset.h index eb67e84..0e12fe9 100644 --- a/src/include/hako_asset.h +++ b/src/include/hako_asset.h @@ -23,6 +23,8 @@ extern int hako_asset_pdu_read(const char *robo_name, HakoPduChannelIdType lchan extern int hako_asset_pdu_write(const char *robo_name, HakoPduChannelIdType lchannel, const char *buffer, size_t buffer_len); extern hako_time_t hako_asset_simulation_time(void); extern int hako_asset_usleep(hako_time_t sleep_time_usec); +extern int hako_master_start(hako_time_t delta_usec, hako_time_t max_delay_usec); +extern void hako_master_stop(void); #ifdef __cplusplus }