From 0e65361d9ddf4009088be828231e5871778fa002 Mon Sep 17 00:00:00 2001
From: rnzenha-s <107403736+rnzenha-s@users.noreply.github.com>
Date: Fri, 19 Apr 2024 16:00:25 +0100
Subject: [PATCH] ROS drivers to interface with new Palm firmware 250 (#265)

* new 250 Palm firmware ethercat drivers. WIP

* Revert some changes to make firm 250 compatible with 7-taxel sensor

* 250 firm is now able to request 17-taxel mst for its information

* Successfuly updated MST's structure to 12 taxels

* Successfuly retrieving information from all 17-taxels (for dummy data - integeres 1-77). Not yet considering how speed of each hand cycle will impact sensor readings

* Updating 250 firmware Ethercat Status data order to comply with changes made in hand-firmware

* Adding docstring documentation to SR10 drivers related files

* Update licensese/documentation on sr10, 240 and 250 ethercat protocol files

* Adding Adding MST sensor to the list of tactile sensor controllers

* Changing new sensor related messages.

* Fixing some SR10 documentation. Starting MST documentation. Fixing sr_robot_lib files licences

* Improve Ethercat protocol related files documentation

* Document MST and MST publisher files. Fix identation and licences in some other files

* Handle new status check on MST sensors

* Add changes to handle new sensor status check information

* Changes to mst tactile controller/publisher so that each sensor is published individually

* Add a check to detect if palm is trying to poll data from the new tactile sensors too soon

* Change generic tactiles stream from INFO to DEBUG

* Properly evaluate the contents of status check byte sent by the sensor (last 2 char from git_version hex string)

* Fix issues with status check flag detection for multiple sensors on the same hand

* Add a 'bad frame counter' to sr10

* Copy files from hand-firmware (except license header)

* Replacing boost shared_ptr by std share_ptr (supported since C11) and removing other unnecessary boost related imports

* Include 'memory' library

* Include 'memory' library

* Remove 'bad_frame_counter_changes'

* Cast IMU polled values to signed int16

* Add Bad frame counter

* Addressing reviews. Updating Licenses, and prunning SR10 code (removing frequent dynamic memory allocations, unnecessary data publishing, unnecessary data castings, etc)

* Change dynamic allocation of debug data realtime publisher msg structure (reserve to resize)

* Delete Frame id from tactiles published messages.

* Update git revision to exclude status check-enable byte

* Remove unnecessary IMU updates

* Update IMU zero catcher
---
 sr_edc_ethercat_drivers/CMakeLists.txt        |   2 +-
 .../ethercat_device_plugin.xml                |   7 +-
 .../include/sr_edc_ethercat_drivers/sr10.h    | 208 +++++++
 sr_edc_ethercat_drivers/src/sr10.cpp          | 566 ++++++++++++++++++
 .../0240_palm_edc_IMU_ethercat_protocol.h     | 110 ++--
 .../0250_palm_edc_IMU_ethercat_protocol.h     | 145 +++++
 .../0250_palm_edc_IMU_MST/CPPLINT.cfg         |   2 +
 .../common/common_edc_ethercat_protocol.h     | 111 ++--
 .../common/ethercat_can_bridge_protocol.h     |  65 +-
 .../common/tactile_edc_ethercat_protocol.h    |  91 ++-
 sr_robot_lib/include/sr_robot_lib/MST.hpp     |  98 ++-
 sr_robot_lib/include/sr_robot_lib/UBI0.hpp    |  49 +-
 .../include/sr_robot_lib/generic_tactiles.hpp |  20 +-
 .../include/sr_robot_lib/generic_updater.hpp  |  22 +-
 .../sr_robot_lib/motor_data_checker.hpp       |  18 +-
 .../include/sr_robot_lib/motor_updater.hpp    |  21 +-
 .../include/sr_robot_lib/sensor_updater.hpp   |  18 +-
 .../include/sr_robot_lib/sr_robot_lib.hpp     |  18 +-
 sr_robot_lib/src/MST.cpp                      | 322 +++++++---
 sr_robot_lib/src/UBI0.cpp                     |  22 +-
 sr_robot_lib/src/biotac.cpp                   |  22 +-
 sr_robot_lib/src/generic_tactiles.cpp         |  34 +-
 sr_robot_lib/src/generic_updater.cpp          |  24 +-
 sr_robot_lib/src/motor_updater.cpp            |  23 +-
 sr_robot_lib/src/sensor_updater.cpp           |  20 +-
 sr_robot_lib/src/shadow_PSTs.cpp              |  22 +-
 sr_robot_lib/src/sr_motor_hand_lib.cpp        |  23 +-
 sr_robot_lib/src/sr_motor_robot_lib.cpp       |  21 +-
 sr_robot_lib/src/sr_robot_lib.cpp             |  20 +-
 sr_tactile_sensor_controller/CMakeLists.txt   |   1 +
 .../sr_mst_tactile_sensor_publisher.hpp       |  80 +++
 .../src/sr_mst_tactile_sensor_publisher.cpp   |  85 +++
 .../src/sr_tactile_sensor_controller.cpp      |   5 +
 33 files changed, 1748 insertions(+), 547 deletions(-)
 create mode 100644 sr_edc_ethercat_drivers/include/sr_edc_ethercat_drivers/sr10.h
 create mode 100644 sr_edc_ethercat_drivers/src/sr10.cpp
 create mode 100644 sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h
 create mode 100644 sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/CPPLINT.cfg
 create mode 100644 sr_tactile_sensor_controller/include/sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp
 create mode 100644 sr_tactile_sensor_controller/src/sr_mst_tactile_sensor_publisher.cpp

diff --git a/sr_edc_ethercat_drivers/CMakeLists.txt b/sr_edc_ethercat_drivers/CMakeLists.txt
index e61e45cc..2de4d466 100644
--- a/sr_edc_ethercat_drivers/CMakeLists.txt
+++ b/sr_edc_ethercat_drivers/CMakeLists.txt
@@ -42,7 +42,7 @@ catkin_package(
 )
 
 
-add_library(sr_edc_ethercat_drivers src/sr0x.cpp src/sr_edc.cpp src/sr06.cpp src/sr08.cpp src/sr09.cpp src/sr_edc_muscle.cpp src/srbridge.cpp src/motor_trace_buffer.cpp)
+add_library(sr_edc_ethercat_drivers src/sr0x.cpp src/sr_edc.cpp src/sr06.cpp src/sr08.cpp src/sr09.cpp src/sr10.cpp src/sr_edc_muscle.cpp src/srbridge.cpp src/motor_trace_buffer.cpp)
 add_dependencies(sr_edc_ethercat_drivers ${sr_edc_ethercat_drivers_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(sr_edc_ethercat_drivers ${BFD_LIBRARY} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 
diff --git a/sr_edc_ethercat_drivers/ethercat_device_plugin.xml b/sr_edc_ethercat_drivers/ethercat_device_plugin.xml
index 2e1e118f..6a6d046e 100644
--- a/sr_edc_ethercat_drivers/ethercat_device_plugin.xml
+++ b/sr_edc_ethercat_drivers/ethercat_device_plugin.xml
@@ -1,5 +1,5 @@
 <!--
- Copyright 2022 Shadow Robot Company Ltd.
+ Copyright 2022-2024 Shadow Robot Company Ltd.
  This program is free software: you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by the Free
  Software Foundation version 2 of the License.
@@ -42,6 +42,11 @@
       SR09 etherCAT driver (IMU)
     </description>
   </class>
+  <class name="10" type="SR10" base_class_type="EthercatDevice">
+    <description>
+      SR10 etherCAT driver (IMU + MST)
+    </description>
+  </class>
   <class name="16777218" type="SrEdcMuscle" base_class_type="EthercatDevice">
     <description>
       SrEdcMuscle etherCAT driver (right hand)
diff --git a/sr_edc_ethercat_drivers/include/sr_edc_ethercat_drivers/sr10.h b/sr_edc_ethercat_drivers/include/sr_edc_ethercat_drivers/sr10.h
new file mode 100644
index 00000000..0e858214
--- /dev/null
+++ b/sr_edc_ethercat_drivers/include/sr_edc_ethercat_drivers/sr10.h
@@ -0,0 +1,208 @@
+/* 
+* Copyright 2023, 2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+  * @file  sr10.h
+  * @brief This is a ROS driver for Shadow Robot #10 EtherCAT product ID
+  */
+
+#ifndef SR_EDC_ETHERCAT_DRIVERS_SR10_H
+#define SR_EDC_ETHERCAT_DRIVERS_SR10_H
+
+#include <ros_ethercat_hardware/ethercat_hardware.h>
+#include <ros_ethercat_model/robot_state.hpp>
+#include <sr_edc_ethercat_drivers/sr_edc.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <std_msgs/Int16.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sr_robot_msgs/SetImuScale.h>
+#include <sr_robot_msgs/SimpleMotorFlasher.h>
+#include <pthread.h>
+#include <bfd.h>
+#include <map>
+#include <vector>
+#include <memory>
+
+#include <sr_robot_lib/sr_motor_hand_lib.hpp>
+
+#include <sr_robot_msgs/EthercatDebug.h>
+
+#include <sr_external_dependencies/types_for_external.h>
+
+extern "C"
+{
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
+}
+
+/** 
+  * Class that contains all functions and member variables relevant to the SR10 ROS drivers.
+  * It inherits from the SrEdc class.
+  */  
+class SR10 :
+        public SrEdc
+{
+public:
+  /**
+    * SR10 ROS driver constructor.
+    */
+  SR10();
+
+  /** 
+    * Construct function, run at startup to set SyncManagers and FMMUs
+    * It sets up the SyncManagers and the FMMUs used by this EtherCAT client.
+    * It sets up two Mailboxes on two different memory areas (for the command requests and state responses)
+    *
+    * @param sh EtherCAT_SlaveHandler pointer
+    * @param start_address the start address of the shared memory area  (this is the address of the first byte of the first Mailbox) 
+    *
+    */
+  virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address);
+
+  /**
+    * Initialize the SR10 driver containg a the necessary EtherCAT data and the SrMotorHandLib structures
+    * necessary to build new commands and read current status from the Hand.
+    *
+    * @param hw the HardwareInterface pointer
+    * @param allow_unprogrammed if true, the driver will not check if the Hand is programmed
+    * @return 0 if successful, -1 otherwise
+    */
+  virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed = true);
+
+  /**
+    *  This function provides diagnostics data that can be displayed by
+    *  the runtime_monitor node. We use the mutliDiagnostics as it publishes
+    *  the diagnostics for each motors.
+    *
+    * @param diagnostic_vector the vector of diagnostic messages
+    * @param buffer is a pointer to some string. Not used in SR10.
+    *        Can we remove this? It would involve also removing
+    *        it from ethercat_hardware (EthercatHardwareDiagnosticsPublisher::publishDiagnostics)
+    */
+  virtual void multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &diagnostic_vector, unsigned char *buffer);
+
+  /** 
+    *  Packs the commands before sending them to the EtherCAT bus
+    *
+    *  This is one of the most important functions of this driver.
+    *  This function is called each millisecond (1 kHz freq) by the EthercatHardware::update() function
+    *  in the controlLoop() of the ros_etherCAT node. buffer is cast into the Ethercat structures related
+    *  to SR10 drivers. The command to be sent to the hand is populated with relevant data.
+    *  There is a second command which is meant to bo sent to the CAN busses. The memory address corresponding 
+    *  to the beggining of the second command can be found by shifting the pointer
+    *  by the size of the first command.
+    * 
+    * @param buffer is a pointer to an array containing 2 commands to be sent to the hand (and CAN busses).
+    *        The buffer has been allocated with command_size_ bytes, which is the sum of the two command size,
+    *        so we have to put the two commands one next to the other.
+    *        These are then sent via EtherCAT.
+    * @param halt   if true, it will disable actuator, usually by disabling H-bridge
+    * @param reset  if true, it will clear diagnostic error conditions device safety disable
+    */
+  virtual void packCommand(unsigned char *buffer, bool halt, bool reset);
+
+  /** 
+    *  This function handles receiving data from the EtherCAT bus
+    *
+    *  It allows the driver to get the data present on the EtherCAT bus and update its internal structures.
+    *
+    *  It gives us access to the logical memory instantiated during construct().
+    *  We access the data sent by PIC32 in a fashon similar to packCommand() (through direct memeory address).
+    *  
+    *  @param this_buffer The data just being received by EtherCAT
+    *  @param prev_buffer The previous data received by EtherCAT
+    */
+  virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
+
+protected:
+  /// Extra analog inputs real time publisher (+ accelerometer and gyroscope)
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray> > extra_analog_inputs_publisher;
+
+  /**
+    * This function will call the reinitialization function for the boards attached to the CAN bus
+    */
+  virtual void reinitialize_boards();
+
+  /**
+    * Given the identifier for a certain board (motor board/ muscle driver) determines the right value
+    * for the CAN bus and the ID of the board in that CAN bus.
+    *
+    * @param board_id the unique identifier for the board
+    * @param can_bus pointer to the can bus number we want to determine
+    * @param board_can_id pointer to the board id we want to determine
+    */
+  virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id);
+
+/**
+  * New scales to be set on IMU's gyroscope or accelerometer.
+  * Sets flag (imu_scale_gyr_) indicating that a new "scalling" command should be sent to the IMU.
+  *
+  * @param request service request containing the new scaling factor to be set for the IMU's gyroscope or accelerometer
+  * @param response service response (empty in this case)
+  * @param which pointer to a string that selects IMU's gyroscope or accelerometer
+  * @return true if the new scaling factor was set successfully, false otherwise  
+  */
+  bool imu_scale_callback_(sr_robot_msgs::SetImuScale::Request &request,
+                           sr_robot_msgs::SetImuScale::Response &response,
+                           const char *which);
+
+
+private:
+  /// Services to set the IMU's gyroscope and accelerometer scales
+  ros::ServiceServer imu_gyr_scale_server_;
+  ros::ServiceServer imu_acc_scale_server_;
+
+  /// Counter for the number of invalid Status replies we recieve.
+  unsigned int zero_buffer_read;
+
+  /// Robot state interface
+  ros_ethercat_model::RobotState * hw_;
+
+  /// IMU state interface
+  ros_ethercat_model::ImuState * imu_state_;
+  /// Pointer to etherCAT hand. Contains the necessary structures to build the (etherCAT) commands and read the status
+  std::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+                                               ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>> sr_hand_lib;
+
+  // IMU scaling variables
+  int imu_scale_gyr_;
+  int imu_scale_acc_;
+  bool imu_scale_change_;
+  std_msgs::Float64MultiArray extra_analog_msg_;
+
+  /// A counter used to publish the tactiles at 100Hz. Counts 10 cycles, then reset the cycle_count to 0.
+  int16_t cycle_count;
+
+  /// A counter for the number of frames received with status EDC_COMMAND_INVALID
+  int invalid_frame_counter_;
+
+  /** 
+    * This funcion reads the ethercat status and fills the imu_state with the relevant values.
+    *
+    * @param status_data pointer to the received EtherCAT Status data
+    */
+  void readImu(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS * status_data);
+
+  /// Debug real time publisher: publishes the raw ethercat data
+  std::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> > debug_publisher;
+};
+
+
+/* For the emacs weenies in the crowd.
+Local Variables:
+   c-basic-offset: 2
+End:
+*/
+
+#endif  // SR_EDC_ETHERCAT_DRIVERS_SR10_H
diff --git a/sr_edc_ethercat_drivers/src/sr10.cpp b/sr_edc_ethercat_drivers/src/sr10.cpp
new file mode 100644
index 00000000..43f7de95
--- /dev/null
+++ b/sr_edc_ethercat_drivers/src/sr10.cpp
@@ -0,0 +1,566 @@
+/* 
+* Copyright 2023, 2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+  * @file  sr10.cpp
+  * @brief This is a ROS driver for Shadow Robot #10 EtherCAT product ID
+  */
+
+
+#include <sr_edc_ethercat_drivers/sr10.h>
+
+#include <realtime_tools/realtime_publisher.h>
+
+#include <math.h>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <iomanip>
+#include <std_msgs/Int16.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <pthread.h>
+#include <memory>
+
+#include <sr_utilities/sr_math_utils.hpp>
+
+using std::string;
+using std::stringstream;
+using std::vector;
+
+#include <sr_external_dependencies/types_for_external.h>
+
+
+namespace is_edc_command_32_bits
+{
+// Assert if the EDC_COMMAND is 32bits (4 bytes) on the computer
+  static_assert(sizeof(EDC_COMMAND) == 4);
+}  // namespace is_edc_command_32_bits
+
+/// The EtherCAT data structures for the SR10 driver
+#define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS)
+#define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND)
+
+#define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
+
+#define ETHERCAT_COMMAND_DATA_ADDRESS                   PALM_0250_ETHERCAT_COMMAND_DATA_ADDRESS
+#define ETHERCAT_STATUS_DATA_ADDRESS                    PALM_0250_ETHERCAT_STATUS_DATA_ADDRESS
+#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS        PALM_0250_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
+#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS         PALM_0250_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
+
+
+PLUGINLIB_EXPORT_CLASS(SR10, EthercatDevice);
+
+/** 
+  * SR10 ROS driver constructor.
+  */
+SR10::SR10()
+        : zero_buffer_read(0),
+          cycle_count(0),
+          imu_scale_change_(false)
+{
+}
+
+/** 
+  *  Construct function, run at startup to set SyncManagers and FMMUs
+  *
+  *  The role of this function is to setup the SyncManagers and the FMMUs used by this EtherCAT slave.
+  *  This slave is using two Mailboxes on two different memory areas.
+  *
+  *  Here we are setting up the way of communicating between ROS and the PIC32 using the EtherCAT protocol.
+  *
+  *  We communicate using Shared Memory areas.
+  *
+  *  The FMMUs are usefull to map the logical memory used by ROS to the Physical memory of the EtherCAT slave chip (ET1200 chip).
+  *  So that the chip receiving the packet will know that the data at address 0x10000 is in reality to be written at physical address 0x1000 of the chip memory for example.
+  *  It is the mapping between the EtherCAT bus address space and each slave's chip own memory address space.
+  *
+  *  The SyncManagers are usefull to give a safe way of accessing this Shared Memory, using a consumer / producer model. There are features like interrupts to tell the consumer
+  *  that there is something to consume or to tell the producer that the Mailbox is empty and then ready to receive a new message.
+  *
+  *  - One Mailbox contains the commands, written by ROS, read by the PIC32
+  *  - One Mailbox contains the status, written back by the PIC32, read by ROS
+  *
+  *  That's basically one Mailbox for upstream and one Mailbox for downstream.
+  *
+  * - The first Mailbox contains in fact two commands, one is the torque demand, the other is a CAN command used in CAN_DIRECT_MODE to communicate with the SimpleMotor for
+  *   test purposes, or to reflash a new firmware in bootloading mode.
+  *   This Mailbox is at logicial address 0x10000 and mapped via a FMMU to physical address 0x1000 (the first address of user memory)
+  * - The second Mailbox contains in fact two status, they are the response of the two previously described commands. One is the status containing the joints data, sensor
+  *   data, finger tips data and motor data. The other is the can command response in CAN_DIRECT_MODE. When doing a flashing in bootloading mode this is usually an acknowledgment
+  *   from the bootloader. This Mailbox is at logical address 0x10038 and mapped via a FMMU to physical address 0x1038.
+  *
+  * This function sets the two private members command_size_ and status_size_ to be the size of each Mailbox.
+  * It is important for these numbers to be accurate since they are used by the EthercatHardware class when manipulating the buffers.
+  * If you need to have several commands like in this SR10 driver, put the sum of the size, same thing for the status.
+  *
+  * @param sh EtherCAT_SlaveHandler pointer
+  * @param start_address the start address of the shared memory area  (this is the address of the first byte of the first Mailbox) 
+  *
+  */
+void SR10::construct(EtherCAT_SlaveHandler *sh, int &start_address)
+{
+  // Calls parent class construct function
+  SrEdc::construct(sh, start_address, ETHERCAT_COMMAND_DATA_SIZE, ETHERCAT_STATUS_DATA_SIZE,
+                   ETHERCAT_CAN_BRIDGE_DATA_SIZE,
+                   ETHERCAT_COMMAND_DATA_ADDRESS, ETHERCAT_STATUS_DATA_ADDRESS,
+                   ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS, ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS);
+
+  ROS_INFO("Finished constructing the SR10 driver");
+}
+
+/**
+  * Initialize the SR10 driver containg a the EtherCAT data and the SrMotorHandLib structures
+  * necessary to build new commands and read current status from the Hand.
+  *
+  * @param hw the HardwareInterface pointer
+  * @param allow_unprogrammed if true, the driver will not check if the Hand is programmed
+  * @return 0 if successful, -1 otherwise
+  */
+int SR10::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
+{
+  int retval = SR0X::initialize(hw, allow_unprogrammed);
+
+  if (retval != 0)
+  {
+    return retval;
+  }
+
+  // Cast the HardwareInterface pointer to a RobotState pointer
+  hw_ = static_cast<ros_ethercat_model::RobotState *> (hw);
+
+  // Create the SrMotorHandLib structure
+  sr_hand_lib = std::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+          ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND> >(
+          new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+                  ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>(hw, nodehandle_, nh_tilde_,
+                                                                 device_id_, device_joint_prefix_));
+
+  ROS_INFO("ETHERCAT_STATUS_DATA_SIZE      = %4d bytes", static_cast<int> (ETHERCAT_STATUS_DATA_SIZE));
+  ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE     = %4d bytes", static_cast<int> (ETHERCAT_COMMAND_DATA_SIZE));
+  ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE  = %4d bytes", static_cast<int> (ETHERCAT_CAN_BRIDGE_DATA_SIZE));
+
+  // Initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
+  extra_analog_inputs_publisher.reset(
+          new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_, "palm_extras", 10));
+
+  // Debug real time publisher: publishes the raw ethercat data
+  debug_publisher = std::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug> >(
+          new realtime_tools::RealtimePublisher<sr_robot_msgs::EthercatDebug>(nodehandle_, "debug_etherCAT_data", 4));
+
+  debug_publisher->msg_.sensors.resize(SENSORS_NUM_0220 + 1);
+
+  // Obtaining information from 10 motors (odd or even motors) for each Palm loop cycle
+  debug_publisher->msg_.motor_data_packet_torque.resize(10);
+  debug_publisher->msg_.motor_data_packet_misc.resize(10);
+
+  std::string imu_name = device_joint_prefix_ + "imu";
+  imu_state_ = hw_->getImuState(imu_name);
+
+  // Imu initialization
+  imu_state_->data_.orientation[0] = 0.0; imu_state_->data_.orientation[1] = 0.0;
+  imu_state_->data_.orientation[2] = 0.0; imu_state_->data_.orientation[3] = 1.0;
+
+  std::fill(imu_state_->data_.linear_acceleration_covariance,
+            imu_state_->data_.linear_acceleration_covariance + 9, 0.0);
+  std::fill(imu_state_->data_.angular_velocity_covariance,
+            imu_state_->data_.angular_velocity_covariance + 9, 0.0);
+  std::fill(imu_state_->data_.orientation_covariance,
+            imu_state_->data_.orientation_covariance + 9, 0.0);
+
+  // Broadcast /set_gyr_scale and /set_acc_scale services
+  imu_gyr_scale_server_ =  nodehandle_.advertiseService
+    <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
+    ("/" + imu_name + "/set_gyr_scale", boost::bind(&SR10::imu_scale_callback_, this, _1, _2, "gyr"));
+  imu_acc_scale_server_ =  nodehandle_.advertiseService
+    <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
+    ("/" + imu_name + "/set_acc_scale", boost::bind(&SR10::imu_scale_callback_, this, _1, _2, "acc"));
+
+  // Initialize default IMU scaling values
+  ros::param::param<int>("/" + imu_name + "/acc_scale", imu_scale_acc_, 0);
+  ros::param::param<int>("/" + imu_name + "/gyr_scale", imu_scale_gyr_, 0);
+
+  // and set the flag to true so that the IMU data is scaled
+  imu_scale_change_ = true;
+
+  // Adjust extra_analog_msg_ data labels and sizes
+  extra_analog_msg_.layout.dim.resize(3);
+  extra_analog_msg_.data.resize(3 + 3 + 4);
+  extra_analog_msg_.layout.dim[0].label = "accelerometer";
+  extra_analog_msg_.layout.dim[0].size = 3;
+  extra_analog_msg_.layout.dim[1].label = "gyrometer";
+  extra_analog_msg_.layout.dim[1].size = 3;
+  extra_analog_msg_.layout.dim[2].label = "analog_inputs";
+  extra_analog_msg_.layout.dim[2].size = 4;
+
+  /// A counter for the number of frames received with status EDC_COMMAND_INVALID
+  invalid_frame_counter_ = 0;
+
+  return retval;
+}
+
+/**
+  *  This function provides diagnostics data that can be displayed by
+  *  the runtime_monitor node. We use the mutliDiagnostics as it publishes
+  *  the diagnostics for each motors.
+  *
+  * @param diagnostic_vector the vector of diagnostic messages
+  * @param buffer is a pointer to some string. Not used in SR10.
+  *        Can we remove this? It would involve also removing
+  *        it from ethercat_hardware (EthercatHardwareDiagnosticsPublisher::publishDiagnostics)
+  */
+void SR10::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &diagnostic_vector, unsigned char *buffer)
+{
+  diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status(diagnostic_status_);
+
+  stringstream name;
+  string prefix = device_id_.empty() ? device_id_ : (device_id_ + " ");
+  diagnostic_status.name = prefix + "EtherCAT Dual CAN Palm";
+  diagnostic_status.summary(diagnostic_status.OK, "OK");
+  stringstream hardware_id;
+  hardware_id << sh_->get_product_code() << "-" << sh_->get_serial();
+  diagnostic_status.hardware_id = hardware_id.str();
+
+  diagnostic_status.clear();
+  diagnostic_status.addf("Position", "%02d", sh_->get_ring_position());
+  diagnostic_status.addf("Product Code", "%d", sh_->get_product_code());
+  diagnostic_status.addf("Serial Number", "%d", sh_->get_serial());
+  diagnostic_status.addf("Revision", "%d", sh_->get_revision());
+  diagnostic_status.addf("Counter", "%d", ++counter_);
+  diagnostic_status.addf("Invalid Frames Received Counter", "%d", invalid_frame_counter_);
+
+  diagnostic_status.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
+  diagnostic_status.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
+  // reset the idle time min to a big number, to get a fresh number on next diagnostic
+  sr_hand_lib->main_pic_idle_time_min = 1000;
+
+  this->ethercatDiagnostics(diagnostic_status, 2);
+  diagnostic_vector.push_back(diagnostic_status);
+
+  // Add the diagnostics from the hand
+  sr_hand_lib->add_diagnostics(diagnostic_vector, diagnostic_status);
+
+  // Add the diagnostics from the tactiles
+  if (sr_hand_lib->tactiles != NULL)
+  {
+    sr_hand_lib->tactiles->add_diagnostics(diagnostic_vector, diagnostic_status);
+  }
+}
+
+/** 
+  *  Packs the commands before sending them to the EtherCAT bus
+  *
+  *  This is one of the most important functions of this driver.
+  *  This function is called each millisecond (1 kHz freq) by the EthercatHardware::update() function
+  *  in the controlLoop() of the ros_etherCAT node. buffer is cast into the Ethercat structures related
+  *  to SR10 drivers. The command to be sent to the hand is populated with relevant data.
+  *  There is a second command which is meant to bo sent to the CAN busses. The memory address corresponding 
+  *  to the beggining of the second command can be found by shifting the pointer
+  *  by the size of the first command.
+  * 
+  * @param buffer is a pointer to an array containing 2 commands to be sent to the hand (and CAN busses).
+  *        The buffer has been allocated with command_size_ bytes, which is the sum of the two command size,
+  *        so we have to put the two commands one next to the other.
+  *        These are then sent via EtherCAT.
+  * @param halt   if true, it will disable actuator, usually by disabling H-bridge
+  * @param reset  if true, it will clear diagnostic error conditions device safety disable
+  */
+void SR10::packCommand(unsigned char *buffer, bool halt, bool reset)
+{
+  SrEdc::packCommand(buffer, halt, reset);
+
+  ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND *command =
+                                    reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND *>(buffer);
+  ETHERCAT_CAN_BRIDGE_DATA *message =
+                                    reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(buffer + ETHERCAT_COMMAND_DATA_SIZE);
+
+  if (!flashing)
+  {
+    command->EDC_command = EDC_COMMAND_SENSOR_DATA;
+  }
+  else
+  {
+    command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
+  }
+
+  if (imu_scale_change_)
+  {
+    command->imu_command.command = IMU_COMMAND_SET_SCALE;
+    command->imu_command.argument[0] = imu_scale_acc_;
+    command->imu_command.argument[1] = imu_scale_gyr_;
+    imu_scale_change_ = false;
+  }
+  else
+  {
+    command->imu_command.command = IMU_COMMAND_NONE;
+  }
+
+  // Request for the different sensors' (including tactile) and motors' status information.
+  // It alternates between even and uneven motors everytime.
+  // It also builds the next control command to send to the motors (e.g. torque control)
+  sr_hand_lib->build_command(command);
+
+  ROS_DEBUG("Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X "
+                    "0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
+            command->to_motor_data_type,
+            command->motor_data[0],
+            command->motor_data[1],
+            command->motor_data[2],
+            command->motor_data[3],
+            command->motor_data[4],
+            command->motor_data[5],
+            command->motor_data[6],
+            command->motor_data[7],
+            command->motor_data[8],
+            command->motor_data[9],
+            command->motor_data[10],
+            command->motor_data[11],
+            command->motor_data[12],
+            command->motor_data[13],
+            command->motor_data[14],
+            command->motor_data[15],
+            command->motor_data[16],
+            command->motor_data[17],
+            command->motor_data[18],
+            command->motor_data[19]);
+
+  build_CAN_message(message);
+}
+
+/** 
+  *  This function handles receiving data from the EtherCAT bus
+  *
+  *  It allows the driver to get the data present on the EtherCAT bus and update its internal structures.
+  *
+  *  It gives us access to the logical memory instantiated during construct().
+  *  We access the data sent by PIC32 in a fashon similar to packCommand() (through direct memeory address).
+  *  
+  *  @param this_buffer The data just being received by EtherCAT
+  *  @param prev_buffer The previous data received by EtherCAT
+  */
+bool SR10::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
+{
+  ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS *status_data =
+          reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS *>(this_buffer + command_size_);
+  ETHERCAT_CAN_BRIDGE_DATA *can_data = reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(this_buffer + command_size_ +
+                                                                                    ETHERCAT_STATUS_DATA_SIZE);
+  // number of packets received since the start of the driver
+  static unsigned int num_rxed_packets = 0;
+
+  ++num_rxed_packets;
+
+  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
+  if (debug_publisher->trylock())
+  {
+    debug_publisher->msg_.header.stamp = ros::Time::now();
+
+    for (unsigned int index = 0; index < SENSORS_NUM_0220 + 1; ++index)
+    {
+      debug_publisher->msg_.sensors[index] = (status_data->sensors[index]);
+    }
+
+    debug_publisher->msg_.motor_data_type.data = static_cast<int> (status_data->motor_data_type);
+    debug_publisher->msg_.which_motors = status_data->which_motors;
+    debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
+    debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
+
+    // Getting information from 10 motors at a time
+    for (unsigned int index = 0; index < 10; ++index)
+    {
+      debug_publisher->msg_.motor_data_packet_torque[index] = (status_data->motor_data_packet[index].torque);
+      debug_publisher->msg_.motor_data_packet_misc[index] = (status_data->motor_data_packet[index].misc);
+    }
+
+    debug_publisher->msg_.tactile_data_type = static_cast<int32u>(status_data->tactile_data_type);
+    debug_publisher->msg_.tactile_data_valid = static_cast<int16u> (status_data->tactile_data_valid);
+
+    debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
+
+    debug_publisher->unlockAndPublish();
+  }
+
+  if (status_data->EDC_command == EDC_COMMAND_INVALID)
+  {
+    // received empty message: the pic is not writing to its mailbox.
+    ++zero_buffer_read;
+    float percentage_packet_loss = 100.f * (static_cast<float>(zero_buffer_read) /
+                                            static_cast<float>(num_rxed_packets));
+
+    invalid_frame_counter_++;
+    ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus",
+              zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
+    return true;
+  }
+
+  // We've received a coherent/valid message.
+  // Read IMU status and update internal structures
+  readImu(status_data);
+
+  // Update the library (positions, diagnostics values, actuators, etc...)
+  // with the received information received from the master
+  sr_hand_lib->update(status_data);
+
+  // Now publish the data at 100Hz (every 10 cycles)
+  if (cycle_count >= 10)
+  {
+    // Publish additional data (accelerometer / gyroscope / analog inputs)
+    extra_analog_msg_.data[0] = (int16_t) status_data->sensors[ACCX];
+    extra_analog_msg_.data[1] = (int16_t) status_data->sensors[ACCY];
+    extra_analog_msg_.data[2] = (int16_t) status_data->sensors[ACCZ];
+
+    extra_analog_msg_.data[3] = (int16_t) status_data->sensors[GYRX];
+    extra_analog_msg_.data[4] = (int16_t) status_data->sensors[GYRY];
+    extra_analog_msg_.data[5] = (int16_t) status_data->sensors[GYRZ];
+
+    extra_analog_msg_.data[6] = status_data->sensors[ANA0];
+    extra_analog_msg_.data[7] = status_data->sensors[ANA1];
+    extra_analog_msg_.data[8] = status_data->sensors[ANA2];
+    extra_analog_msg_.data[9] = status_data->sensors[ANA3];
+
+    if (extra_analog_inputs_publisher->trylock())
+    {
+      extra_analog_inputs_publisher->msg_ = extra_analog_msg_;
+      extra_analog_inputs_publisher->unlockAndPublish();
+    }
+
+    cycle_count = 0;
+  }
+  ++cycle_count;
+
+
+  // If we're flashing, check is the packet has been acked
+  if (flashing & !can_packet_acked)
+  {
+    if (can_data_is_ack(can_data))
+    {
+      can_packet_acked = true;
+    }
+  }
+
+  return true;
+}
+
+/**
+  * This function will call the reinitialization function for the boards attached to the CAN bus
+  */
+void SR10::reinitialize_boards()
+{
+  // Reinitialize motors information
+  sr_hand_lib->reinitialize_motors();
+}
+
+/**
+  * Given the identifier for a certain board (motor board/ muscle driver) determines the right value
+  * for the CAN bus and the ID of the board in that CAN bus.
+  *
+  * @param board_id the unique identifier for the board
+  * @param can_bus pointer to the can bus number we want to determine
+  * @param board_can_id pointer to the board id we want to determine
+*/
+void SR10::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
+{
+  // We're using 2 can busses,
+  // if motor id is between 0 and 9, then we're using the can_bus 1
+  // else, we're using the can bus 2.
+  int motor_id_tmp = board_id;
+  if (motor_id_tmp > 9)
+  {
+    motor_id_tmp -= 10;
+    *can_bus = 2;
+  }
+  else
+  {
+    *can_bus = 1;
+  }
+
+  *board_can_id = motor_id_tmp;
+}
+
+/**
+  * New scales to be set on IMU's gyroscope or accelerometer.
+  * Sets flag (imu_scale_gyr_) indicating that a new "scalling" command should be sent to the IMU.
+  *
+  * @param request service request containing the new scaling factor to be set for the IMU's gyroscope or accelerometer
+  * @param response service response (empty in this case)
+  * @param which pointer to a string that selects IMU's gyroscope or accelerometer
+  * @return true if the new scaling factor was set successfully, false otherwise  
+  */
+bool SR10::imu_scale_callback_(sr_robot_msgs::SetImuScale::Request &request,
+                               sr_robot_msgs::SetImuScale::Response &response,
+                               const char *which)
+{
+  if (request.scale == 0 || request.scale == 1 || request.scale == 2)
+  {
+    if (which == "acc")
+    {
+      imu_scale_acc_ = request.scale;
+    }
+    else if (which == "gyr")
+    {
+      imu_scale_gyr_ = request.scale;
+    }
+
+    imu_scale_change_ = true;
+    return true;
+  }
+  else
+  {
+    ROS_WARN_STREAM("Tried to set illegal value: " << (int) request.scale);
+    return false;
+  }
+}
+
+/** 
+  * This funcion reads the ethercat status and fills the imu_state with the relevant values.
+  *
+  * @param status_data pointer to the received EtherCAT Status data
+  */
+void SR10::readImu(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS * status_data)
+{
+  double acc_multiplier = 1 << imu_scale_acc_;
+  double gyr_multiplier = 1 << imu_scale_gyr_;
+
+  int16_t zero_catch = 0;
+
+  for (size_t index = 0; index < 2; ++index)
+  {
+    if (status_data->sensors[ACCX + index] == 0)
+    {
+      ++zero_catch;
+    }
+    if (status_data->sensors[GYRX + index] == 0)
+    {
+      ++zero_catch;
+    }
+  }
+  // It seems that this was an attempt to identify an invalid status sent by the IMU
+  // However, it is not clear that "all zeros" would not be a valid IMU state
+  if (zero_catch < 6)
+  {
+    imu_state_->data_.linear_acceleration[0] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCX]);
+    imu_state_->data_.linear_acceleration[1] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCY]);
+    imu_state_->data_.linear_acceleration[2] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCZ]);
+
+    imu_state_->data_.angular_velocity[0] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRX]);
+    imu_state_->data_.angular_velocity[1] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRY]);
+    imu_state_->data_.angular_velocity[2] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRZ]);
+  }
+}
+
+/* For the emacs weenies in the crowd.
+   Local Variables:
+   c-basic-offset: 2
+   End:
+*/
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h b/sr_external_dependencies/include/sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h
index 68a03ef0..a83301bc 100644
--- a/sr_external_dependencies/include/sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h
@@ -1,61 +1,34 @@
-//
-// © 2010 Shadow Robot Company Limited.
-//
-// FileName:        this_node.h
-// Dependencies:
-// Processor:       PIC32
-// Compiler:        MPLAB® C32
-//
-//  +------------------------------------------------------------------------+
-//  | This file is part of The Shadow Robot PIC32 firmware code base.        |
-//  |                                                                        |
-//  | It is free software: you can redistribute it and/or modify             |
-//  | it under the terms of the GNU General Public License as published by   |
-//  | the Free Software Foundation, either version 3 of the License, or      |
-//  | (at your option) any later version.                                    |
-//  |                                                                        |
-//  | It is distributed in the hope that it will be useful,                  |
-//  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
-//  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
-//  | GNU General Public License for more details.                           |
-//  |                                                                        |
-//  | You should have received a copy of the GNU General Public License      |
-//  | along with this code repository. The text of the license can be found  |
-//  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
-//  +------------------------------------------------------------------------+
-//
-//
-//
-//  Doxygen
-//  -------
-//
-//! @file
-//!
-//! The term "Command" means data going from the ROS PC to the Node on the robot
-//! Previously known as "Incoming"
-//!
-//! The term "Status"  means data going from Node on the robot the to the ROS PC
-//! Previously known as "Outgoing"
-//!
-//!
-//! @addtogroup
-//
+/* 
+* Copyright 2010, 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/** 
+  * @file 0240_palm_edc_IMU_ethercat_protocol.h
+  *
+  * @brief EtherCat protocol for devices with Product ID 9
+  *
+  *  The Ethercat "Command" packet refers to data sent from the ROS host to the Node on the robot
+  *  The Ethercat "Status" packet refers to data sent from Node on the robot the to the ROS host.
+  */
 
 #ifndef PALM_EDC_0240_ETHERCAT_PROTOCOL_H_INCLUDED
 #define PALM_EDC_0240_ETHERCAT_PROTOCOL_H_INCLUDED
 
 #include "../0220_palm_edc/0220_palm_edc_ethercat_protocol.h"
 
