From 1d017d5bc64832a36c353a92596bcb19f824ef20 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 6 Sep 2023 15:08:24 +0200 Subject: [PATCH 1/2] Return std::optional in RobotDriver::get_error() This is to keep up with a change in robot_interfaces (to be released in version 2.0). Also refactor a bit. --- CHANGELOG.md | 1 + CMakeLists.txt | 2 + include/robot_fingers/fake_finger_driver.hpp | 4 +- .../n_joint_blmc_robot_driver.hpp | 3 +- .../n_joint_blmc_robot_driver.hxx | 63 ++++++++----------- include/robot_fingers/pybullet_driver.hpp | 4 +- 6 files changed, 34 insertions(+), 43 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6a22c80..95ef036 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ broken light panels. ### Changed +- Upgrade to robot_interfaces v2 (not yet released). - plot_post_submission_log.py now plots multiple log files side by side instead of merging them. This is useful for comparing different robots. The option to merge multiple log files has been dropped but the merging can easily be diff --git a/CMakeLists.txt b/CMakeLists.txt index a20673d..f317c67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(trifinger_object_tracking REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen3 REQUIRED) +find_package(fmt REQUIRED) find_package(OpenCV REQUIRED) @@ -47,6 +48,7 @@ target_include_directories( ) target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen + fmt::fmt yaml_utils::yaml_utils blmc_drivers::blmc_drivers robot_interfaces::robot_interfaces diff --git a/include/robot_fingers/fake_finger_driver.hpp b/include/robot_fingers/fake_finger_driver.hpp index c244c0c..46dffcd 100644 --- a/include/robot_fingers/fake_finger_driver.hpp +++ b/include/robot_fingers/fake_finger_driver.hpp @@ -59,9 +59,9 @@ class FakeFingerDriver : public robot_interfaces::RobotDriver< return desired_action; } - std::string get_error() override + std::optional get_error() override { - return ""; // no errors + return std::nullopt; // no errors } void shutdown() override diff --git a/include/robot_fingers/n_joint_blmc_robot_driver.hpp b/include/robot_fingers/n_joint_blmc_robot_driver.hpp index 3f28a0c..43581c4 100644 --- a/include/robot_fingers/n_joint_blmc_robot_driver.hpp +++ b/include/robot_fingers/n_joint_blmc_robot_driver.hpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -139,7 +140,7 @@ class NJointBlmcRobotDriver virtual Observation get_latest_observation() override = 0; Action apply_action(const Action &desired_action) override; - std::string get_error() override; + std::optional get_error() override; void shutdown() override; /** diff --git a/include/robot_fingers/n_joint_blmc_robot_driver.hxx b/include/robot_fingers/n_joint_blmc_robot_driver.hxx index d69548e..29a5ce4 100644 --- a/include/robot_fingers/n_joint_blmc_robot_driver.hxx +++ b/include/robot_fingers/n_joint_blmc_robot_driver.hxx @@ -374,61 +374,43 @@ auto NJBRD::apply_action(const NJBRD::Action &desired_action) -> Action } TPL_NJBRD -std::string NJBRD::get_error() +std::optional NJBRD::get_error() { // Checks each board for errors and translates the error codes into // human-readable strings. If multiple boards have errors, the messages // are concatenated. Each message is prepended with the index of the // corresponding board. - std::string error_msg = ""; + std::optional error_msg = std::nullopt; for (size_t i = 0; i < motor_boards_.size(); i++) { auto status_timeseries = motor_boards_[i]->get_status(); if (status_timeseries->length() > 0) { - std::string board_error_msg = ""; + std::string_view board_error_msg = ""; using ErrorCodes = blmc_drivers::MotorBoardStatus::ErrorCodes; - switch (status_timeseries->newest_element().error_code) - { - case ErrorCodes::NONE: - break; - case ErrorCodes::ENCODER: - board_error_msg = "Encoder Error"; - break; - case ErrorCodes::CAN_RECV_TIMEOUT: - board_error_msg = "CAN Receive Timeout"; - break; - case ErrorCodes::CRIT_TEMP: - board_error_msg = "Critical Temperature"; - break; - case ErrorCodes::POSCONV: - board_error_msg = - "Error in SpinTAC Position Convert module"; - break; - case ErrorCodes::POS_ROLLOVER: - board_error_msg = "Position Rollover"; - break; - case ErrorCodes::OTHER: - board_error_msg = "Other Error"; - break; - default: - board_error_msg = "Unknown Error"; - break; - } - if (!board_error_msg.empty()) + if (status_timeseries->newest_element().error_code != + ErrorCodes::NONE) { - if (!error_msg.empty()) + board_error_msg = + blmc_drivers::MotorBoardStatus::get_error_description( + status_timeseries->newest_element().error_code); + + if (error_msg) + { + *error_msg += " "; + } + else { - error_msg += " "; + // initialise + error_msg = ""; } // error of the board with board index to the error message // string - error_msg += - "[Board " + std::to_string(i) + "] " + board_error_msg; + *error_msg += fmt::format("[Board {}] {}", i, board_error_msg); } } } @@ -437,12 +419,17 @@ std::string NJBRD::get_error() Vector position = this->joint_modules_.get_measured_angles(); if (!config_.is_within_hard_position_limits(position)) { - if (!error_msg.empty()) + if (error_msg) + { + *error_msg += " | "; + } + else { - error_msg += " | "; + // initialise + error_msg = ""; } - error_msg += "Position limits exceeded."; + *error_msg += "Position limits exceeded."; } return error_msg; diff --git a/include/robot_fingers/pybullet_driver.hpp b/include/robot_fingers/pybullet_driver.hpp index 0ed832e..c4d430e 100644 --- a/include/robot_fingers/pybullet_driver.hpp +++ b/include/robot_fingers/pybullet_driver.hpp @@ -112,9 +112,9 @@ class BasePyBulletFingerDriver return applied_action; } - std::string get_error() override + std::optional get_error() override { - return ""; // no errors + return std::nullopt; // no errors } void shutdown() override From a399f08367a1c6444b3bce27ba883901f71df8a4 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 19 Sep 2023 15:01:56 +0200 Subject: [PATCH 2/2] Auto-format C++ --- include/robot_fingers/n_joint_blmc_robot_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/robot_fingers/n_joint_blmc_robot_driver.hpp b/include/robot_fingers/n_joint_blmc_robot_driver.hpp index 43581c4..3dfedbc 100644 --- a/include/robot_fingers/n_joint_blmc_robot_driver.hpp +++ b/include/robot_fingers/n_joint_blmc_robot_driver.hpp @@ -12,9 +12,9 @@ #include #include +#include #include #include -#include #include #include