Skip to content

Commit

Permalink
Cleanup comments
Browse files Browse the repository at this point in the history
Signed-off-by: Yadunund <[email protected]>
  • Loading branch information
Yadunund committed Dec 27, 2023
1 parent f50f8f3 commit 4bbfd3c
Show file tree
Hide file tree
Showing 3 changed files with 1 addition and 89 deletions.
27 changes: 0 additions & 27 deletions rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -136,18 +136,6 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
);
return;
}
// RCUTILS_LOG_INFO_NAMED(
// "rmw_zenoh_cpp",
// "[client_data_handler] triggered for %s",
// client_data->service_name
// );
// if (!z_check(*reply)) {
// RCUTILS_LOG_ERROR_NAMED(
// "rmw_zenoh_cpp",
// "z_check returned False"
// );
// return;
// }
if (!z_reply_check(reply)) {
RCUTILS_LOG_ERROR_NAMED(
"rmw_zenoh_cpp",
Expand All @@ -162,21 +150,8 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
);
return;
}

// z_sample_t sample = z_reply_ok(reply);

// z_owned_str_t keystr = z_keyexpr_to_string(sample.keyexpr);

// RCUTILS_LOG_DEBUG_NAMED(
// "rmw_zenoh_cpp",
// "[client_data_handler] keyexpr of sample: %s",
// z_loan(keystr)
// );

{
std::lock_guard<std::mutex> msg_lock(client_data->message_mutex);
// client_data->message = std::make_unique<saved_msg_data>(
// zc_sample_payload_rcinc(&sample), sample.timestamp.time, sample.timestamp.id.id);
// Take ownership of the reply.
client_data->replies.emplace_back(*reply);
*reply = z_reply_null();
Expand All @@ -187,6 +162,4 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
client_data->condition->notify_one();
}
}

// z_drop(z_move(keystr));
}
4 changes: 0 additions & 4 deletions rmw_zenoh_cpp/src/detail/rmw_data_types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,6 @@ void service_data_handler(const z_query_t * query, void * service_data);

void client_data_handler(z_owned_reply_t * reply, void * client_data);


///==============================================================================

struct rmw_service_data_t
Expand Down Expand Up @@ -172,15 +171,12 @@ struct rmw_service_data_t

struct rmw_client_data_t
{
// const char * service_name;
z_owned_keyexpr_t keyexpr;

z_owned_closure_reply_t zn_closure_reply;
// z_owned_reply_channel_t channel;

std::mutex message_mutex;
std::vector<z_owned_reply_t> replies;
// std::unique_ptr<saved_msg_data> message;

const void * request_type_support_impl;
const void * response_type_support_impl;
Expand Down
59 changes: 1 addition & 58 deletions rmw_zenoh_cpp/src/rmw_zenoh.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1974,53 +1974,11 @@ rmw_send_request(
opts.target = Z_QUERY_TARGET_ALL;
opts.value.payload = z_bytes_t{data_length, reinterpret_cast<const uint8_t *>(request_bytes)};
opts.value.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL);

// z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key(
// client->service_name, client_data->context->actual_domain_id, allocator);
// auto always_free_ros_keyexpr = rcpputils::make_scope_exit(
// [&keyexpr]() {
// z_keyexpr_drop(z_move(keyexpr));
// });
// if (!z_keyexpr_check(&keyexpr)) {
// RMW_SET_ERROR_MSG("unable to create zenoh keyexpr.");
// return RMW_RET_ERROR;
// }

// client_data->service_name = client->service_name;

client_data->zn_closure_reply = z_closure(client_data_handler, nullptr, client_data);

z_get(
z_loan(context_impl->session), z_loan(
client_data->keyexpr), "", &client_data->zn_closure_reply, &opts);

// z_owned_reply_channel_t channel = zc_reply_non_blocking_fifo_new(16);
// z_get(z_loan(context_impl->session), z_loan(client_data->keyexpr), "", z_move(channel.send),
// &opts); // here, the send is moved and will be dropped by zenoh when adequate
// z_owned_reply_t reply = z_reply_null();
// for (bool call_success = z_call(channel.recv, &reply); !call_success || z_check(reply);
// call_success = z_call(channel.recv, &reply)) {
// if (!call_success) {
// RCUTILS_LOG_WARN_NAMED(
// "rmw_zenoh_cpp", "[rmw_send_request] call unsuccessful");
// continue;
// }
// if (z_reply_is_ok(&reply)) {
// client_data->replies.push_back(std::move(reply));
// // reply = z_reply_null();
// } else {
// RCUTILS_LOG_WARN_NAMED(
// "rmw_zenoh_cpp", "[rmw_send_request] z_reply is not ok");
// return RMW_RET_ERROR;
// }
// }
// std::lock_guard<std::mutex> internal_lock(client_data->internal_mutex);
// if (client_data->condition != nullptr) {
// client_data->condition->notify_one();
// }

// z_drop(z_move(channel));
// z_reply_drop(&reply);
return RMW_RET_OK;
}

Expand Down Expand Up @@ -2061,9 +2019,6 @@ rmw_take_response(
return RMW_RET_ERROR;
}
latest_reply = &client_data->replies.back();
// msg_data = std::move(client_data->message);
// client_data->message.release();

z_sample_t sample = z_reply_ok(latest_reply);

// Object that manages the raw buffer
Expand Down Expand Up @@ -2091,7 +2046,6 @@ rmw_take_response(
z_reply_drop(&reply);
}
client_data->replies.clear();
// zc_payload_drop(&(msg_data->payload));

// TODO(francocipollone): Verify request_header information.
request_header->request_id.sequence_number = 0;
Expand Down Expand Up @@ -2433,16 +2387,10 @@ rmw_take_request(
RMW_SET_ERROR_MSG("Query id not found in id_query_map");
return RMW_RET_ERROR;
}
// const z_owned_query_t * owned_query_ptr = (*query_it).second.get();
// TODO(francocipollone): Remove the query id from the to_take collection
service_data->to_take.pop_back();
service_data->query_queue_mutex.unlock();

// DESERIALIZE MESSAGE ========================================================
// if (!z_query_check(owned_query_ptr)) {
// RMW_SET_ERROR_MSG("onwed_query_t contains gravestone, can't deserialize message");
// return RMW_RET_ERROR;
// }
const z_query_t z_loaned_query = z_query_loan(&query_it->second);
z_value_t payload_value = z_query_value(&z_loaned_query);

Expand All @@ -2465,8 +2413,6 @@ rmw_take_request(
return RMW_RET_ERROR;
}

RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "[rmw_take_request] deserialized message");

// Fill in the request header.
request_header->request_id.sequence_number = query_id;

Expand Down Expand Up @@ -2553,13 +2499,10 @@ rmw_send_response(
return RMW_RET_ERROR;
}
const z_query_t z_loaned_query = z_query_loan(&query_it->second);

z_query_reply_options_t options = z_query_reply_options_default();
options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL);
// const z_query_t loaned_query = z_loan(*owned_query_ptr);
// const z_query_t * query_ptr = &loaned_query;
z_query_reply(
&z_loaned_query, z_query_keyexpr(&z_loaned_query), reinterpret_cast<const uint8_t *>(
&z_loaned_query, z_loan(service_data->keyexpr), reinterpret_cast<const uint8_t *>(
response_bytes), data_length + meta_length, &options);

z_drop(z_move(query_it->second));
Expand Down

0 comments on commit 4bbfd3c

Please sign in to comment.