-#define  IMU_COMMAND_NONE        0
-#define  IMU_COMMAND_SET_SCALE   1
-
-typedef struct
-{
-  int16u command;
-  int8u  argument[2];
-} IMU_COMMAND_TYPE;
-
-//! These are the data sent from the Palm to the host.
+//! Data structure sent from the Palm to the host (Status).
+//! Data is written to the EtherCAT bus sequencially, in the same order defined below.
 typedef struct
 {
     EDC_COMMAND                 EDC_command;                        //!< This tells us the contents of the data below.
@@ -63,7 +36,7 @@ typedef struct
                                                                     //!< value which arrived from the host in the previous
                                                                     //!< EtherCAT packet
 
-	int16u					    sensors[SENSORS_NUM_0220+1];
+    int16u                      sensors[SENSORS_NUM_0220+1];        //!< Joint sensors data
 
     FROM_MOTOR_DATA_TYPE        motor_data_type;                    //!< Which data does motor[] contain?
                                                                     //!< This value should agree with the previous value
@@ -77,9 +50,9 @@ typedef struct
 
     MOTOR_DATA_PACKET           motor_data_packet[10];              //!< Data for 10 motors only. (Even ones or Odd ones)
 
-    int32u                      tactile_data_type;
+    int32u                      tactile_data_type;                  //!< Identifies the tactile data type
     int16u                      tactile_data_valid;                 //!< Bit 0: FF. Bit 4: TH.
-    TACTILE_SENSOR_STATUS_v1    tactile[5];                         //
+    TACTILE_SENSOR_STATUS_v1    tactile[5];                         //!< Tactile sensors data
 
     int16u                      idle_time_us;                       //!< The idle time from when the palm has finished dealing with one EtherCAT
                                                                     //!< packet, and the next packet arriving. Ideally, this number should be more than 50.
@@ -87,7 +60,8 @@ typedef struct
 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS;
 
 
-//! These are the data sent by the host.
+//! Data structure sent from the host to the Palm (Command).
+//! Data is written to the EtherCAT bus sequencially, in the same order defined below.
 typedef struct
 {
     EDC_COMMAND                 EDC_command;                        //!< What type of data should the palm send back in the next packet?
@@ -100,28 +74,20 @@ typedef struct
     int16s                      motor_data[NUM_MOTORS];             //!< Data to send to motors. Typically torque/PWM demands, or configs.
 
     int32u                      tactile_data_type;                  //!< Request for specific tactile data
-    IMU_COMMAND_TYPE            imu_command;                        // Command to configure the IMU
+    IMU_COMMAND_TYPE            imu_command;                        //!< Command to configure the IMU
 
 } __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND;
 
-/*
-#define ETHERCAT_COMMAND_HEADER_SIZE  (  sizeof(EDC_COMMAND)           \       //!< What's the minimum amount of
-                                       + sizeof(FROM_MOTOR_DATA_TYPE)   \
-                                       + sizeof(int16s)  )
-*/
-
-#define PALM_0240_ETHERCAT_COMMAND_HEADER_SIZE  (  sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s)  )
+//! EtherCAT protocol packet/header sizes
+#define PALM_0240_ETHERCAT_COMMAND_HEADER_SIZE  (sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s))
 
 #define PALM_0240_ETHERCAT_STATUS_DATA_SIZE       sizeof(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS)
 #define PALM_0240_ETHERCAT_COMMAND_DATA_SIZE      sizeof(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND)
 
-                                                //  Now we need to be *sure* that the Host and the Slave definitely
-                                                //  agree on the size of the EtherCAT packets, even if the host is a
-                                                //  64-bit machine or something. So we have these calculated sizes.
-                                                //  The host and slave can ASSERT that the sizeof() the packets
-                                                //  matches the agreed sizes.
-#define ETHERCAT_STATUS_0240_AGREED_SIZE     232     //! This is the size of the Status  EtherCAT packet (Status + CAN packet)
-#define ETHERCAT_COMMAND_0240_AGREED_SIZE    74      //! This is the size of the Command EtherCAT packet (Status + CAN packet)
+//! Ethercat Command and Status packets "agreed" sizes.
+//! They are use by the host and clients to assert if the incoming packets are the correct size.
+#define ETHERCAT_STATUS_0240_AGREED_SIZE     232  //!< This is the size of the Status  EtherCAT packet (Status + CAN packet)
+#define ETHERCAT_COMMAND_0240_AGREED_SIZE    74   //!< This is the size of the Command EtherCAT packet (Command + CAN packet)
 
 
 
@@ -136,6 +102,7 @@ typedef struct
 //!
 //!
 
+//! Command/Status EtherCAT packet memory addresses. Necessary to Read/write data through Direct Memory Access (DMA)
 #define PALM_0240_ETHERCAT_COMMAND_DATA_ADDRESS               0x1000
 #define PALM_0240_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS    (PALM_0240_ETHERCAT_COMMAND_DATA_ADDRESS            + PALM_0240_ETHERCAT_COMMAND_DATA_SIZE)
 
@@ -144,5 +111,4 @@ typedef struct
 
 //#define NUM_CONFIGS_REQUIRED 5
 
-
 #endif
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h b/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h
new file mode 100644
index 00000000..d22b1c52
--- /dev/null
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h
@@ -0,0 +1,145 @@
+/* 
+* Copyright 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/** 
+  * @file 0250_palm_edc_IMU_ethercat_protocol.h
+  *
+  * @brief EtherCat protocol for devices with Product ID 10
+  *
+  *  The Ethercat "Command" packet refers to data sent from the ROS host to the Node on the robot
+  *  The Ethercat "Status" packet refers to data sent from Node on the robot the to the ROS host.
+  */
+
+#ifndef PALM_EDC_0250_ETHERCAT_PROTOCOL_H_INCLUDED
+#define PALM_EDC_0250_ETHERCAT_PROTOCOL_H_INCLUDED
+
+#include "../0220_palm_edc/0220_palm_edc_ethercat_protocol.h"
+
+//! Data structure sent from the Palm to the host (Status).
+//! Data is written to the EtherCAT bus sequencially, in the same order defined below.
+typedef struct
+{
+  EDC_COMMAND                 EDC_command;                  //!< Identifies the contents of the data below and should be identical to the EDC_command
+                                                            //!< value which arrived from the host in the previous EtherCAT packet
+                                                            // 4 bytes
+
+  int16u					            sensors[SENSORS_NUM_0220+1];  //!< Joints sensors data
+                                                            // 74 bytes
+
+  int32u                      tactile_data_type;            //!< Identifies the tactile data type
+                                                            // 4 bytes
+  int16u                      tactile_data_valid;           //!< Identifies if tactile data is valid for each finger (Bit 0: FF. Bit 4: TH)
+                                                            // 2 bytes
+  TACTILE_SENSOR_STATUS_v3    tactile[5];                   //!< Tacile sensors data
+                                                            // 78 * 5 = 390 bytes
+                                                            // TOTAL Tactile data = 396 bytes
+
+  FROM_MOTOR_DATA_TYPE        motor_data_type;              //!< Identifies the contents present in motor_data_packet[] below
+                                                            //!< This value should be the same as the one sent in the previous command
+                                                            //!< ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND
+                                                            // 4 bytes
+                                                                  
+  int16s                      which_motors;                 //!< Identifies the group of motors sending data in motor_data_packet[] below.
+                                                            //!< 0: Even motor numbers.  1: Odd motor numbers
+                                                            //!< This value should be the same as the one sent in the previous command
+                                                            //!< ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND                                                              
+                                                            // 2 bytes
+
+  int32u                      which_motor_data_arrived;     //!< Bit N set when motor CAN message arrives.
+                                                            //!< Ideally, bits 0..19 get set
+                                                            // 4 bytes     
+
+  int32u                      which_motor_data_had_errors;  //!< Bit N set when motor sends bad CAN message.
+                                                            //!< Ideally, no bits get set.
+                                                            // 4 bytes     
+
+  MOTOR_DATA_PACKET           motor_data_packet[10];        //!< Data from the motors. 
+                                                            //!< Only 10 motors send data at a time (even or odd ones).
+                                                            // 4 * 10 = 40 bytes
+                                                            // Total Motor data = 54 bytes
+
+  int16u                      idle_time_us;                 //!< Time distance between the previous EtherCAT Status being sent, and next Command packet arriving.
+                                                            //!< Ideally, this number should be more than 50.
+                                                            // 2 bytes
+
+                                                            // Total: 530 bytes
+
+} __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS;
+
+
+//! Data structure sent from the host to the Palm (Command).
+//! Data is written to the EtherCAT bus sequencially, in the same order defined below.
+typedef struct
+{
+    EDC_COMMAND                 EDC_command;                        //!< Identifies the data being requested to the Palm
+                                                                    // 4 bytes
+
+    FROM_MOTOR_DATA_TYPE        from_motor_data_type;               //!< Identifies the type of data we want to read from the motors
+                                                                    // 4  bytes
+    
+    int16s                      which_motors;                       //!< Identifies the group of motors (odd or even) we want to read data from.
+                                                                    //!< 0: Even motor numbers. 1: Odd motor numbers
+                                                                    // 2  bytes
+
+    TO_MOTOR_DATA_TYPE          to_motor_data_type;                 //!< Identifies the command/data type being sent to the motors
+                                                                    // 4  bytes
+    
+    int16s                      motor_data[NUM_MOTORS];             //!< Data to send to the motors. Typically torque/PWM demands, or configs.
+                                                                    // 2 * 20 = 40 bytes
+
+    int32u                      tactile_data_type;                  //!< Identifies the type of data we want to read from the tactile sensors
+                                                                    // 4  bytes
+    
+    IMU_COMMAND_TYPE            imu_command;                        //!< Identifies the command being sent to the IMU.
+                                                                    //!< For IMU configuration purposes.
+                                                                    // 4 bytes
+
+                                                                    // Total: 62 bytes
+
+} __attribute__((packed)) ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND;
+
+//! EtherCAT protocol packet/header sizes
+#define PALM_0250_ETHERCAT_COMMAND_HEADER_SIZE  (sizeof(EDC_COMMAND) + sizeof(FROM_MOTOR_DATA_TYPE) + sizeof(int16s))
+
+#define PALM_0250_ETHERCAT_STATUS_DATA_SIZE       sizeof(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS)
+#define PALM_0250_ETHERCAT_COMMAND_DATA_SIZE      sizeof(ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND)
+
+//! Ethercat Command and Status packets "agreed" sizes.
+//! They are use by the host and clients to assert if the incoming packets are the correct size.
+#define ETHERCAT_STATUS_0250_AGREED_SIZE     542  //!< This is the size of the Status  EtherCAT packet (Status + CAN packet) 
+#define ETHERCAT_COMMAND_0250_AGREED_SIZE    74   //!< This is the size of the Command EtherCAT packet (Command + CAN packet)
+
+
+//! | ETHERCAT_COMMAND_DATA | ETHERCAT_CAN_BRIDGE_DATA_COMMAND | ETHERCAT_STATUS_DATA | ETHERCAT_CAN_BRIDGE_DATA_STATUS |
+//! |                       |                                  |                      |
+//! |                       |                                  |                      ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
+//! |                       |                                  |
+//! |                       |                                  ETHERCAT_STATUS_DATA_ADDRESS
+//! |                       ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
+//! |
+//! ETHERCAT_COMMAND_DATA_ADDRESS
+//!
+//!
+
+//! Command/Status EtherCAT packet memory addresses. Necessary to Read/write data through Direct Memory Access (DMA)
+#define PALM_0250_ETHERCAT_COMMAND_DATA_ADDRESS               0x1000
+#define PALM_0250_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS    (PALM_0250_ETHERCAT_COMMAND_DATA_ADDRESS            + PALM_0250_ETHERCAT_COMMAND_DATA_SIZE)
+
+#define PALM_0250_ETHERCAT_STATUS_DATA_ADDRESS                (PALM_0250_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS + ETHERCAT_CAN_BRIDGE_DATA_SIZE)
+#define PALM_0250_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS     (PALM_0250_ETHERCAT_STATUS_DATA_ADDRESS             + PALM_0250_ETHERCAT_STATUS_DATA_SIZE)
+
+//#define NUM_CONFIGS_REQUIRED 5
+
+#endif
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/CPPLINT.cfg b/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/CPPLINT.cfg
new file mode 100644
index 00000000..9513cbb6
--- /dev/null
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/0250_palm_edc_IMU_MST/CPPLINT.cfg
@@ -0,0 +1,2 @@
+exclude_files=.*\.h
+
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/common/common_edc_ethercat_protocol.h b/sr_external_dependencies/include/sr_external_dependencies/external/common/common_edc_ethercat_protocol.h
index 94f30699..bb7eb374 100644
--- a/sr_external_dependencies/include/sr_external_dependencies/external/common/common_edc_ethercat_protocol.h
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/common/common_edc_ethercat_protocol.h
@@ -1,45 +1,26 @@
-//
-// © 2010 Shadow Robot Company Limited.
-//
-// FileName:        common_edc_ethercat_protocol.h
-// Dependencies:
-// Processor:       PIC32
-// Compiler:        MPLAB® C32
-//
-//  +------------------------------------------------------------------------+
-//  | This file is part of The Shadow Robot PIC32 firmware code base.        |
-//  |                                                                        |
-//  | It is free software: you can redistribute it and/or modify             |
-//  | it under the terms of the GNU General Public License as published by   |
-//  | the Free Software Foundation, either version 3 of the License, or      |
-//  | (at your option) any later version.                                    |
-//  |                                                                        |
-//  | It is distributed in the hope that it will be useful,                  |
-//  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
-//  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
-//  | GNU General Public License for more details.                           |
-//  |                                                                        |
-//  | You should have received a copy of the GNU General Public License      |
-//  | along with this code repository. The text of the license can be found  |
-//  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
-//  +------------------------------------------------------------------------+
-//
-//
-//
-//  Doxygen
-//  -------
-//
-//! @file
-//!
-//! The term "Command" means data going from the ROS PC to the Node on the robot
-//! Previously known as "Incoming"
-//!
-//! The term "Status"  means data going from Node on the robot the to the ROS PC
-//! Previously known as "Outgoing"
-//!
-//!
-//! @addtogroup
-//
+/* Copyright 2010, 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/** 
+  * @file common_edc_ethercat_protocol.h
+  *
+  * @brief Defines the EDC commands available to all Ethercat devices
+  *        as well as the list of joints and sensors in the hand.
+  *
+  *  The Ethercat "Command" packet refers to data sent from the ROS host to the Node on the robot
+  *  The Ethercat "Status" packet refers to data sent from Node on the robot the to the ROS host.
+  */
 
 #ifndef COMMON_EDC_ETHERCAT_PROTOCOL_H_INCLUDED
 #define COMMON_EDC_ETHERCAT_PROTOCOL_H_INCLUDED
@@ -56,15 +37,23 @@ typedef enum
     //EDC_COMMAND_TEST_RESULTS,                                   //!< Might be used in the future for running automated tests inside the firmware.
 }EDC_COMMAND;
 
