| 42 | } |
| 43 | |
| 44 | void Reset::command(const void * request, void * response) |
| 45 | { |
| 46 | (void) request; |
| 47 | |
| 48 | std_srvs::srv::Trigger::Response * res = (std_srvs::srv::Trigger::Response *)response; |
| 49 | std::string result_msg; |
| 50 | |
| 51 | uint8_t reset = 1; |
| 52 | dxl_sdk_wrapper_->set_data_to_device( |
| 53 | extern_control_table.imu_re_calibration.addr, |
| 54 | extern_control_table.imu_re_calibration.length, |
| 55 | &reset, |
| 56 | &result_msg); |
| 57 | |
| 58 | RCLCPP_INFO(nh_->get_logger(), "Start Calibration of Gyro"); |
| 59 | rclcpp::sleep_for(std::chrono::seconds(5)); |
| 60 | RCLCPP_INFO(nh_->get_logger(), "Calibration End"); |
| 61 | res->success = true; |
| 62 | res->message = "Calibration End, Odom reset requested"; |
| 63 | |
| 64 | if (!reset_odom_client_->wait_for_service(std::chrono::seconds(1))) { |
| 65 | RCLCPP_WARN(nh_->get_logger(), "reset_odometry service not available"); |
| 66 | return; |
| 67 | } |
| 68 | |
| 69 | auto request_reset_odom = std::make_shared<std_srvs::srv::Trigger::Request>(); |
| 70 | reset_odom_client_->async_send_request( |
| 71 | request_reset_odom, |
| 72 | [this](rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future) |
| 73 | { |
| 74 | auto response_reset_odom = future.get(); |
| 75 | RCLCPP_INFO( |
| 76 | nh_->get_logger(), |
| 77 | "odom reset response: %s", |
| 78 | response_reset_odom->success ? "success" : "failed"); |
| 79 | }); |
| 80 | } |
| 81 | |
| 82 | void Reset::request( |
| 83 | rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client, |
no test coverage detected