Skip to content

Commit

Permalink
Merge pull request #165 from open-dynamic-robot-initiative/fkloss/get…
Browse files Browse the repository at this point in the history
…_error

Return std::optional in RobotDriver::get_error()
  • Loading branch information
luator authored Sep 19, 2023
2 parents 772fe4d + a399f08 commit 25d0aaa
Show file tree
Hide file tree
Showing 6 changed files with 34 additions and 43 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)


Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions include/robot_fingers/fake_finger_driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,9 @@ class FakeFingerDriver : public robot_interfaces::RobotDriver<
return desired_action;
}

std::string get_error() override
std::optional<std::string> get_error() override
{
return ""; // no errors
return std::nullopt; // no errors
}

void shutdown() override
Expand Down
3 changes: 2 additions & 1 deletion include/robot_fingers/n_joint_blmc_robot_driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <iterator>
#include <string>

#include <fmt/format.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>

Expand Down Expand Up @@ -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<std::string> get_error() override;
void shutdown() override;

/**
Expand Down
63 changes: 25 additions & 38 deletions include/robot_fingers/n_joint_blmc_robot_driver.hxx
Original file line number Diff line number Diff line change
Expand Up @@ -374,61 +374,43 @@ auto NJBRD::apply_action(const NJBRD::Action &desired_action) -> Action
}

TPL_NJBRD
std::string NJBRD::get_error()
std::optional<std::string> 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<std::string> 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);
}
}
}
Expand All @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions include/robot_fingers/pybullet_driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -112,9 +112,9 @@ class BasePyBulletFingerDriver
return applied_action;
}

std::string get_error() override
std::optional<std::string> get_error() override
{
return ""; // no errors
return std::nullopt; // no errors
}

void shutdown() override
Expand Down

0 comments on commit 25d0aaa

Please sign in to comment.