+//! Define IMU available commands
+typedef struct
+{
+  int16u command;
+  int8u  argument[2];
+} IMU_COMMAND_TYPE;
 
+#define  IMU_COMMAND_NONE        0
+#define  IMU_COMMAND_SET_SCALE   1
 
-#ifndef NO_STRINGS													                        // The PIC compiler doesn't deal well with strings.
+#ifndef NO_STRINGS                                              // The PIC compiler doesn't deal well with strings.
 
     static const char* slow_data_types[17] = {  "Invalid",                                  // 0x0000
-                                                "SVN revision",                             // 0x0001
-                                                "SVN revision on server at build time",     // 0x0002
+                                                "Git revision",                             // 0x0001
+                                                "Git revision on server at build time",     // 0x0002
 
-                                                "Modified from SVN revision",               // 0x0003
+                                                "Modified from Git revision",               // 0x0003
                                                 "Serial number low",                        // 0x0004
                                                 "Serial number high",                       // 0x0005
                                                 "Motor gear ratio",                         // 0x0006
@@ -114,7 +103,7 @@ typedef enum
 
 
 
-#ifndef NO_STRINGS                                              //   The PIC compiler doesn't deal well with strings.
+#ifndef NO_STRINGS                                              //  The PIC compiler doesn't deal well with strings.
 
     static const char* joint_names[JOINTS_NUM_0X20] = {  "FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4",
                                                          "MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4",
@@ -143,18 +132,18 @@ typedef enum
 //! This enum defines which ADC reading goes into which sensors[].
 typedef enum
 {
-	FFJ1=0, FFJ2,  FFJ3, FFJ4,                      // [ 0...3]
-	MFJ1,   MFJ2,  MFJ3, MFJ4,                      // [ 4...7]
-	RFJ1,   RFJ2,  RFJ3, RFJ4,                      // [ 8..11]
-	LFJ1,   LFJ2,  LFJ3, LFJ4, LFJ5,                // [12..16]
-    THJ1,   THJ2,  THJ3, THJ4, THJ5A, THJ5B,        // [17..22]
-    WRJ1A,  WRJ1B, WRJ2,                            // [23..25]
-
-	ACCX, ACCY, ACCZ,                               // [26..28]
-	GYRX, GYRY, GYRZ,                               // [29..32]
-
-	ANA0, ANA1, ANA2, ANA3,                     // [31..35]
-    IGNORE                                          // [36]
+  FFJ1=0, FFJ2,  FFJ3, FFJ4,                      // [ 0...3]
+  MFJ1,   MFJ2,  MFJ3, MFJ4,                      // [ 4...7]
+  RFJ1,   RFJ2,  RFJ3, RFJ4,                      // [ 8..11]
+  LFJ1,   LFJ2,  LFJ3, LFJ4, LFJ5,                // [12..16]
+  THJ1,   THJ2,  THJ3, THJ4, THJ5A, THJ5B,        // [17..22]
+  WRJ1A,  WRJ1B, WRJ2,                            // [23..25]
+
+  ACCX, ACCY, ACCZ,                               // [26..28]
+  GYRX, GYRY, GYRZ,                               // [29..31]
+
+  ANA0, ANA1, ANA2, ANA3,                         // [32..35]
+  IGNORE                                          // [36]
 }SENSOR_NAME_ENUM;
 
 
@@ -162,8 +151,8 @@ typedef enum
 
 typedef enum
 {
-      PALM_SVN_VERSION              =  0,
-    SERVER_SVN_VERSION              =  1
+      PALM_GIT_VERSION              =  0,
+    SERVER_GIT_VERSION              =  1
 }HARD_CONFIGURATION_INFORMATION;
 
 
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h b/sr_external_dependencies/include/sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h
index 7bf328ed..425a502f 100644
--- a/sr_external_dependencies/include/sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/common/ethercat_can_bridge_protocol.h
@@ -1,45 +1,26 @@
-//
-// © 2010 Shadow Robot Company Limited.
-//
-// FileName:        ethercat_can_bridge_protocol.h
-// Dependencies:
-// Processor:       PIC32
-// Compiler:        MPLAB® C32
-//
-//  +------------------------------------------------------------------------+
-//  | This file is part of The Shadow Robot PIC32 firmware code base.        |
-//  |                                                                        |
-//  | It is free software: you can redistribute it and/or modify             |
-//  | it under the terms of the GNU General Public License as published by   |
-//  | the Free Software Foundation, either version 3 of the License, or      |
-//  | (at your option) any later version.                                    |
-//  |                                                                        |
-//  | It is distributed in the hope that it will be useful,                  |
-//  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
-//  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
-//  | GNU General Public License for more details.                           |
-//  |                                                                        |
-//  | You should have received a copy of the GNU General Public License      |
-//  | along with this code repository. The text of the license can be found  |
-//  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
-//  +------------------------------------------------------------------------+
-//
-//
-//
-//  Doxygen
-//  -------
-//
-//! @file
-//!
-//! The term "Command" means data going from the ROS PC to the Node on the robot
-//! Previously known as "Incoming"
-//!
-//! The term "Status"  means data going from Node on the robot the to the ROS PC
-//! Previously known as "Outgoing"
-//!
-//!
-//! @addtogroup
-//
+/* Copyright 2010, 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/** 
+  * @file ethercat_can_bridge_protocol.h
+  *
+  * @brief Can Bridge specific Ethercat Protocol, common to all devices 
+  *
+  *  The Ethercat "Command" packet refers to data sent from the ROS host to the Node on the robot
+  *  The Ethercat "Status" packet refers to data sent from Node on the robot the to the ROS host.
+  */
+
 
 #ifndef ETHERCAT_CAN_BRIDGE_PROTOCOL_H_INCLUDED
 #define ETHERCAT_CAN_BRIDGE_PROTOCOL_H_INCLUDED
diff --git a/sr_external_dependencies/include/sr_external_dependencies/external/common/tactile_edc_ethercat_protocol.h b/sr_external_dependencies/include/sr_external_dependencies/external/common/tactile_edc_ethercat_protocol.h
index 8ef846b9..604e48b9 100644
--- a/sr_external_dependencies/include/sr_external_dependencies/external/common/tactile_edc_ethercat_protocol.h
+++ b/sr_external_dependencies/include/sr_external_dependencies/external/common/tactile_edc_ethercat_protocol.h
@@ -1,45 +1,27 @@
-//
-// © 2010 Shadow Robot Company Limited.
-//
-// FileName:        tactile_edc_ethercat_protocol.h
-// Dependencies:
-// Processor:       PIC32
-// Compiler:        MPLAB® C32
-//
-//  +------------------------------------------------------------------------+
-//  | This file is part of The Shadow Robot PIC32 firmware code base.        |
-//  |                                                                        |
-//  | It is free software: you can redistribute it and/or modify             |
-//  | it under the terms of the GNU General Public License as published by   |
-//  | the Free Software Foundation, either version 3 of the License, or      |
-//  | (at your option) any later version.                                    |
-//  |                                                                        |
-//  | It is distributed in the hope that it will be useful,                  |
-//  | but WITHOUT ANY WARRANTY; without even the implied warranty of         |
-//  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          |
-//  | GNU General Public License for more details.                           |
-//  |                                                                        |
-//  | You should have received a copy of the GNU General Public License      |
-//  | along with this code repository. The text of the license can be found  |
-//  | in Pic32/License/gpl.txt. If not, see <http://www.gnu.org/licenses/>.  |
-//  +------------------------------------------------------------------------+
-//
-//
-//
-//  Doxygen
-//  -------
-//
-//! @file
-//!
-//! The term "Command" means data going from the ROS PC to the Node on the robot
-//! Previously known as "Incoming"
-//!
-//! The term "Status"  means data going from Node on the robot the to the ROS PC
-//! Previously known as "Outgoing"
-//!
-//!
-//! @addtogroup
-//
+/* Copyright 2010, 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/** 
+  * @file tactile_edc_ethercat_protocol.h
+  *
+  * @brief Tactile sensor specific EtherCat protocol, common to all devices
+  *
+  * The term "Command" refers to data going from the ROS PC to the Node on the robot
+  * Previously known as "Incoming"
+  * The term "Status" refers to data going from Node on the robot the to the ROS PC
+  * Previously known as "Outgoing"
+  */
 
 #ifndef TACTILE_EDC_ETHERCAT_PROTOCOL_H_INCLUDED
 #define TACTILE_EDC_ETHERCAT_PROTOCOL_H_INCLUDED
@@ -123,7 +105,7 @@ typedef enum                                                            // Data
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4     = 0x0007,
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5     = 0x0008,
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6     = 0x0009,
-    TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7     = 0x000A, 
+    TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7     = 0x000A,
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8     = 0x000B,
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9     = 0x000C,
     TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10    = 0x000D,
@@ -169,7 +151,7 @@ typedef struct
 
     int16u  other_sensor_1;                                 // This is placed here to make this struct backwards compatible
                                                             // with older versions of the host side driver.
-    
+
     int16u  nothing[TACTILE_DATA_LENGTH_WORDS_v1 - 5];
 }TACTILE_SENSOR_BIOTAC_DATA_CONTENTS;
 
@@ -186,6 +168,18 @@ typedef union
     char    string[TACTILE_DATA_LENGTH_BYTES_v2];
 }TACTILE_SENSOR_STATUS_v2;
 
