From 140609e11e5233e2917d3ea68d2d02e9c3be17d6 Mon Sep 17 00:00:00 2001
From: Felix Widmaier <felix.widmaier@tuebingen.mpg.de>
Date: Fri, 22 Apr 2022 15:31:07 +0200
Subject: [PATCH 1/3] Add powerboard measurements to Solo12 sensor data

---
 include/solo/solo12.hpp                  | 31 ++++++++++++++++++++++++
 src/dynamic_graph_manager/dgm_solo12.cpp |  3 +++
 2 files changed, 34 insertions(+)

diff --git a/include/solo/solo12.hpp b/include/solo/solo12.hpp
index 59a67cf..a204bbf 100644
--- a/include/solo/solo12.hpp
+++ b/include/solo/solo12.hpp
@@ -297,6 +297,37 @@ class Solo12
         return imu_linear_acceleration_;
     }
 
+    /**
+     * @brief Get current [A] measurement from the power board.
+     *
+     * Will be zero if no power board is used.
+     */
+    float get_powerboard_current() const
+    {
+        return main_board_ptr_->powerboard_current();
+    }
+
+    /**
+     * @brief Get voltage [V] measurement from the power board.
+     *
+     * Will be zero if no power board is used.
+     */
+    float get_powerboard_voltage() const
+    {
+        return main_board_ptr_->powerboard_voltage();
+    }
+
+    /**
+     * @brief Get energy [J] consumed since start as measured by the power
+     * board.
+     *
+     * Will be zero if no power board is used.
+     */
+    float get_powerboard_energy() const
+    {
+        return main_board_ptr_->powerboard_energy();
+    }
+
     /*
      * Hardware Status
      */
diff --git a/src/dynamic_graph_manager/dgm_solo12.cpp b/src/dynamic_graph_manager/dgm_solo12.cpp
index 26a5f6f..174da9d 100644
--- a/src/dynamic_graph_manager/dgm_solo12.cpp
+++ b/src/dynamic_graph_manager/dgm_solo12.cpp
@@ -121,6 +121,9 @@ void DGMSolo12::get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map)
     map.at("imu_gyroscope") = solo_.get_imu_gyroscope();
     map.at("imu_attitude") = solo_.get_imu_attitude();
     map.at("imu_linear_acceleration") = solo_.get_imu_linear_acceleration();
+    map.at("powerboard_current")[0] = solo_.get_powerboard_current();
+    map.at("powerboard_voltage")[0] = solo_.get_powerboard_voltage();
+    map.at("powerboard_energy")[0] = solo_.get_powerboard_energy();
 
     /**
      * Robot status.

From 70c3b4df61bf3c8f32e4251a6f34958544c5aef5 Mon Sep 17 00:00:00 2001
From: Felix Widmaier <felix.widmaier@tuebingen.mpg.de>
Date: Mon, 23 May 2022 13:45:10 +0200
Subject: [PATCH 2/3] Add PowerBoard to Robot class

This enables power-board-related safety checks.
---
 src/solo12.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/solo12.cpp b/src/solo12.cpp
index 6c3acbb..44f18f6 100644
--- a/src/solo12.cpp
+++ b/src/solo12.cpp
@@ -1,8 +1,8 @@
 #include "solo/solo12.hpp"
 #include <cmath>
 #include <odri_control_interface/common.hpp>
-#include "solo/common_programs_header.hpp"
 #include "real_time_tools/spinner.hpp"
+#include "solo/common_programs_header.hpp"
 
 namespace solo
 {
@@ -79,8 +79,7 @@ void Solo12::initialize(const std::string& network_id,
     network_id_ = network_id;
 
     // Use a serial port to read slider values.
-    serial_reader_ =
-        std::make_shared<slider_box::SerialReader>(serial_port, 5);
+    serial_reader_ = std::make_shared<slider_box::SerialReader>(serial_port, 5);
 
     main_board_ptr_ = std::make_shared<MasterBoardInterface>(network_id_);
 
@@ -141,9 +140,12 @@ void Solo12::initialize(const std::string& network_id,
     calib_ctrl_ = std::make_shared<odri_control_interface::JointCalibrator>(
         joints_, directions, position_offsets, 5., 0.05, 1.0, 0.001);
 
+    auto powerboard =
+        std::make_shared<odri_control_interface::PowerBoard>(main_board_ptr_);
+
     // Define the robot.
     robot_ = std::make_shared<odri_control_interface::Robot>(
-        main_board_ptr_, joints_, imu_, calib_ctrl_);
+        main_board_ptr_, joints_, imu_, calib_ctrl_, powerboard);
 
     // Initialize the robot.
     robot_->Init();
@@ -279,7 +281,7 @@ void Solo12::send_target_joint_position(
 }
 
 void Solo12::send_target_joint_velocity(
-        const Eigen::Ref<Vector12d> target_joint_velocity)
+    const Eigen::Ref<Vector12d> target_joint_velocity)
 {
     robot_->joints->SetDesiredVelocities(target_joint_velocity);
 }
@@ -301,7 +303,7 @@ void Solo12::wait_until_ready()
     real_time_tools::Spinner spinner;
     spinner.set_period(0.001);
     static long int count_wait_until_ready = 0;
-    while(state_ != Solo12State::ready)
+    while (state_ != Solo12State::ready)
     {
         if (count_wait_until_ready % 200 == 0)
         {

From f9070c3742f7ee496a52e733060236cf55653483 Mon Sep 17 00:00:00 2001
From: Felix Widmaier <felix.widmaier@tuebingen.mpg.de>
Date: Tue, 25 Oct 2022 14:02:05 +0200
Subject: [PATCH 3/3] Add FIXME

---
 include/solo/solo12.hpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/include/solo/solo12.hpp b/include/solo/solo12.hpp
index a204bbf..3062d01 100644
--- a/include/solo/solo12.hpp
+++ b/include/solo/solo12.hpp
@@ -304,6 +304,8 @@ class Solo12
      */
     float get_powerboard_current() const
     {
+        // FIXME better get in acquire_sensors() so its in sync with other
+        // sensor data
         return main_board_ptr_->powerboard_current();
     }