From 4e3f1a2ed35ff99820ef46942b66d92b5c1b7aa2 Mon Sep 17 00:00:00 2001 From: FaaizHaikal Date: Fri, 13 Oct 2023 22:47:45 +0700 Subject: [PATCH] fix: fix array traversal --- src/akushon/config/grpc/config.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/akushon/config/grpc/config.cpp b/src/akushon/config/grpc/config.cpp index bfb8562..180b6ec 100644 --- a/src/akushon/config/grpc/config.cpp +++ b/src/akushon/config/grpc/config.cpp @@ -68,7 +68,7 @@ void ConfigGrpc::Run(uint16_t port, const std::string path, rclcpp::Node::Shared new ConfigGrpc::CallDataPublishSetJoints(&service_, cq_.get(), path, node); new ConfigGrpc::CallDataPublishSetTorques(&service_, cq_.get(), path, node); new ConfigGrpc::CallDataRunAction(&service_, cq_.get(), path, node); - new ConfigGrpc::CallDataSubscribeCurrentJoints(&service_, cq_.get(), path, node); + // new ConfigGrpc::CallDataSubscribeCurrentJoints(&service_, cq_.get(), path, node); void * tag; // uniquely identifies a request. bool ok = true; while (true) { @@ -197,11 +197,10 @@ void ConfigGrpc::CallDataPublishSetJoints::HandleRequest() set_joints_.control_type = control_type; // set_joints_.joints.push_back(joint_actions); nlohmann::json publish_set_joint = nlohmann::json::parse(request_.joints_actions()); - for (const auto & items_json_ : publish_set_joint["joints"].items()) { - nlohmann::json obj = items_json_.value(); + for (const auto & items_json_ : publish_set_joint) { tachimawari_interfaces::msg::Joint joint; - joint.id = (uint8_t)obj.at("id").get(); - joint.position = (float)obj.at("position").get(); + joint.id = items_json_.at("id").get(); + joint.position = items_json_.at("position").get(); set_joints_.joints.push_back(joint); } set_joints_publisher_->publish(set_joints_); @@ -245,7 +244,6 @@ void ConfigGrpc::CallDataRunAction::HandleRequest() run_action.json = request_.json_action(); run_action_publisher_->publish(run_action); RCLCPP_INFO(rclcpp::get_logger("PublishSetJoints"), "run action config has been published!"); - } ConfigGrpc::CallDataPublishSetTorques::CallDataPublishSetTorques( @@ -276,10 +274,10 @@ void ConfigGrpc::CallDataPublishSetTorques::HandleRequest() nlohmann::json publish_set_torque = nlohmann::json::parse(request_.ids()); - for (const auto & items : publish_set_torque["id"].items()) { - set_torque.ids.push_back((uint8_t)items.value().get()); + for (const auto & items : publish_set_torque) { + set_torque.ids.push_back(items.get()); } - set_torque_publisher_->publish(set_torque); + set_torque_publisher_->publish(set_torque); RCLCPP_INFO(rclcpp::get_logger("PublishSetTorques"), "set torques has been published!"); } catch (std::ofstream::failure f) { @@ -294,13 +292,14 @@ ConfigGrpc::CallDataSubscribeCurrentJoints::CallDataSubscribeCurrentJoints( akushon_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq, const std::string path, rclcpp::Node::SharedPtr node) : CallData(service, cq, path), node_(node) -{ +{ current_joint_subscription_ = node_->create_subscription( "/joint/current_joints", 10, [this](const tachimawari_interfaces::msg::CurrentJoints::SharedPtr curr_joints) { - // RCLCPP_INFO(rclcpp::get_logger("SubscribeCurrentJoints"), "current joints received from tachimawari"); - curr_joints_.joints = curr_joints->joints; + RCLCPP_INFO( + rclcpp::get_logger("SubscribeCurrentJoints"), "current joints received from tachimawari"); + curr_joints_.joints = curr_joints->joints; }); Proceed(); } // namespace akushon @@ -318,11 +317,10 @@ void ConfigGrpc::CallDataSubscribeCurrentJoints::WaitForRequest() void ConfigGrpc::CallDataSubscribeCurrentJoints::HandleRequest() { try { - nlohmann::json curr_joints; // curr_joints["control_type"] = curr_joints_->joints; - for (const auto & items : curr_joints_.joints) { + for (const auto & items : curr_joints_.joints) { nlohmann::json obj; obj["id"] = std::to_string(items.id); obj["position"] = std::to_string(items.position);