+// Length for 17 taxel-MST tactile sensors
+#define TACTILE_DATA_LENGTH_BYTES_v3   78 /// In reality we only need 77 bytes
+                                          /// But due to the underlying union structure bellow
+                                          /// this number needs to be even so that it's divisible by 2
+#define TACTILE_DATA_LENGTH_WORDS_v3   (TACTILE_DATA_LENGTH_BYTES_v3/2)
+
+typedef union
+{
+    int16u  word[TACTILE_DATA_LENGTH_WORDS_v3];
+    char    string[TACTILE_DATA_LENGTH_BYTES_v3];
+}TACTILE_SENSOR_STATUS_v3;
+
 typedef struct
 {
     int16u  distal[TACTILE_DATA_LENGTH_WORDS_v2];
@@ -242,15 +236,16 @@ typedef struct
 
 
 
-    static const char* tactile_sensor_manufacturer_strings[4] = {   "None",
+    static const char* tactile_sensor_manufacturer_strings[5] = {   "None",
                                                                     "Shadow Robot Company Ltd.",
                                                                     "Syntouch",
-                                                                    "Bielefeld University"};
+                                                                    "Bielefeld University",
+                                                                    "Shadow Robot Company Ltd."};
 
     static const char* tactile_sensor_generic_info_strings[7] = {   "Invalid",
-                                                                    "SVN revision",
+                                                                    "Git revision",
                                                                     "Revision is modified",
-                                                                    "SVN revision on server at build time",
+                                                                    "Git revision on server at build time",
                                                                     "PCB version",
                                                                     "Part serial number low",
                                                                     "Part serial number high"};
diff --git a/sr_robot_lib/include/sr_robot_lib/MST.hpp b/sr_robot_lib/include/sr_robot_lib/MST.hpp
index 1b8d3df2..0c56ba17 100644
--- a/sr_robot_lib/include/sr_robot_lib/MST.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/MST.hpp
@@ -1,4 +1,4 @@
-/* Copyright 2021 Shadow Robot Company Ltd.
+/* Copyright 2021, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -11,10 +11,11 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the MST tactiles.
 */
+/**
+  * @file  MST.hpp
+  * @brief This is a class for accessing the data from the MST tactiles.
+  */
 
 #ifndef _MST_HPP_
 #define _MST_HPP_
@@ -22,56 +23,90 @@
 #include <vector>
 #include <string>
 #include <memory>
-#include <sr_robot_msgs/MSTPalm.h>
+#include <sr_robot_msgs/MSTAll.h>
 
 #include "sr_robot_lib/generic_tactiles.hpp"
 #include "sr_robot_lib/generic_updater.hpp"
 
 namespace tactiles
 {
-
+/** 
+  * Class that contains all functions and member variables relevant to read the MST sensor data.
+  * It inherits from the GenericTactiles class.
+  */  
 template<class StatusType, class CommandType>
 class MST :
         public GenericTactiles<StatusType, CommandType>
 {
 public:
+  /**
+    * MST object constructor.
+    *
+    * @param nh ROS node handle.
+    * @param device_id The device id.
+    * @param update_configs_vector A vector containing information regarding which type of data to poll.
+    * @param update_state Defines current operation state of the sensor (INITIALIZATION or OPERATION).
+    * @param init_tactiles_vector A generic tactile data vector containing sensor information (e.g. Serial Number).
+    */
   MST(ros::NodeHandle nh, std::string device_id, std::vector<generic_updater::UpdateConfig> update_configs_vector,
        operation_mode::device_update_state::DeviceUpdateState update_state,
        boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector);
 
-  /*
-   * This function is called each time a new etherCAT message
-   * is received in the sr08.cpp driver. It  updates the tactile
-   * sensors values contained in sensor_data.
-   *
-   * @param status_data the received etherCAT message
-   */
+  /**
+    * Initialise relevant MST sensor data arrays.
+    */
+  void initialise_tactile_data_structure(boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector);
+
+  /**
+    * Extract the tactile specific data from the incoming EtherCAT status message from the Palm.
+    * And update the tactile data structure.
+    *
+    * @param status_data Is a pointer to an incoming EtherCAT status message from the Palm.
+    */
   virtual void update(StatusType *status_data);
 
-  /*
-   * Publish the information to a ROS topic.
-   *
-   */
+  /**
+    * Publish MST tactile data. 
+    * Not implemented, as this is published from the controller publisher.
+    * But still necessary to keep it in the class, as it maybe be called by the hand drivers
+    */
   virtual void publish();
 
-  /*
-   * This function adds the diagnostics for the tactiles to the
-   * multi diagnostic status published by the hand.
-   */
-  virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
-                               diagnostic_updater::DiagnosticStatusWrapper &d);
+  /**
+    *  This function appends MST specific diagnostic data to the runtime_monitor node of a running Hand.
+    *
+    * @param diagnostic_vector The vector of diagnostic messages which to append the sensor's diagnostic data.
+    * @param diagnostic_status_wrapper A diagnostic status wrapper used to update the sensor's diagnostics with.
+    */
+  virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &diagnostic_vector,
+                               diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status_wrapper);
+
+  /**
+    * Getter for the current MST tactile data
+    * 
+    * @return A vector of tactiles::AllTactileData messages, updated with MST tactile data
+    */
+  virtual std::vector<AllTactileData> *get_tactile_data();
 
 private:
-  sr_robot_msgs::MSTPalm sensor_data;
-  std::shared_ptr<ros::Publisher> publisher;
-  std::vector<GenericTactileData> diagnostic_data;
-
+  /// Stores current MST tactile data
+  sr_robot_msgs::MSTAll sensor_data_;
+  /// Stores MST diagnostics data
+  std::vector<GenericTactileData> diagnostic_data_;
+  /// Status Check "enable flag" found for each sensor
+  int8_t status_check_byte_[5];
+
+  /**
+    * Decode incoming message from the MST tactile sensor, which are sent in chunks of 12 bits
+    * Message is decoded for specified index of the data array. 
+    *
+    * @param buffer Is pointer to array containing incoming tactile data bytes.
+    * @param index Is an index that corresponds the taxel (and channel - X, Y or Z) to be decoded.
+    * @return The decoded integer value of the requested sensor's taxel and channel.
+    */
   int read12bits(char* buffer, int index);
 };  // end class
 
