Add powerboard measurements to Solo12 sensor data

include/solo/solo12.hpp

@@ -297,6 +297,39 @@ 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
+    {
+        // FIXME better get in acquire_sensors() so its in sync with other
+        // sensor data
+        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
      */

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.

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)
         {