From 7020330af8e80d408cf7364d06f5289a61fc3283 Mon Sep 17 00:00:00 2001 From: Oleksii Kurochko Date: Sat, 21 Dec 2024 16:25:03 +0000 Subject: [PATCH] WIP Signed-off-by: Oleksii Kurochko --- vehicle_gateway_demo/src/follower.cpp | 24 +++++++---- .../src/vehicle_gateway_multi_bridge.cpp | 37 +++++++++++------ .../vehicle_gateway_multi_bridge_client.cpp | 41 ++++++++++++++----- .../src/vehicle_gateway_px4.cpp | 38 ++++++++++------- 4 files changed, 94 insertions(+), 46 deletions(-) diff --git a/vehicle_gateway_demo/src/follower.cpp b/vehicle_gateway_demo/src/follower.cpp index 9b3c8fd..cc78a05 100644 --- a/vehicle_gateway_demo/src/follower.cpp +++ b/vehicle_gateway_demo/src/follower.cpp @@ -55,21 +55,24 @@ public: } std::shared_ptr gateway_; - void state_handler(const z_sample_t * sample); + void state_handler(z_loaned_sample_t * sample); private: std::shared_ptr> loader_; bool following_active{false}; }; -void Follower::state_handler(const z_sample_t * sample) +void Follower::state_handler(z_loaned_sample_t * sample) { + z_owned_string_t payload_string; + z_bytes_to_string(z_sample_payload(sample), &payload_string); + json j; try { j = json::parse( std::string( - reinterpret_cast(sample->payload.start), - sample->payload.len)); + z_string_data(z_loan(payload_string)), + (int)z_string_len(z_loan(payload_string)))); } catch (...) { printf("error parsing json\n"); return; @@ -122,7 +125,7 @@ void Follower::state_handler(const z_sample_t * sample) } } -void state_handler(const z_sample_t * sample, void * context) +void state_handler(z_loaned_sample_t * sample, void * context) { Follower * follower = reinterpret_cast(context); follower->state_handler(sample); @@ -149,13 +152,16 @@ int main(int argc, const char * argv[]) // desired vehicle_id is living in. If not, this will silently be sad. // todo: detect this situation and print a helpful warning message. - z_owned_closure_sample_t callback = z_closure(state_handler, NULL, vg.get()); + z_owned_closure_sample_t callback; + z_closure(&callback, state_handler, NULL, vg.get()); z_owned_session_t * zenoh_session = reinterpret_cast( vg->gateway_->get_multirobot_session()); - z_owned_subscriber_t state_sub = z_declare_subscriber( - z_loan(*zenoh_session), - z_keyexpr("vehicle_gateway/*/state"), z_move(callback), NULL); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, "vehicle_gateway/*/state"); + + z_owned_subscriber_t state_sub; + z_declare_subscriber( z_loan(*zenoh_session), &state_sub, z_loan(ke), z_move(callback), NULL); rclcpp::WallRate loop_rate(std::chrono::milliseconds(500)); while (rclcpp::ok()) { diff --git a/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge.cpp b/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge.cpp index afc8ac1..dcf24f0 100644 --- a/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge.cpp +++ b/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge.cpp @@ -78,9 +78,11 @@ public: }); std::cout << "sending state message...\n"; - this->pub_ = z_declare_publisher( - z_loan(*this->session_), z_keyexpr(this->zenoh_key_name_.c_str()), NULL); - if (!z_check(this->pub_)) { + + z_view_keyexpr_t ping; + z_view_keyexpr_from_str_unchecked(&ping, (vehicle_id_prefix + "/fmu/out/vehicle_gps_position").c_str()); + + if ( z_declare_publisher(z_loan(*this->session_), &this->pub_, z_loan(ping), NULL) ) { std::cout << "Unable to declare Publisher for key expression!\n"; return -1; } @@ -104,12 +106,21 @@ public: this->newdata_ = false; string s = j.dump(); - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_TEXT_JSON, NULL); + + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + + z_owned_encoding_t encoding; + z_encoding_clone(&encoding, z_encoding_text_json()); + options.encoding = z_move(encoding); + + z_owned_bytes_t payload; + z_bytes_copy_from_str(&payload, s.c_str()); + + z_publisher_put( z_loan(this->pub_), - reinterpret_cast(s.c_str()), - s.size() + 1, + z_move(payload), &options); } @@ -136,14 +147,16 @@ int main(int argc, char ** argv) return EXIT_FAILURE; } const char * zenoh_config_file = argv[1]; - z_owned_config_t config = zc_config_from_file(zenoh_config_file); - if (!z_check(config)) { + z_owned_config_t config; + + if (zc_config_from_file(&config, zenoh_config_file)) { std::cout << "unable to parse zenoh config from [" << zenoh_config_file << "]\n"; return EXIT_FAILURE; } std::cout << "opening zenoh session...\n"; - z_owned_session_t session = z_open(z_move(config)); - if (!z_check(session)) { + + z_owned_session_t session; + if ( z_open(&session, z_move(config), NULL) ) { std::cout << "unable to open zenoh session\n"; return EXIT_FAILURE; } @@ -180,7 +193,7 @@ int main(int argc, char ** argv) rclcpp::shutdown(); std::cout << "closing zenoh session...\n"; - z_close(z_move(session)); + z_drop(z_move(session)); return EXIT_SUCCESS; } diff --git a/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge_client.cpp b/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge_client.cpp index 956eaec..c84f6aa 100644 --- a/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge_client.cpp +++ b/vehicle_gateway_multi/src/vehicle_gateway_multi_bridge_client.cpp @@ -18,13 +18,27 @@ const char * kind_to_str(z_sample_kind_t kind); -void data_handler(const z_sample_t * sample, void * /*arg*/) +void data_handler(z_loaned_sample_t * sample, void * /*arg*/) { + z_view_string_t key_string; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &key_string); + + z_owned_string_t payload_string; + z_bytes_to_string(z_sample_payload(sample), &payload_string); + + printf(">> [Subscriber] Received %s ('%.*s': '%.*s')\n", kind_to_str(z_sample_kind(sample)), + (int)z_string_len(z_loan(key_string)), z_string_data(z_loan(key_string)), + (int)z_string_len(z_loan(payload_string)), z_string_data(z_loan(payload_string))); + + z_drop(z_move(payload_string)); + +/* z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); printf( ">> [Subscriber] Received %s ('%s': '%.*s')\n", kind_to_str(sample->kind), z_loan(keystr), static_cast(sample->payload.len), sample->payload.start); z_drop(z_move(keystr)); +*/ } int main(int argc, char ** argv) @@ -34,9 +48,11 @@ int main(int argc, char ** argv) expr = argv[1]; } - z_owned_config_t config = z_config_default(); + z_owned_config_t config; + z_config_default(&config); + if (argc > 2) { - if (zc_config_insert_json(z_loan(config), Z_CONFIG_LISTEN_KEY, argv[2]) < 0) { + if (zc_config_insert_json5(z_loan_mut(config), Z_CONFIG_LISTEN_KEY, argv[2]) < 0) { printf( "Couldn't insert value `%s` in configuration at `%s`. " "This is likely because `%s` expects a " @@ -48,17 +64,22 @@ int main(int argc, char ** argv) } printf("Opening session...\n"); - z_owned_session_t s = z_open(z_move(config)); - if (!z_check(s)) { + z_owned_session_t s; + if (z_open(&s, z_move(config), NULL)) { printf("Unable to open session!\n"); exit(-1); } - z_owned_closure_sample_t callback = z_closure(data_handler); + z_owned_closure_sample_t callback; + z_closure(&callback, data_handler, NULL, NULL); + printf("Declaring Subscriber on '%s'...\n", expr); - z_owned_subscriber_t sub = - z_declare_subscriber(z_loan(s), z_keyexpr(expr), z_move(callback), NULL); - if (!z_check(sub)) { + + z_owned_subscriber_t sub; + + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, expr); + if (z_declare_subscriber(z_loan(s), &sub, z_loan(ke), z_move(callback), NULL)){ printf("Unable to declare subscriber.\n"); exit(-1); } @@ -73,7 +94,7 @@ int main(int argc, char ** argv) } z_undeclare_subscriber(z_move(sub)); - z_close(z_move(s)); + z_drop(z_move(s)); return 0; } diff --git a/vehicle_gateway_px4/src/vehicle_gateway_px4.cpp b/vehicle_gateway_px4/src/vehicle_gateway_px4.cpp index c890008..a283d1a 100644 --- a/vehicle_gateway_px4/src/vehicle_gateway_px4.cpp +++ b/vehicle_gateway_px4/src/vehicle_gateway_px4.cpp @@ -56,8 +56,8 @@ double calc_distance_latlon(double lat_1, double lon_1, double lat_2, double lon VehicleGatewayPX4::VehicleGatewayPX4() : VehicleGateway() { - zenoh_session_ = z_session_null(); - zenoh_state_pub_ = z_publisher_null(); + z_internal_null(&zenoh_session_); + z_internal_null(&zenoh_state_pub_); } void VehicleGatewayPX4::set_vehicle_id(unsigned int _vehicle_id) @@ -891,8 +891,8 @@ bool VehicleGatewayPX4::create_multirobot_session(const char * config_filename) return false; } - z_owned_config_t config = zc_config_from_str(zenoh_config.c_str()); - if (!z_check(config)) { + z_owned_config_t config; + if ( zc_config_from_str(&config, zenoh_config.c_str()) ) { RCLCPP_FATAL( this->px4_node_->get_logger(), "Unable to create zenoh config from %s", @@ -901,8 +901,7 @@ bool VehicleGatewayPX4::create_multirobot_session(const char * config_filename) RCLCPP_INFO( this->px4_node_->get_logger(), "Created zenoh config successfully"); - zenoh_session_ = z_open(z_move(config)); - if (!z_check(zenoh_session_)) { + if ( z_open(&zenoh_session_, z_move(config), NULL) ) { RCLCPP_FATAL( this->px4_node_->get_logger(), "Unable to create zenoh session"); @@ -920,9 +919,11 @@ bool VehicleGatewayPX4::create_multirobot_session(const char * config_filename) RCLCPP_INFO( this->px4_node_->get_logger(), "Declaring state publisher on Zenoh keyexpr %s", state_key.c_str()); - zenoh_state_pub_ = z_declare_publisher( - z_loan(zenoh_session_), z_keyexpr(state_key.c_str()), NULL); - if (!z_check(zenoh_state_pub_)) { + + z_view_keyexpr_t statekey; + z_view_keyexpr_from_str_unchecked(&statekey, state_key.c_str()); + + if ( z_declare_publisher(z_loan(zenoh_session_), &zenoh_state_pub_, z_loan(statekey), NULL) ) { RCLCPP_FATAL( this->px4_node_->get_logger(), "Unable to create zenoh state publisher"); @@ -956,24 +957,31 @@ void VehicleGatewayPX4::zenoh_transmit() // plus out local clock elapsed time since GPS time was last updated? string s = j.dump(); - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_JSON, NULL); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + + z_owned_encoding_t encoding; + z_encoding_clone(&encoding, z_encoding_application_json()); + options.encoding = z_move(encoding); + + z_owned_bytes_t payload; + z_bytes_copy_from_str(&payload, s.c_str()); + z_publisher_put( z_loan(this->zenoh_state_pub_), - reinterpret_cast(s.c_str()), - s.size() + 1, + z_move(payload), &options); } bool VehicleGatewayPX4::destroy_multirobot_session() { - if (!z_check(zenoh_session_)) { + if (!z_internal_check(zenoh_session_)) { RCLCPP_WARN( this->px4_node_->get_logger(), "Zenoh session is not open; can't destroy it!"); return false; } - z_close(z_move(zenoh_session_)); + z_drop(z_move(zenoh_session_)); return true; } -- 2.34.1