-
-// class template specialization.
-
 /*
 * This is a specialization of the template class to avoid that the compiler try to access non existent fields,
 * as it will compile the class for all the desired combinations of StatusType and CommandType,
@@ -165,8 +200,7 @@ class MST<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_
   ~MST()
   {
   };
-};  // end class
-
+};
 }  // namespace tactiles
 
 #endif  // _MST_HPP_
diff --git a/sr_robot_lib/include/sr_robot_lib/UBI0.hpp b/sr_robot_lib/include/sr_robot_lib/UBI0.hpp
index 46834cf2..4f696caf 100644
--- a/sr_robot_lib/include/sr_robot_lib/UBI0.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/UBI0.hpp
@@ -1,9 +1,4 @@
-/*
-* @file   UBI0.hpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the
-*        Biotac tactiles.
-*
-*
 */
+/**
+  * @file   UBI0.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a class for accessing the data from the
+  *        Biotac tactiles.
+  */
 
 #ifndef _UBI0_HPP_
 #define _UBI0_HPP_
@@ -193,6 +188,36 @@ class UBI0<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE
   };
 };  // end class
 
+template<>
+class UBI0<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND> :
+        public GenericTactiles<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+                ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>
+{
+public:
+  UBI0(ros::NodeHandle nh, std::string device_id, std::vector<generic_updater::UpdateConfig> update_configs_vector,
+       operation_mode::device_update_state::DeviceUpdateState update_state)
+          : GenericTactiles<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+          ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>(nh, device_id, update_configs_vector, update_state)
+  {
+    ROS_WARN("This UBI0 tactile object should not have been instantiated for this type of ETHERCAT_DATA_STRUCTURE. "
+             "Ignore this message if you are using a mockup tactile device.");
+  }
+
+  UBI0(ros::NodeHandle nh, std::string device_id, std::vector<generic_updater::UpdateConfig> update_configs_vector,
+       operation_mode::device_update_state::DeviceUpdateState update_state,
+       boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector)
+          : GenericTactiles<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS,
+          ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>(nh, device_id, update_configs_vector, update_state)
+  {
+    ROS_WARN("This UBI0 tactile object should not have been instantiated for this type of ETHERCAT_DATA_STRUCTURE. "
+             "Ignore this message if you are using a mockup tactile device.");
+  }
+
+  ~UBI0()
+  {
+  };
+};  // end class
+
 }  // namespace tactiles
 
 /* For the emacs weenies in the crowd.
diff --git a/sr_robot_lib/include/sr_robot_lib/generic_tactiles.hpp b/sr_robot_lib/include/sr_robot_lib/generic_tactiles.hpp
index 6f62b1fc..b6743a58 100644
--- a/sr_robot_lib/include/sr_robot_lib/generic_tactiles.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/generic_tactiles.hpp
@@ -1,9 +1,4 @@
-/*
-* @file   generic_tactiles.hpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is the main class for accessing the data from the
-*        tactiles.
-*
-*
 */
