| 60 | int dxl_comm_result = COMM_TX_FAIL; |
| 61 | |
| 62 | ReadWriteNode::ReadWriteNode() |
| 63 | : Node("read_write_node") |
| 64 | { |
| 65 | RCLCPP_INFO(this->get_logger(), "Run read write node"); |
| 66 | |
| 67 | this->declare_parameter("qos_depth", 10); |
| 68 | int8_t qos_depth = 0; |
| 69 | this->get_parameter("qos_depth", qos_depth); |
| 70 | |
| 71 | const auto QOS_RKL10V = |
| 72 | rclcpp::QoS(rclcpp::KeepLast(qos_depth)).reliable().durability_volatile(); |
| 73 | |
| 74 | set_position_subscriber_ = |
| 75 | this->create_subscription<SetPosition>( |
| 76 | "set_position", |
| 77 | QOS_RKL10V, |
| 78 | [this](const SetPosition::SharedPtr msg) -> void |
| 79 | { |
| 80 | uint8_t dxl_error = 0; |
| 81 | |
| 82 | // Position Value of X series is 4 byte data. |
| 83 | // For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. |
| 84 | uint32_t goal_position = (unsigned int)msg->position; // Convert int32 -> uint32 |
| 85 | |
| 86 | // Write Goal Position (length : 4 bytes) |
| 87 | // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead. |
| 88 | dxl_comm_result = |
| 89 | packetHandler->write4ByteTxRx( |
| 90 | portHandler, |
| 91 | (uint8_t) msg->id, |
| 92 | ADDR_GOAL_POSITION, |
| 93 | goal_position, |
| 94 | &dxl_error |
| 95 | ); |
| 96 | |
| 97 | if (dxl_comm_result != COMM_SUCCESS) { |
| 98 | RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getTxRxResult(dxl_comm_result)); |
| 99 | } else if (dxl_error != 0) { |
| 100 | RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getRxPacketError(dxl_error)); |
| 101 | } else { |
| 102 | RCLCPP_INFO(this->get_logger(), "Set [ID: %d] [Goal Position: %d]", msg->id, msg->position); |
| 103 | } |
| 104 | } |
| 105 | ); |
| 106 | |
| 107 | auto get_present_position = |
| 108 | [this]( |
| 109 | const std::shared_ptr<GetPosition::Request> request, |
| 110 | std::shared_ptr<GetPosition::Response> response) -> void |
| 111 | { |
| 112 | // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 |
| 113 | // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. |
| 114 | dxl_comm_result = packetHandler->read4ByteTxRx( |
| 115 | portHandler, |
| 116 | (uint8_t) request->id, |
| 117 | ADDR_PRESENT_POSITION, |
| 118 | reinterpret_cast<uint32_t *>(&present_position), |
| 119 | &dxl_error |
nothing calls this directly
no test coverage detected