+/**
+  * @file   generic_tactiles.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is the main class for accessing the data from the
+  *        tactiles.
+  */
 
 #ifndef GENERIC_TACTILES_HPP_
 #define GENERIC_TACTILES_HPP_
@@ -35,6 +30,7 @@ extern "C"
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h>
 }
 
diff --git a/sr_robot_lib/include/sr_robot_lib/generic_updater.hpp b/sr_robot_lib/include/sr_robot_lib/generic_updater.hpp
index ff315eca..872e8042 100644
--- a/sr_robot_lib/include/sr_robot_lib/generic_updater.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/generic_updater.hpp
@@ -1,9 +1,4 @@
-/*
-* @file   generic_updater.hpp
-* @author toni <toni@shadowrobot.com>
-* @date   20 Oct 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,14 +11,14 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a generic command updater: it has 2 different queues one for
-*        the important data which is updated as fast as possible, one for
-*        unimportant data which is updated on a time basis.
-*
-*
 */
+/** 
+  * @file   generic_updater.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a generic command updater: it has 2 different queues one for
+  *        the important data which is updated as fast as possible, one for
+  *        unimportant data which is updated on a time basis.
+  */
 
 #ifndef GENERIC_UPDATER_HPP_
 #define GENERIC_UPDATER_HPP_
@@ -41,6 +36,7 @@ extern "C"
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h>
 }
 
diff --git a/sr_robot_lib/include/sr_robot_lib/motor_data_checker.hpp b/sr_robot_lib/include/sr_robot_lib/motor_data_checker.hpp
index 89ba2712..7dee0cb0 100644
--- a/sr_robot_lib/include/sr_robot_lib/motor_data_checker.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/motor_data_checker.hpp
@@ -1,9 +1,4 @@
-/*
-* @file   motor_data_checker.hpp
-* @author toni <toni@shadowrobot.com>
-* @date   25 Oct 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,12 +11,12 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class to check that all expected initialization data have been received from each motor.
-*
-*
 */
+/**
+  * @file   motor_data_checker.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a class to check that all expected initialization data have been received from each motor.
+  */
 
 #ifndef MOTOR_DATA_CHECKER_HPP_
 #define MOTOR_DATA_CHECKER_HPP_
@@ -38,6 +33,7 @@ extern "C"
 {
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 }
 
 namespace generic_updater
diff --git a/sr_robot_lib/include/sr_robot_lib/motor_updater.hpp b/sr_robot_lib/include/sr_robot_lib/motor_updater.hpp
index 730bb4d5..ac1c5b47 100644
--- a/sr_robot_lib/include/sr_robot_lib/motor_updater.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/motor_updater.hpp
@@ -1,10 +1,4 @@
-/*
-* @file   motor_updater.hpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>, <contact@shadowrobot.com>
-* @date   Tue Jun  7 09:15:21 2011
-*
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -17,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief  This contains a class used to determin which data we should ask the motor for,
-* depending on the config we're using.
-*
-*
 */
+/**
+  * @file   motor_updater.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>, <contact@shadowrobot.com>
+  * @brief  This contains a class used to determin which data we should ask the motor for,
+  *         depending on the config we're using.
+  */
 
 #ifndef _MOTOR_UPDATER_HPP_
 #define _MOTOR_UPDATER_HPP_
@@ -41,6 +35,7 @@ extern "C"
 {
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 }
 
 namespace generic_updater
diff --git a/sr_robot_lib/include/sr_robot_lib/sensor_updater.hpp b/sr_robot_lib/include/sr_robot_lib/sensor_updater.hpp
index 8511e6e9..50953620 100644
--- a/sr_robot_lib/include/sr_robot_lib/sensor_updater.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/sensor_updater.hpp
@@ -1,9 +1,4 @@
-/*
-* @file   sensor_updater.hpp
-* @author toni <toni@shadowrobot.com>
-* @date   20 Oct 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,11 +11,11 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This class is used to update the tactile sensor command.
-*
-*
+*/
+/**
+  * @file   sensor_updater.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This class is used to update the tactile sensor command.
 */
 
 #ifndef SENSOR_UPDATER_HPP_
@@ -39,6 +34,7 @@ extern "C"
 {
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 }
 
 namespace generic_updater
diff --git a/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp b/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp
index 40befb6e..61bde295 100644
--- a/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp
+++ b/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp
@@ -1,10 +1,4 @@
-/*
-* @file   sr_robot_lib.hpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>
-* @date   Fri Jun  3 12:12:13 2011
-*
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -17,11 +11,12 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @brief This is a generic robot library for Shadow Robot's Hardware.
-*
-*
 */
+/**
+  * @file   sr_robot_lib.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>
+  * @brief This is a generic robot library for Shadow Robot's Hardware.
+  */
 
 #ifndef _SR_ROBOT_LIB_HPP_
 #define _SR_ROBOT_LIB_HPP_
@@ -65,6 +60,7 @@ extern "C"
 #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0240_palm_edc_IMU/0240_palm_edc_IMU_ethercat_protocol.h>
+#include <sr_external_dependencies/external/0250_palm_edc_IMU_MST/0250_palm_edc_IMU_ethercat_protocol.h>
 #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h>
 #include <sr_external_dependencies/external/simplemotor-bootloader/bootloader.h>
 }
diff --git a/sr_robot_lib/src/MST.cpp b/sr_robot_lib/src/MST.cpp
index 7c2ce244..92928b13 100644
--- a/sr_robot_lib/src/MST.cpp
+++ b/sr_robot_lib/src/MST.cpp
@@ -1,4 +1,4 @@
-/* Copyright 2021 Shadow Robot Company Ltd.
+/* Copyright 2021, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -11,10 +11,11 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the MST tactiles.
 */
+/**
+  * @file  MST.cpp
+  * @brief This is a class for accessing the data from the MST tactiles.
+  */
 
 #include "sr_robot_lib/MST.hpp"
 #include <sr_utilities/sr_math_utils.hpp>
@@ -22,119 +23,268 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <ros/console.h>
 
-#define NUMBER_OF_SENSORS 7
+/// The number of sensitive taxels on each MST tactile sensor
+#define NUMBER_OF_TAXELS 17
+#define MAGNETIC_DATA_BYTES       77
+#define TEMPERATURE_DATA_BYTES    27
+#define MAX_STATUS_VALUE 15
 
 namespace tactiles
 {
-  template<class StatusType, class CommandType>
-  MST<StatusType, CommandType>::MST(ros::NodeHandle nh, std::string device_id,
-                                      std::vector<generic_updater::UpdateConfig> update_configs_vector,
-                                      operation_mode::device_update_state::DeviceUpdateState update_state,
-                                      boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector)
-          : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
-  {
-    diagnostic_data = *init_tactiles_vector;
-    publisher = std::make_shared<ros::Publisher>(nh.advertise<sr_robot_msgs::MSTPalm>("mst", 1));
-  }
+/**
+  * MST object constructor.
+  *
+  * @param nh ROS node handle.
+  * @param device_id The device id.
+  * @param update_configs_vector A vector containing information regarding which type of data to poll.
+  * @param update_state Defines current operation state of the sensor (INITIALIZATION or OPERATION).
+  * @param init_tactiles_vector A generic tactile data vector containing sensor information (e.g. Serial Number).
+  */
+template<class StatusType, class CommandType>
+MST<StatusType, CommandType>::MST(ros::NodeHandle nh, std::string device_id,
+                                    std::vector<generic_updater::UpdateConfig> update_configs_vector,
+                                    operation_mode::device_update_state::DeviceUpdateState update_state,
+                                    boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector) :
+  GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
+{
+  diagnostic_data_ = *init_tactiles_vector;
+  initialise_tactile_data_structure(init_tactiles_vector);
+}
 
-  template<class StatusType, class CommandType>
-  int MST<StatusType, CommandType>::read12bits(char* buffer, int index)
+/**
+  * Initialise relevant MST sensor data arrays.
+  */
+template<class StatusType, class CommandType>
+void MST<StatusType, CommandType>::initialise_tactile_data_structure(
+                                          boost::shared_ptr<std::vector<GenericTactileData>> init_tactiles_vector)
+{
+  // Initialise the vector of tactiles
+  this->all_tactile_data =
+                boost::shared_ptr<std::vector<AllTactileData>>(new std::vector<AllTactileData>(this->nb_tactiles));
+
+  for (uint8_t id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
   {
-    int start = index * 3 / 2;
-    if (index % 2 == 0)
+    // For each sensor found, add the type and diagnostics data (freq, sw version, git version, status check)
+    MSTData mst_diagnostics(init_tactiles_vector->at(id_sensor));
+    this->all_tactile_data->at(id_sensor).mst = mst_diagnostics;
+    this->all_tactile_data->at(id_sensor).type = "mst";
+
+    // Resize the tactile data structure to match the number of taxels on each sensor
+    sensor_data_.tactiles[id_sensor].magnetic_data.resize(NUMBER_OF_TAXELS);
+    sensor_data_.tactiles[id_sensor].temperature_data.resize(NUMBER_OF_TAXELS);
+    sensor_data_.tactiles[id_sensor].status = -1;  // Status check is inactive by default
+
+    // Fetch last git_revision byte sent by each sensor (related to status check enable flag) from hex string
+    // For now, this byte is either set to 0x00 (disabled) or 0x01 (enabled)
+    if (diagnostic_data_[id_sensor].git_revision.length() > 2)
+    {
+      std::string git_revision_hex_string_last_byte =
+            diagnostic_data_[id_sensor].git_revision.substr(diagnostic_data_[id_sensor].git_revision.length() - 2);
+      status_check_byte_[id_sensor] = std::stoi(git_revision_hex_string_last_byte, 0, 16);
+      // Update git revision to exclude status check-enable byte
+      diagnostic_data_[id_sensor].git_revision =
+            diagnostic_data_[id_sensor].git_revision.substr(0, diagnostic_data_[id_sensor].git_revision.length() - 2);
+    }
+    else
     {
-      int byte1 = buffer[start];
-      int byte2 = buffer[start + 1];
-      return byte1 << 4 | byte2 >> 4 & 0x0F;
+      // If git_version received is invalid, or no sensor was found for this finger, treat it as status check disbaled
+      status_check_byte_[id_sensor] = 0x00;
     }
-    int byte1 = (int8_t)(buffer[start] << 4);
+  }
+}
+
+/**
+  * Decode incoming message from the MST tactile sensor, which are sent in chunks of 12 bits
+  * Message is decoded for specified index of the data array.
+  *
+  * @param buffer Is pointer to array containing incoming tactile data bytes.
+  * @param index Is an index that corresponds the taxel (and channel - X, Y or Z) to be decoded.
+  * @return The decoded integer value of the requested sensor's taxel and channel.
+  */
+template<class StatusType, class CommandType>
+int MST<StatusType, CommandType>::read12bits(char* buffer, int index)
+{
+  // Finds the correct index of the data in the data buffer
+  int start = index * 3 / 2;
+
+  // If requested index is even, return the current byte, and the first 4 bits of the next byte
+  if (index % 2 == 0)
+  {
+    int byte1 = buffer[start];
     int byte2 = buffer[start + 1];
-    return byte1 << 4 | byte2 & 0xFF;
+
+    return byte1 << 4 | byte2 >> 4 & 0x0F;
   }
+  // If requested index is odd, return the last 4 bits of the current byte, and the next byte
+  int byte1 = (int8_t)(buffer[start] << 4);
+  int byte2 = buffer[start + 1];
+
+  return byte1 << 4 | byte2 & 0xFF;
+}
 
-  template<class StatusType, class CommandType>
-  void MST<StatusType, CommandType>::update(StatusType *status_data)
+/**
+  * Extract the tactile specific data from the incoming EtherCAT status message from the Palm.
+  * And update the tactile data structure.
+  *
+  * @param status_data Is a pointer to an incoming EtherCAT status message from the Palm.
+  */
+template<class StatusType, class CommandType>
+void MST<StatusType, CommandType>::update(StatusType *status_data)
+{
+  // Returns a bit mask describing which fingers contain MST tactile sensors
+  int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
+  for (unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
   {
-    int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
-    for (unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
+    switch (static_cast<int32u>(status_data->tactile_data_type))
     {
-      switch (static_cast<int32u>(status_data->tactile_data_type))
-      {
-        case TACTILE_SENSOR_TYPE_MST_MAGNETIC_INDUCTION:
-          if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
+      case TACTILE_SENSOR_TYPE_MST_MAGNETIC_INDUCTION:
+        // Check if the current sensor (id_sensor) is present in the bit mask
+        if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
+        {
+          for (uint8_t taxel_index = 0; taxel_index < NUMBER_OF_TAXELS; taxel_index++)
+          {
+            geometry_msgs::Point taxel_magnetic_data;
+            // Decode incoming tactile data for each taxel and channel (X, Y or Z)
+            // And build ROS message
+            taxel_magnetic_data.x = read12bits(status_data->tactile[id_sensor].string, taxel_index * 3);
+            taxel_magnetic_data.y = read12bits(status_data->tactile[id_sensor].string, taxel_index * 3 + 1);
+            taxel_magnetic_data.z = read12bits(status_data->tactile[id_sensor].string, taxel_index * 3 + 2);
+            sensor_data_.tactiles[id_sensor].magnetic_data[taxel_index] = taxel_magnetic_data;
+          }
+
+          /// Detects if there was a sensor polling error, i.e. 2 consecutive tactile data pollings
+          /// being less than 900ms apart
+          /// see https://shadowrobot.atlassian.net/wiki/spaces/MST/pages/3401318401/MST+firmware#Communication
+          /// We do this by checking if batches of 12 bits are following a pattern that indicates "repeated bytes"
+          if (sensor_data_.tactiles[id_sensor].magnetic_data[0].x ==
+                                                                sensor_data_.tactiles[id_sensor].magnetic_data[0].z &&
+              sensor_data_.tactiles[id_sensor].magnetic_data[0].x ==
+                                                                sensor_data_.tactiles[id_sensor].magnetic_data[1].y &&
+              sensor_data_.tactiles[id_sensor].magnetic_data[0].y ==
+                                                                sensor_data_.tactiles[id_sensor].magnetic_data[1].x &&
+              sensor_data_.tactiles[id_sensor].magnetic_data[0].y ==
+                                                                sensor_data_.tactiles[id_sensor].magnetic_data[1].z)
+          {
+            sensor_data_.tactiles[id_sensor].status = MAX_STATUS_VALUE + 1;
+          }
+          // If MST sensor has Status Check enabled, retrieve status from latest measurment
+          else if (status_check_byte_[id_sensor] == 0x01)
+          {
+            sensor_data_.tactiles[id_sensor].status =
+                                  (int8_t)status_data->tactile[id_sensor].string[MAGNETIC_DATA_BYTES - 1] & 0x0F;
+          }
+          // Otherwise set status as "disabled", and tactiles messages are published with "status = -1"
+          else
           {
-            for (int i = 0; i < NUMBER_OF_SENSORS; i++)
-            {
-              sensor_data.fingers[id_sensor].sensors[i].magnetic_induction_x = read12bits(
-                status_data->tactile[id_sensor].string, i * 3);
-              sensor_data.fingers[id_sensor].sensors[i].magnetic_induction_y = read12bits(
-                status_data->tactile[id_sensor].string, i * 3 + 1);
-              sensor_data.fingers[id_sensor].sensors[i].magnetic_induction_z = read12bits(
-                status_data->tactile[id_sensor].string, i * 3 + 2);
-            }
+            sensor_data_.tactiles[id_sensor].status = -1;
           }
-          break;
+        }
+        break;
 
-        case TACTILE_SENSOR_TYPE_MST_TEMPERATURE:
-          if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
+      case TACTILE_SENSOR_TYPE_MST_TEMPERATURE:
+        // Check if the current sensor (id_sensor) is present in the bit mask
+        if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
+        {
+          for (uint8_t taxel_index = 0; taxel_index < NUMBER_OF_TAXELS; taxel_index++)
+          {
+            char* tactile_data_pointer = status_data->tactile[id_sensor].string;
+            // Decode incoming tactile data for each taxel; Skiping first byte, as it contains MST's PSoC temperature
+            sensor_data_.tactiles[id_sensor].temperature_data[taxel_index] =
+                (read12bits(++tactile_data_pointer, taxel_index) - 1180) * 0.24 + 25;  // Converts to Celsius degrees
+          }
+          // If MST sensor has Status Check enabled, retrieve status from latest measurment
+          if (status_check_byte_[id_sensor] == 0x01)
+          {
+            sensor_data_.tactiles[id_sensor].status =
+                                (int8_t)status_data->tactile[id_sensor].string[TEMPERATURE_DATA_BYTES - 1] & 0x0F;
+          }
+          else
           {
-            for (int i = 0; i < NUMBER_OF_SENSORS; i++)
-            {
-              // Temperature is send as little-endian value
-              sensor_data.fingers[id_sensor].sensors[i].temperature =
-                status_data->tactile[id_sensor].string[i * 2 + 1] << 8 |
-                (uint8_t)status_data->tactile[id_sensor].string[i * 2];
-            }
+          // Otherwise set status as "disabled", and tactiles messages are published with "status = -1"
+            sensor_data_.tactiles[id_sensor].status = -1;
           }
-          break;
-      }
+        }
+        break;
     }
   }
+}
 
-  template<class StatusType, class CommandType>
-  void MST<StatusType, CommandType>::publish()
-  {
-    sensor_data.header.stamp = ros::Time::now();
-    publisher->publish(sensor_data);
-  }
+/**
+  * Publish MST tactile data.
+  * Not implemented, as this is published from the controller publisher.
+  * But still necessary to keep it in the class, as it maybe be called by the hand drivers
+  */
+template<class StatusType, class CommandType>
+void MST<StatusType, CommandType>::publish()
+{
+  // Left empty, as this is published from the controller publisher
+}
 
-  template<class StatusType, class CommandType>
-  void MST<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
-                                                      diagnostic_updater::DiagnosticStatusWrapper &d)
+/**
+  *  This function appends MST specific diagnostic data to the runtime_monitor node of a running Hand.
+  *
+  * @param diagnostic_vector The vector of diagnostic messages which to append the sensor's diagnostic data.
+  * @param diagnostic_status_wrapper A diagnostic status wrapper, used to update the sensor's diagnostics with
+  */
+template<class StatusType, class CommandType>
+void MST<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &diagnostic_vector,
+                                              diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status_wrapper)
+{
+  for (int id_sensor = 0; id_sensor < this->nb_tactiles; id_sensor++)
   {
-    for (int i = 0; i < this->nb_tactiles; i++)
-    {
-      std::stringstream ss;
-      std::string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
+    std::stringstream ss;
+    std::string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
 
-      ss << prefix << "Tactile " << i + 1;
+    ss << prefix << "Tactile " << id_sensor + 1;
 
-      d.name = ss.str().c_str();
-      d.summary(d.OK, "OK");
-      d.clear();
+    diagnostic_status_wrapper.name = ss.str().c_str();
+    diagnostic_status_wrapper.summary(diagnostic_status_wrapper.OK, "OK");
+    diagnostic_status_wrapper.clear();
 
-      d.addf("Sample Frequency", "%d", diagnostic_data[i].sample_frequency);
-      d.addf("Manufacturer", "%s", diagnostic_data[i].manufacturer.c_str());
-      d.addf("Serial Number", "%s", diagnostic_data[i].serial_number.c_str());
-      d.addf("Software Version", "%s", diagnostic_data[i].git_revision.c_str());
-      d.addf("PCB Version", "%s", diagnostic_data[i].pcb_version.c_str());
+    diagnostic_status_wrapper.addf("Sample Frequency", "%d", diagnostic_data_[id_sensor].sample_frequency);
+    diagnostic_status_wrapper.addf("Manufacturer", "%s", diagnostic_data_[id_sensor].manufacturer.c_str());
+    diagnostic_status_wrapper.addf("Serial Number", "%s", diagnostic_data_[id_sensor].serial_number.c_str());
+    diagnostic_status_wrapper.addf("Software Version", "%s", diagnostic_data_[id_sensor].git_revision.c_str());
+    diagnostic_status_wrapper.addf("PCB Version", "%s", diagnostic_data_[id_sensor].pcb_version.c_str());
+    diagnostic_status_wrapper.addf("Status Enabled", "%s", (status_check_byte_[id_sensor] == 0x01 ? "ON" : "OFF"));
+    diagnostic_status_wrapper.addf("Status", "%d", sensor_data_.tactiles[id_sensor].status);
 
-      vec.push_back(d);
-    }
+    diagnostic_vector.push_back(diagnostic_status_wrapper);
+  }
+}
+
+/**
+  * Getter for the current MST tactile data
+  * 
+  * @return A vector of tactiles::AllTactileData messages, updated with MST tactile data
+  */
+template<class StatusType, class CommandType>
+std::vector<AllTactileData> *MST<StatusType, CommandType>::get_tactile_data()
+{
+  for (unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; id_sensor++)
+  {
+    this->all_tactile_data->at(id_sensor).mst.magnetic_data = sensor_data_.tactiles[id_sensor].magnetic_data;
+    this->all_tactile_data->at(id_sensor).mst.temperature_data = sensor_data_.tactiles[id_sensor].temperature_data;
+    this->all_tactile_data->at(id_sensor).mst.status_check = sensor_data_.tactiles[id_sensor].status;
   }
+  return this->all_tactile_data.get();
+}
 
-  // Only to ensure that the template class is compiled for the types we are interested in
-  template
-  class MST<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
+// Only to ensure that the template class is compiled for the types we are interested in
+template
+class MST<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
 
-  template
-  class MST<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
+template
+class MST<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
 
-  template
-  class MST<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
+template
+class MST<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 
-  template
-  class MST<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
+template
+class MST<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+template
+class MST<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
 }  // namespace tactiles
diff --git a/sr_robot_lib/src/UBI0.cpp b/sr_robot_lib/src/UBI0.cpp
index f5b4cd71..07a6147a 100644
--- a/sr_robot_lib/src/UBI0.cpp
+++ b/sr_robot_lib/src/UBI0.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   UBI0.cpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the
-*        Biotac tactiles.
-*
-*
 */
+/**
+  * @file   UBI0.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a class for accessing the data from the
+  *        Biotac tactiles.
+  */
 
 #include "sr_robot_lib/UBI0.hpp"
 #include <sr_utilities/sr_math_utils.hpp>
@@ -240,6 +235,9 @@ namespace tactiles
   template
   class UBI0<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class UBI0<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
 }  // namespace tactiles
 
 /* For the emacs weenies in the crowd.
diff --git a/sr_robot_lib/src/biotac.cpp b/sr_robot_lib/src/biotac.cpp
index bcc738d8..98caa1f9 100644
--- a/sr_robot_lib/src/biotac.cpp
+++ b/sr_robot_lib/src/biotac.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   biotac.cpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the
-*        Biotac tactiles.
-*
-*
 */
+/**
+  * @file   biotac.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>
+  * @brief This is a class for accessing the data from the
+  *        Biotac tactiles.
+  */
 
 #include "sr_robot_lib/biotac.hpp"
 #include <sr_utilities/sr_math_utils.hpp>
@@ -477,6 +472,9 @@ namespace tactiles
   template
   class Biotac<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class Biotac<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class Biotac<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 }  // namespace tactiles
diff --git a/sr_robot_lib/src/generic_tactiles.cpp b/sr_robot_lib/src/generic_tactiles.cpp
index d430959c..74497049 100644
--- a/sr_robot_lib/src/generic_tactiles.cpp
+++ b/sr_robot_lib/src/generic_tactiles.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   generic_tactiles.cpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,19 +11,21 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is the main class for accessing the data from the
-*        tactiles.
-*
-*
 */
+/**
+  * @file   generic_tactiles.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is the main class for accessing the data from the
+  *        tactiles.
+  */
 
 #include "sr_robot_lib/generic_tactiles.hpp"
 #include <sr_utilities/sr_math_utils.hpp>
 #include <cctype>
 #include <string>
 #include <vector>
+#include <ros/console.h>
+
 
 // NOTE: The length used in this generic tactile class (that is used to obtain common information to determine
 // the actual type of tactile sensors)
@@ -100,6 +97,8 @@ namespace tactiles
               tactiles_vector->at(id_sensor).sample_frequency =
                       static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
             }
+            ROS_DEBUG_STREAM(" tact[" << id_sensor << "]: TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ = " <<
+                            tactiles_vector->at(id_sensor).sample_frequency);
           }
           break;
 
@@ -110,6 +109,8 @@ namespace tactiles
             tactiles_vector->at(id_sensor).manufacturer = sanitise_string(status_data->tactile[id_sensor].string,
                                                                           TACTILE_DATA_LENGTH_BYTES);
           }
+          ROS_DEBUG_STREAM(" tact[" << id_sensor << "]: TACTILE_SENSOR_TYPE_MANUFACTURER = " <<
+                          tactiles_vector->at(id_sensor).manufacturer);
         }
           break;
 
@@ -120,6 +121,8 @@ namespace tactiles
             tactiles_vector->at(id_sensor).serial_number = sanitise_string(status_data->tactile[id_sensor].string,
                                                                            TACTILE_DATA_LENGTH_BYTES);
           }
+          ROS_DEBUG_STREAM(" tact[" << id_sensor << "]: TACTILE_SENSOR_TYPE_SERIAL_NUMBER = " <<
+                          tactiles_vector->at(id_sensor).serial_number);
         }
           break;
 
@@ -130,6 +133,8 @@ namespace tactiles
             {
               tactiles_vector->at(id_sensor).set_software_version(status_data->tactile[id_sensor].string);
             }
+          ROS_DEBUG_STREAM(" tact[" << id_sensor << "]: TACTILE_SENSOR_TYPE_SOFTWARE_VERSION = " <<
+                          status_data->tactile[id_sensor].string);
           }
           break;
 
@@ -141,6 +146,8 @@ namespace tactiles
               tactiles_vector->at(id_sensor).pcb_version = sanitise_string(status_data->tactile[id_sensor].string,
                                                                            TACTILE_DATA_LENGTH_BYTES);
             }
+          ROS_DEBUG_STREAM(" tact[" << id_sensor << "]: TACTILE_SENSOR_TYPE_PCB_VERSION = " <<
+                          tactiles_vector->at(id_sensor).pcb_version);
           }
           break;
 
@@ -249,6 +256,9 @@ namespace tactiles
   template
   class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 }  // namespace tactiles
diff --git a/sr_robot_lib/src/generic_updater.cpp b/sr_robot_lib/src/generic_updater.cpp
index 8263fb80..91fc1b8c 100644
--- a/sr_robot_lib/src/generic_updater.cpp
+++ b/sr_robot_lib/src/generic_updater.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   generic_updater.cpp
-* @author toni <toni@shadowrobot.com>
-* @date   20 Oct 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,14 +11,14 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a generic command updater: it has 2 different queues one for
-*        the important data which is updated as fast as possible, one for
-*        unimportant data which is updated on a time basis.
-*
-*
 */
+/** 
+  * @file   generic_updater.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a generic command updater: it has 2 different queues one for
+  *        the important data which is updated as fast as possible, one for
+  *        unimportant data which is updated on a time basis.
+  */
 
 #include "sr_robot_lib/generic_updater.hpp"
 #include <boost/foreach.hpp>
@@ -94,6 +89,9 @@ namespace generic_updater
   template
   class GenericUpdater<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class GenericUpdater<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class GenericUpdater<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 }  // end namespace generic_updater
diff --git a/sr_robot_lib/src/motor_updater.cpp b/sr_robot_lib/src/motor_updater.cpp
index 0a8c8814..3aacab8e 100644
--- a/sr_robot_lib/src/motor_updater.cpp
+++ b/sr_robot_lib/src/motor_updater.cpp
@@ -1,10 +1,4 @@
-/**
-* @file   motor_updater.cpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>, <contact@shadowrobot.com>
-* @date   Tue Jun  7 09:15:21 2011
-*
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -17,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief  This contains a class used to determin which data we should ask the motor for,
-* depending on the config we're using.
-*
-*
 */
+/**
+  * @file   motor_updater.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>, <contact@shadowrobot.com>
+  * @brief  This contains a class used to determin which data we should ask the motor for,
+  *         depending on the config we're using.
+  */
 
 #include "sr_robot_lib/motor_updater.hpp"
 #include <boost/foreach.hpp>
@@ -154,6 +148,9 @@ namespace generic_updater
 
   template
   class MotorUpdater<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
+
+  template
+  class MotorUpdater<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
 }  // namespace generic_updater
 
 
diff --git a/sr_robot_lib/src/sensor_updater.cpp b/sr_robot_lib/src/sensor_updater.cpp
index f209c349..9faf5e15 100644
--- a/sr_robot_lib/src/sensor_updater.cpp
+++ b/sr_robot_lib/src/sensor_updater.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   sensor_updater.cpp
-* @author toni <toni@shadowrobot.com>
-* @date   20 Oct 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,11 +11,11 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This class is used to update the tactile sensor command.
-*
-*
+*/
+/**
+  * @file   sensor_updater.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This class is used to update the tactile sensor command.
 */
 
 
@@ -150,6 +145,9 @@ namespace generic_updater
   template
   class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 }  // namespace generic_updater
diff --git a/sr_robot_lib/src/shadow_PSTs.cpp b/sr_robot_lib/src/shadow_PSTs.cpp
index c529693e..0dce4630 100644
--- a/sr_robot_lib/src/shadow_PSTs.cpp
+++ b/sr_robot_lib/src/shadow_PSTs.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   shadow_PSTs.cpp
-* @author Toni Oliver <toni@shadowrobot.com>
-* @date   Th Oct 20 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a class for accessing the data from the
-*        PSTs tactiles.
-*
-*
 */
+/** 
+  * @file   shadow_PSTs.hpp
+  * @author Toni Oliver <toni@shadowrobot.com>
+  * @brief This is a class for accessing the data from the
+  *        PSTs tactiles.
+  */
 
 #include "sr_robot_lib/shadow_PSTs.hpp"
 #include <sr_utilities/sr_math_utils.hpp>
@@ -231,6 +226,9 @@ namespace tactiles
   template
   class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 }  // namespace tactiles
diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp
index c3f2b4eb..5be5dc52 100644
--- a/sr_robot_lib/src/sr_motor_hand_lib.cpp
+++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp
@@ -1,10 +1,4 @@
-/**
-* @file   sr_motor_hand_lib.cpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>, <contact@shadowrobot.com>
-* @date   Fri Jun  3 13:05:10 2011
-*
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -17,13 +11,13 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a library for the etherCAT hand.
-* You can find it instantiated in the sr_edc_ethercat_drivers.
-*
-*
 */
+/**
+  * @file   sr_motor_hand_lib.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>
+  * @brief This is a library for the etherCAT hand.
+  *        You can find it instantiated in the sr_edc_ethercat_drivers.
+  */
 
 #include "sr_robot_lib/sr_motor_hand_lib.hpp"
 #include <algorithm>
@@ -570,6 +564,9 @@ namespace shadow_robot
   template
   class SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
 
 }  // namespace shadow_robot
 
diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp
index 3ec373c7..c2add0e6 100644
--- a/sr_robot_lib/src/sr_motor_robot_lib.cpp
+++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp
@@ -1,10 +1,4 @@
-/**
-* @file   sr_motor_robot_lib.cpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>, Toni Oliver <toni@shadowrobot.com>, contact <software@shadowrobot.com>
-* @date   Tue Mar  19 17:12:13 2013
-*
-*
-/* Copyright 2013 Shadow Robot Company Ltd.
+/* Copyright 2013, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -17,12 +11,12 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a generic robot library for Shadow Robot's  motor-actuated Hardware.
-*
-*
 */
+/**
+  * @file   sr_motor_robot_lib.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>, Toni Oliver <toni@shadowrobot.com>, contact <software@shadowrobot.com>
+  * @brief This is a generic robot library for Shadow Robot's motor-actuated Hardware.
+  */
 
 #include "sr_robot_lib/sr_motor_robot_lib.hpp"
 #include <string>
@@ -1358,6 +1352,9 @@ namespace shadow_robot
 
   template
   class SrMotorRobotLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
+
+  template
+  class SrMotorRobotLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
 }  // namespace shadow_robot
 
 /* For the emacs weenies in the crowd.
diff --git a/sr_robot_lib/src/sr_robot_lib.cpp b/sr_robot_lib/src/sr_robot_lib.cpp
index 66b3a23c..fa47fd4d 100644
--- a/sr_robot_lib/src/sr_robot_lib.cpp
+++ b/sr_robot_lib/src/sr_robot_lib.cpp
@@ -1,9 +1,4 @@
-/**
-* @file   sr_robot_lib.cpp
-* @author Ugo Cupcic <ugo@shadowrobot.com>
-* @date   Wed Jun 22 10:06:14 2011
-*
-/* Copyright 2011 Shadow Robot Company Ltd.
+/* Copyright 2011, 2023-2024 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -16,12 +11,12 @@
 *
 * You should have received a copy of the GNU General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-*
-* @brief This is a generic robot library for Shadow Robot's Hardware.
-*
-*
 */
+/**
+  * @file   sr_robot_lib.hpp
+  * @author Ugo Cupcic <ugo@shadowrobot.com>
+  * @brief This is a generic robot library for Shadow Robot's Hardware.
+  */
 
 #include "sr_robot_lib/sr_robot_lib.hpp"
 #include <string>
@@ -678,6 +673,9 @@ namespace shadow_robot
   template
   class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;
 
+  template
+  class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0250_PALM_EDC_COMMAND>;
+
   template
   class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
 
diff --git a/sr_tactile_sensor_controller/CMakeLists.txt b/sr_tactile_sensor_controller/CMakeLists.txt
index a6095fb4..c8e7e788 100644
--- a/sr_tactile_sensor_controller/CMakeLists.txt
+++ b/sr_tactile_sensor_controller/CMakeLists.txt
@@ -27,6 +27,7 @@ include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
 add_library(${PROJECT_NAME} src/sr_tactile_sensor_controller.cpp
                             src/sr_tactile_sensor_publisher.cpp
                             src/sr_pst_tactile_sensor_publisher.cpp
+                            src/sr_mst_tactile_sensor_publisher.cpp
                             src/sr_biotac_tactile_sensor_publisher.cpp
                             src/sr_ubi_tactile_sensor_publisher.cpp)
 add_dependencies(sr_tactile_sensor_controller ${catkin_EXPORTED_TARGETS})
diff --git a/sr_tactile_sensor_controller/include/sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp b/sr_tactile_sensor_controller/include/sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp
new file mode 100644
index 00000000..134b0970
--- /dev/null
+++ b/sr_tactile_sensor_controller/include/sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp
@@ -0,0 +1,80 @@
+/* Copyright 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+  * @file  sr_mst_tactile_sensor_publisher.hpp
+  *
+  * @brief Publish MST tactile state
+  */
+
+
+#pragma once
+
+#include <sr_tactile_sensor_controller/sr_tactile_sensor_publisher.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <sr_robot_msgs/MSTAll.h>
+#include <vector>
+#include <string>
+
+namespace controller
+{
+/** 
+* Class that contains all functions and member variables relevant to publish the MST sensor data.
+* It inherits from the SrTactileSensorPublisher class.
+*/ 
+class SrMSTTactileSensorPublisher: public SrTactileSensorPublisher
+{
+  public:
+    /**
+      * MST tactile sensor publisher constructor.
+      *
+      * @param sensors Is a pointer to the current tactile data to be published
+      * @param publish_rate Defines the data publishing time rate.
+      * @param nh_prefix The prefix of the ROS node handle to be used for publishing.
+      * @param prefix The hand prefix (e.g. "rh_" or "lh_")
+      */
+    SrMSTTactileSensorPublisher(std::vector<tactiles::AllTactileData>* sensors,
+                                double publish_rate, ros::NodeHandle nh_prefix, std::string prefix) :
+      SrTactileSensorPublisher(sensors, publish_rate, nh_prefix, prefix)
+      {
+      }
+
+    /**
+      * Initialize the MST sensor publisher.
+      *
+      * @param time The current process time with wich to initialize last_publish_time_
+      */
+    virtual void init(const ros::Time& time);
+
+    /**
+      * Populate and publish the MST sensor data.
+      *
+      * @param time The current process time.
+      * @param period The time since the last call to update. Not used by this class
+      */
+    virtual void update(const ros::Time& time, const ros::Duration& period);
+
+  private:
+    // ROS realtime publisher of sr_robot_msgs::MSTAll
+    typedef realtime_tools::RealtimePublisher<sr_robot_msgs::MSTAll> MSTPublisher;
+    typedef boost::shared_ptr<MSTPublisher > MSTPublisherPtr;
+    MSTPublisherPtr mst_realtime_pub_;
+};
+}  // namespace controller
+
+/* For the emacs weenies in the crowd.
+Local Variables:
+   c-basic-offset: 2
+End:
+*/
diff --git a/sr_tactile_sensor_controller/src/sr_mst_tactile_sensor_publisher.cpp b/sr_tactile_sensor_controller/src/sr_mst_tactile_sensor_publisher.cpp
new file mode 100644
index 00000000..76ada4ed
--- /dev/null
+++ b/sr_tactile_sensor_controller/src/sr_mst_tactile_sensor_publisher.cpp
@@ -0,0 +1,85 @@
+/* Copyright 2023-2024 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+  * @file  sr_mst_tactile_sensor_publisher.cpp
+  *
+  * @brief Publish MST tactile state
+  */
+
+#include "sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp"
+
+namespace controller
+{
+/**
+  * Initialize the MST sensor publisher.
+  *
+  * @param time The current process time with wich to initialize last_publish_time_
+  */
+void SrMSTTactileSensorPublisher::init(const ros::Time& time)
+{
+  // Tracks the last time the sensors were published (used to enforce the publish rate)
+  last_publish_time_ = time;
+
+  // Instantiate MST realtime publisher object
+  mst_realtime_pub_ = MSTPublisherPtr(
+    new realtime_tools::RealtimePublisher<sr_robot_msgs::MSTAll>(nh_prefix_, "tactile", 4));
+  mst_realtime_pub_->lock();
+  for (unsigned int id_sensor = 0; id_sensor < sensors_->size(); id_sensor++)
+  {
+    mst_realtime_pub_->msg_.tactiles[id_sensor].magnetic_data.resize(
+                                                                  sensors_->at(id_sensor).mst.magnetic_data.size());
+    mst_realtime_pub_->msg_.tactiles[id_sensor].temperature_data.resize(
+                                                                  sensors_->at(id_sensor).mst.temperature_data.size());
+  }
+  mst_realtime_pub_->unlock();
+}
+
+/**
+  * Populate and publish the MST sensor data.
+  *
+  * @param time The current process time.
+  * @param period The time since the last call to update. Not used by this class
+  */
+void SrMSTTactileSensorPublisher::update(const ros::Time& time, const ros::Duration& period)
+{
+  // Limit data publishing rate according to value defined in publish_rate_
+  if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time)
+  {
+    // Publish MST tactile data
+    if (mst_realtime_pub_->trylock())
+    {
+      // Update time in last_publish_time_
+      last_publish_time_ = last_publish_time_ + ros::Duration(1.0 / publish_rate_);
+      mst_realtime_pub_->msg_.header.stamp = time;
+
+      // Populate message to be published
+      for (unsigned int id_sensor = 0; id_sensor < sensors_->size(); id_sensor++)
+      {
+        mst_realtime_pub_->msg_.tactiles[id_sensor].magnetic_data = sensors_->at(id_sensor).mst.magnetic_data;
+        mst_realtime_pub_->msg_.tactiles[id_sensor].temperature_data = sensors_->at(id_sensor).mst.temperature_data;
+        mst_realtime_pub_->msg_.tactiles[id_sensor].status = sensors_->at(id_sensor).mst.status_check;
+        mst_realtime_pub_->msg_.tactiles[id_sensor].timestamp = time;
+      }
+      mst_realtime_pub_->unlockAndPublish();
+    }
+  }
+}
+}  // end namespace controller
+
+/* For the emacs weenies in the crowd.
+Local Variables:
+   c-basic-offset: 2
+End:
+*/
diff --git a/sr_tactile_sensor_controller/src/sr_tactile_sensor_controller.cpp b/sr_tactile_sensor_controller/src/sr_tactile_sensor_controller.cpp
index 7232cdf9..81521cc9 100644
--- a/sr_tactile_sensor_controller/src/sr_tactile_sensor_controller.cpp
+++ b/sr_tactile_sensor_controller/src/sr_tactile_sensor_controller.cpp
@@ -41,6 +41,7 @@
 #include "sr_tactile_sensor_controller/sr_tactile_sensor_controller.hpp"
 #include <pluginlib/class_list_macros.h>
 #include <sr_tactile_sensor_controller/sr_pst_tactile_sensor_publisher.hpp>
+#include <sr_tactile_sensor_controller/sr_mst_tactile_sensor_publisher.hpp>
 #include <sr_tactile_sensor_controller/sr_biotac_tactile_sensor_publisher.hpp>
 #include <sr_tactile_sensor_controller/sr_ubi_tactile_sensor_publisher.hpp>
 #include <string>
@@ -146,6 +147,10 @@ void SrTactileSensorController::update(const ros::Time& time, const ros::Duratio
     {
       sensor_publisher_.reset(new SrUbiTactileSensorPublisher(sensors_, publish_rate_, nh_prefix_, prefix_));
     }
+    else if (sensors_->at(0).type == "mst")
+    {
+      sensor_publisher_.reset(new SrMSTTactileSensorPublisher(sensors_, publish_rate_, nh_prefix_, prefix_));
+    }
     else
     {
       ROS_FATAL_STREAM("Unknown tactile sensor type: " << sensors_->at(0).type);