diff --git a/bl_force_torque_sensor_ros/CMakeLists.txt b/bl_force_torque_sensor_ros/CMakeLists.txt
new file mode 100644
index 000000000..6eb814fc6
--- /dev/null
+++ b/bl_force_torque_sensor_ros/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(bl_force_torque_sensor_ros)
+
+find_package(catkin REQUIRED COMPONENTS
+ message_generation
+)
+
+add_message_files(
+ FILES
+ AmplifierOutputRawArray.msg
+)
+
+generate_messages()
+
+catkin_package(
+ CATKIN_DEPENDS message_runtime
+)
+
+file(GLOB SCRIPT_PROGRAMS node_scripts/*.py)
+catkin_install_python(PROGRAMS ${SCRIPT_PROGRAMS}
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/node_scripts
+)
+
+install(DIRECTORY arduino circuits config launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
diff --git a/bl_force_torque_sensor_ros/README.md b/bl_force_torque_sensor_ros/README.md
new file mode 100644
index 000000000..1b5e7ad79
--- /dev/null
+++ b/bl_force_torque_sensor_ros/README.md
@@ -0,0 +1,59 @@
+# bl_force_torque_sensor_ros
+
+ROS driver package for [BL force torque sensor](https://www.bl-autotec.co.jp/products/index.php?cid=5).
+
+## Circuit
+
+Before using this package, you have to prepare a circuit connecting your sensor and PC.
+
+### Circuit using Arduino and [ADS131M04](https://www.ti.com/product/ADS131M04) as ADC for sensor with RMD amplifier
+
+
+
+## How to install
+
+- Install dependency of `bl_force_torque_sensor_ros` (e.g., by `rosdep install`) and build `bl_force_torque_sensor_ros` (e.g., by `catkin build`)
+ - See http://wiki.ros.org/rosdep#Install_dependency_of_all_packages_in_the_workspace for `rosdep` usage
+
+ For example:
+ ```bash
+ mkdir -p ~/ws_bl_force_torque_sensor_ros/src
+ cd ~/ws_bl_force_torque_sensor_ros/src
+ git clone https://github.com/pazeshun/jsk_3rdparty.git -b bl_force_torque_sensor_ros
+ rosdep install -y -r --ignore-src --from-paths jsk_3rdparty/bl_force_torque_sensor_ros
+ cd ..
+ catkin build bl_force_torque_sensor_ros
+ source ~/ws_bl_force_torque_sensor_ros/devel/setup.bash # Do this every time you open a new terminal
+ ```
+
+- If you prepare a circuit using Arduino,
+ - [Setup Arduino IDE for rosserial](http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup#Install_ros_lib_into_the_Arduino_Environment). Note that you must do it after sourcing `setup.bash` of the workspace including `bl_force_torque_sensor_ros`. If you did it without sourcing correctly, do it again
+ - Upload corresponding firmware to your Arduino. E.g., upload `arduino/sensor_driver_raw_rmd_ads131m04/sensor_driver_raw_rmd_ads131m04.ino` when your circuit uses ADS131M04 for RMD amplifier
+ - The firmware may require additional libraries
+ - `sensor_driver_raw_rmd_ads131m04.ino`: search and install [CRC](https://www.arduino.cc/reference/en/libraries/crc/) on library manager of Arduino IDE
+
+## How to use
+
+To run sample, you have to specify the calibration matrix of your sensor.
+You can pass it via command line, but it may be easier to write and run a simple launch file like:
+```xml:my_sample.launch
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+```
diff --git a/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.cpp b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.cpp
new file mode 100644
index 000000000..d9493e47d
--- /dev/null
+++ b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.cpp
@@ -0,0 +1,537 @@
+/*
+ Based on https://github.com/LucasEtchezuri/Arduino-ADS131M04/blob/ed879ecd0da5ac0d1dfa0494ff65b1e7ad4ae6d5/ADS131M04.cpp
+ We cannot use it as is because it is not correctly compiled on default Arduino IDE targeting original Arduinos (e.g., Arduino Uno, Nano):
+ - It passes arguments to SPI.begin(), while original SPI.begin() does not accept arguments
+ - https://github.com/LucasEtchezuri/Arduino-ADS131M04/issues/1
+ - https://www.arduino.cc/reference/en/language/functions/communication/spi/begin/
+ - It operates bit shift greater than variable width. This causes undefined behavior
+ - https://github.com/LucasEtchezuri/Arduino-ADS131M04/issues/1
+ - https://mfumi.hatenadiary.org/entry/20111217/1324137736
+ Additional changes:
+ - Call SPI.beginTransaction() and SPI.endTransaction() every time to accept other SPI devices
+ - https://stupiddog.jp/note/archives/976
+ - Set chip select (CS) high in begin() to accept other SPI devices
+ - Fix some typos & Spanish comments
+ - Use CRC to detect SPI communication error
+ - Enable to change SPI speed
+*/
+
+#include "Arduino.h"
+#include "ADS131M04.h"
+#include "SPI.h"
+#include "CRC.h" // Search and install CRC on library manager of Arduino IDE
+// https://www.arduino.cc/reference/en/libraries/crc/
+
+ADS131M04::ADS131M04()
+{
+}
+
+uint8_t ADS131M04::writeRegister(uint8_t address, uint16_t value)
+{
+ uint16_t res;
+ uint8_t addressRcv;
+ uint8_t bytesRcv;
+ uint16_t cmd = 0;
+
+ SPI.beginTransaction(SPI_SETTINGS);
+ digitalWrite(ADS131M04_CS_PIN, LOW);
+ delayMicroseconds(1);
+
+ cmd = (CMD_WRITE_REG) | ((uint16_t)address << 7) | 0;
+
+ //res = SPI.transfer16(cmd);
+ SPI.transfer16(cmd);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(value);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ res = SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ delayMicroseconds(1);
+ digitalWrite(ADS131M04_CS_PIN, HIGH);
+ SPI.endTransaction();
+
+ addressRcv = (res & REGMASK_CMD_READ_REG_ADDRESS) >> 7;
+ bytesRcv = (res & REGMASK_CMD_READ_REG_BYTES);
+
+ if (addressRcv == address)
+ {
+ return bytesRcv + 1;
+ }
+ return 0;
+}
+
+void ADS131M04::writeRegisterMasked(uint8_t address, uint16_t value, uint16_t mask)
+{
+ // Write a value to the register, applying the mask to touch only the necessary bits.
+ // It does not perform the bit shift, the shifted value must be moved to the correct position.
+
+ // Read the current contents of the register
+ uint16_t register_contents = readRegister(address);
+
+ // Bit by bit change of the mask (1 in the bits not to be touched and 0 in the bits to be modified).
+ // AND is performed on the current contents of the register. There are "0" in the part to be modified.
+ register_contents = register_contents & ~mask;
+
+ // OR is performed with the value to be loaded into the register. Note that the value must be in the correct position (shitf).
+ register_contents = register_contents | value;
+
+ // Rewrite the register
+ writeRegister(address, register_contents);
+}
+
+uint16_t ADS131M04::readRegister(uint8_t address)
+{
+ uint16_t cmd;
+ uint16_t data;
+
+ cmd = CMD_READ_REG | ((uint16_t)address << 7 | 0);
+
+ SPI.beginTransaction(SPI_SETTINGS);
+ digitalWrite(ADS131M04_CS_PIN, LOW);
+ delayMicroseconds(1);
+
+ //data = SPI.transfer16(cmd);
+ SPI.transfer16(cmd);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ data = SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ SPI.transfer16(0x0000);
+ SPI.transfer(0x00);
+
+ delayMicroseconds(1);
+ digitalWrite(ADS131M04_CS_PIN, HIGH);
+ SPI.endTransaction();
+ return data;
+}
+
+void ADS131M04::begin(uint8_t cs_pin, uint8_t drdy_pin, uint32_t spi_speed)
+{
+ // Set pins up
+ ADS131M04_CS_PIN = cs_pin;
+ ADS131M04_DRDY_PIN = drdy_pin;
+
+ SPI.begin();
+ // Configure chip select as an output
+ pinMode(ADS131M04_CS_PIN, OUTPUT);
+ // Set chip select high to accept other SPI devices
+ digitalWrite(ADS131M04_CS_PIN, HIGH);
+ delayMicroseconds(1); // Should be longer than t_w(CSH)?
+ // Configure DRDY as an input
+ pinMode(ADS131M04_DRDY_PIN, INPUT);
+
+ // Store SPI settings
+ SPI_SETTINGS = SPISettings(spi_speed, MSBFIRST, SPI_MODE1);
+}
+
+int8_t ADS131M04::isDataReadySoft(byte channel)
+{
+ if (channel == 0)
+ {
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_DRDY0);
+ }
+ else if (channel == 1)
+ {
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_DRDY1);
+ }
+ else if (channel == 2)
+ {
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_DRDY2);
+ }
+ else if (channel == 3)
+ {
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_DRDY3);
+ }
+ else
+ {
+ return -1;
+ }
+}
+
+bool ADS131M04::isResetStatus(void)
+{
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_RESET);
+}
+
+bool ADS131M04::isLockSPI(void)
+{
+ return (readRegister(REG_STATUS) & REGMASK_STATUS_LOCK);
+}
+
+bool ADS131M04::setDrdyFormat(uint8_t drdyFormat)
+{
+ if (drdyFormat > 1)
+ {
+ return false;
+ }
+ else
+ {
+ writeRegisterMasked(REG_MODE, drdyFormat, REGMASK_MODE_DRDY_FMT);
+ return true;
+ }
+}
+
+bool ADS131M04::setDrdyStateWhenUnavailable(uint8_t drdyState)
+{
+ if (drdyState > 1)
+ {
+ return false;
+ }
+ else
+ {
+ writeRegisterMasked(REG_MODE, drdyState < 1, REGMASK_MODE_DRDY_HiZ);
+ return true;
+ }
+}
+
+bool ADS131M04::setPowerMode(uint8_t powerMode)
+{
+ if (powerMode > 3)
+ {
+ return false;
+ }
+ else
+ {
+ writeRegisterMasked(REG_CLOCK, powerMode, REGMASK_CLOCK_PWR);
+ return true;
+ }
+}
+
+bool ADS131M04::setOsr(uint16_t osr)
+{
+ if (osr > 7)
+ {
+ return false;
+ }
+ else
+ {
+ writeRegisterMasked(REG_CLOCK, osr << 2 , REGMASK_CLOCK_OSR);
+ return true;
+ }
+}
+
+bool ADS131M04::setChannelEnable(uint8_t channel, uint16_t enable)
+{
+ if (channel > 3)
+ {
+ return false;
+ }
+ if (channel == 0)
+ {
+ writeRegisterMasked(REG_CLOCK, enable << 8, REGMASK_CLOCK_CH0_EN);
+ return true;
+ }
+ else if (channel == 1)
+ {
+ writeRegisterMasked(REG_CLOCK, enable << 9, REGMASK_CLOCK_CH1_EN);
+ return true;
+ }
+ else if (channel == 2)
+ {
+ writeRegisterMasked(REG_CLOCK, enable << 10, REGMASK_CLOCK_CH2_EN);
+ return true;
+ }
+ else if (channel == 3)
+ {
+ writeRegisterMasked(REG_CLOCK, enable << 11, REGMASK_CLOCK_CH3_EN);
+ return true;
+ }
+}
+
+bool ADS131M04::setChannelPGA(uint8_t channel, uint16_t pga)
+{
+ if (channel > 3)
+ {
+ return false;
+ }
+ if (channel == 0)
+ {
+ writeRegisterMasked(REG_GAIN, pga, REGMASK_GAIN_PGAGAIN0);
+ return true;
+ }
+ else if (channel == 1)
+ {
+ writeRegisterMasked(REG_GAIN, pga << 4, REGMASK_GAIN_PGAGAIN1);
+ return true;
+ }
+ else if (channel == 2)
+ {
+ writeRegisterMasked(REG_GAIN, pga << 8, REGMASK_GAIN_PGAGAIN2);
+ return true;
+ }
+ else if (channel == 3)
+ {
+ writeRegisterMasked(REG_GAIN, pga << 12, REGMASK_GAIN_PGAGAIN3);
+ return true;
+ }
+}
+
+void ADS131M04::setGlobalChop(uint16_t global_chop)
+{
+ writeRegisterMasked(REG_CFG, global_chop << 8, REGMASK_CFG_GC_EN);
+}
+
+void ADS131M04::setGlobalChopDelay(uint16_t delay)
+{
+ writeRegisterMasked(REG_CFG, delay << 9, REGMASK_CFG_GC_DLY);
+}
+
+bool ADS131M04::setInputChannelSelection(uint8_t channel, uint8_t input)
+{
+ if (channel > 3)
+ {
+ return false;
+ }
+ if (channel == 0)
+ {
+ writeRegisterMasked(REG_CH0_CFG, input, REGMASK_CHX_CFG_MUX);
+ return true;
+ }
+ else if (channel == 1)
+ {
+ writeRegisterMasked(REG_CH1_CFG, input, REGMASK_CHX_CFG_MUX);
+ return true;
+ }
+ else if (channel == 2)
+ {
+ writeRegisterMasked(REG_CH2_CFG, input, REGMASK_CHX_CFG_MUX);
+ return true;
+ }
+ else if (channel == 3)
+ {
+ writeRegisterMasked(REG_CH3_CFG, input, REGMASK_CHX_CFG_MUX);
+ return true;
+ }
+}
+
+bool ADS131M04::setChannelOffsetCalibration(uint8_t channel, int32_t offset)
+{
+
+ uint16_t MSB = offset >> 8;
+ uint8_t LSB = offset;
+
+ if (channel > 3)
+ {
+ return false;
+ }
+ if (channel == 0)
+ {
+ writeRegisterMasked(REG_CH0_OCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH0_OCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_OCAL0_LSB);
+ return true;
+ }
+ else if (channel == 1)
+ {
+ writeRegisterMasked(REG_CH1_OCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH1_OCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_OCAL0_LSB);
+ return true;
+ }
+ else if (channel == 2)
+ {
+ writeRegisterMasked(REG_CH2_OCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH2_OCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_OCAL0_LSB);
+ return true;
+ }
+ else if (channel == 3)
+ {
+ writeRegisterMasked(REG_CH3_OCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH3_OCAL_LSB, (uint16_t)LSB << 8 , REGMASK_CHX_OCAL0_LSB);
+ return true;
+ }
+}
+
+bool ADS131M04::setChannelGainCalibration(uint8_t channel, uint32_t gain)
+{
+
+ uint16_t MSB = gain >> 8;
+ uint8_t LSB = gain;
+
+ if (channel > 3)
+ {
+ return false;
+ }
+ if (channel == 0)
+ {
+ writeRegisterMasked(REG_CH0_GCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH0_GCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_GCAL0_LSB);
+ return true;
+ }
+ else if (channel == 1)
+ {
+ writeRegisterMasked(REG_CH1_GCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH1_GCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_GCAL0_LSB);
+ return true;
+ }
+ else if (channel == 2)
+ {
+ writeRegisterMasked(REG_CH2_GCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH2_GCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_GCAL0_LSB);
+ return true;
+ }
+ else if (channel == 3)
+ {
+ writeRegisterMasked(REG_CH3_GCAL_MSB, MSB, 0xFFFF);
+ writeRegisterMasked(REG_CH3_GCAL_LSB, (uint16_t)LSB << 8, REGMASK_CHX_GCAL0_LSB);
+ return true;
+ }
+}
+
+bool ADS131M04::isDataReady()
+{
+ if (digitalRead(ADS131M04_DRDY_PIN) == HIGH)
+ {
+ return false;
+ }
+ return true;
+}
+
+bool ADS131M04::readADC(adcOutput* reading)
+{
+ uint32_t x = 0;
+ uint32_t x2 = 0;
+ uint32_t x3 = 0;
+ uint8_t crc_inputs[15];
+ uint16_t crc_received = 0;
+ int32_t aux;
+
+ SPI.beginTransaction(SPI_SETTINGS);
+ digitalWrite(ADS131M04_CS_PIN, LOW);
+ delayMicroseconds(1);
+
+ x = crc_inputs[0] = SPI.transfer(0x00);
+ x2 = crc_inputs[1] = SPI.transfer(0x00);
+ crc_inputs[2] = SPI.transfer(0x00);
+
+ reading->status = ((x << 8) | x2);
+
+ x = crc_inputs[3] = SPI.transfer(0x00);
+ x2 = crc_inputs[4] = SPI.transfer(0x00);
+ x3 = crc_inputs[5] = SPI.transfer(0x00);
+
+ aux = (((x << 16) | (x2 << 8) | x3) & 0x00FFFFFF);
+ if (aux > 0x7FFFFF)
+ {
+ reading->ch0 = ((~(aux)&0x00FFFFFF) + 1) * -1;
+ }
+ else
+ {
+ reading->ch0 = aux;
+ }
+
+ x = crc_inputs[6] = SPI.transfer(0x00);
+ x2 = crc_inputs[7] = SPI.transfer(0x00);
+ x3 = crc_inputs[8] = SPI.transfer(0x00);
+
+ aux = (((x << 16) | (x2 << 8) | x3) & 0x00FFFFFF);
+ if (aux > 0x7FFFFF)
+ {
+ reading->ch1 = ((~(aux)&0x00FFFFFF) + 1) * -1;
+ }
+ else
+ {
+ reading->ch1 = aux;
+ }
+
+ x = crc_inputs[9] = SPI.transfer(0x00);
+ x2 = crc_inputs[10] = SPI.transfer(0x00);
+ x3 = crc_inputs[11] = SPI.transfer(0x00);
+
+ aux = (((x << 16) | (x2 << 8) | x3) & 0x00FFFFFF);
+ if (aux > 0x7FFFFF)
+ {
+ reading->ch2 = ((~(aux)&0x00FFFFFF) + 1) * -1;
+ }
+ else
+ {
+ reading->ch2 = aux;
+ }
+
+ x = crc_inputs[12] = SPI.transfer(0x00);
+ x2 = crc_inputs[13] = SPI.transfer(0x00);
+ x3 = crc_inputs[14] = SPI.transfer(0x00);
+
+ aux = (((x << 16) | (x2 << 8) | x3) & 0x00FFFFFF);
+ if (aux > 0x7FFFFF)
+ {
+ reading->ch3 = ((~(aux)&0x00FFFFFF) + 1) * -1;
+ }
+ else
+ {
+ reading->ch3 = aux;
+ }
+
+ x = SPI.transfer(0x00);
+ x2 = SPI.transfer(0x00);
+ SPI.transfer(0x00);
+ crc_received = ((x << 8) | x2);
+
+ delayMicroseconds(1);
+ digitalWrite(ADS131M04_CS_PIN, HIGH);
+ SPI.endTransaction();
+
+ if (crc_received != crc16_CCITT(crc_inputs, 15))
+ {
+ // CRC error (SPI communication error)
+ return false;
+ }
+
+ return true;
+}
diff --git a/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.h b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.h
new file mode 100644
index 000000000..8584c48b8
--- /dev/null
+++ b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/ADS131M04.h
@@ -0,0 +1,274 @@
+/*
+ Based on https://github.com/LucasEtchezuri/Arduino-ADS131M04/blob/ed879ecd0da5ac0d1dfa0494ff65b1e7ad4ae6d5/ADS131M04.h
+ See ADS131M04.cpp for detailed comments about modification.
+*/
+
+#ifndef ADS131M04_h
+#define ADS131M04_h
+
+#include "Arduino.h"
+#include "SPI.h"
+
+struct adcOutput
+{
+ uint16_t status;
+ int32_t ch0;
+ int32_t ch1;
+ int32_t ch2;
+ int32_t ch3;
+};
+
+#define DRDY_STATE_LOGIC_HIGH 0 // Default
+#define DRDY_STATE_HI_Z 1
+
+#define POWER_MODE_VERY_LOW_POWER 0
+#define POWER_MODE_LOW_POWER 1
+#define POWER_MODE_HIGH_RESOLUTION 2 // Default
+
+#define CHANNEL_PGA_1 0
+#define CHANNEL_PGA_2 1
+#define CHANNEL_PGA_4 2
+#define CHANNEL_PGA_8 3
+#define CHANNEL_PGA_16 4
+#define CHANNEL_PGA_32 5
+#define CHANNEL_PGA_64 6
+#define CHANNEL_PGA_128 7
+
+#define INPUT_CHANNEL_MUX_AIN0P_AIN0N 0 // Default
+#define INPUT_CHANNEL_MUX_INPUT_SHORTED 1
+#define INPUT_CHANNEL_MUX_POSITIVE_DC_TEST_SIGNAL 2
+#define INPUT_CHANNEL_MUX_NEGATIVE_DC_TEST_SIGNAL 3
+
+#define OSR_128 0
+#define OSR_256 1
+#define OSR_512 2
+#define OSR_1024 3 // Default
+#define OSR_2018 4
+#define OSR_4096 5
+#define OSR_8192 6
+#define OSR_16384 7
+
+
+// Commands
+#define CMD_NULL 0x0000 // This command gives the STATUS REGISTER
+#define CMD_RESET 0x0011
+#define CMD_STANDBY 0x0022
+#define CMD_WAKEUP 0x0033
+#define CMD_LOCK 0x0555
+#define CMD_UNLOCK 0x0655
+#define CMD_READ_REG 0xA000 // 101a aaaa annn nnnn a=address n=number of registers-1
+#define CMD_WRITE_REG 0x6000
+
+// Responses
+#define RSP_RESET_OK 0xFF24
+#define RSP_RESET_NOK 0x0011
+
+// Registers Read Only
+#define REG_ID 0x00
+#define REG_STATUS 0x01
+
+// Registers Global Settings across channels
+#define REG_MODE 0x02
+#define REG_CLOCK 0x03
+#define REG_GAIN 0x04
+#define REG_CFG 0x06
+#define THRSHLD_MSB 0x07
+#define THRSHLD_LSB 0x08
+
+// Registers Channel 0 Specific
+#define REG_CH0_CFG 0x09
+#define REG_CH0_OCAL_MSB 0x0A
+#define REG_CH0_OCAL_LSB 0x0B
+#define REG_CH0_GCAL_MSB 0x0C
+#define REG_CH0_GCAL_LSB 0x0D
+
+// Registers Channel 1 Specific
+#define REG_CH1_CFG 0x0E
+#define REG_CH1_OCAL_MSB 0x0F
+#define REG_CH1_OCAL_LSB 0x10
+#define REG_CH1_GCAL_MSB 0x11
+#define REG_CH1_GCAL_LSB 0x12
+
+// Registers Channel 2 Specific
+#define REG_CH2_CFG 0x13
+#define REG_CH2_OCAL_MSB 0x14
+#define REG_CH2_OCAL_LSB 0x15
+#define REG_CH2_GCAL_MSB 0x16
+#define REG_CH2_GCAL_LSB 0x17
+
+// Registers Channel 3 Specific
+#define REG_CH3_CFG 0x18
+#define REG_CH3_OCAL_MSB 0x19
+#define REG_CH3_OCAL_LSB 0x1A
+#define REG_CH3_GCAL_MSB 0x1B
+#define REG_CH3_GCAL_LSB 0x1C
+
+// Registers MAP CRC
+#define REG_MAP_CRC 0x3E
+
+// ------------------------------------------------------------------------------------
+
+// Mask READ_REG
+#define REGMASK_CMD_READ_REG_ADDRESS 0x1F80
+#define REGMASK_CMD_READ_REG_BYTES 0x007F
+
+// Mask Register STATUS
+#define REGMASK_STATUS_LOCK 0x8000
+#define REGMASK_STATUS_RESYNC 0x4000
+#define REGMASK_STATUS_REGMAP 0x2000
+#define REGMASK_STATUS_CRC_ERR 0x1000
+#define REGMASK_STATUS_CRC_TYPE 0x0800
+#define REGMASK_STATUS_RESET 0x0400
+#define REGMASK_STATUS_WLENGTH 0x0300
+#define REGMASK_STATUS_DRDY3 0x0008
+#define REGMASK_STATUS_DRDY2 0x0004
+#define REGMASK_STATUS_DRDY1 0x0002
+#define REGMASK_STATUS_DRDY0 0x0001
+
+// Mask Register MODE
+#define REGMASK_MODE_REG_CRC_EN 0x2000
+#define REGMASK_MODE_RX_CRC_EN 0x1000
+#define REGMASK_MODE_CRC_TYPE 0x0800
+#define REGMASK_MODE_RESET 0x0400
+#define REGMASK_MODE_WLENGTH 0x0300
+#define REGMASK_MODE_TIMEOUT 0x0010
+#define REGMASK_MODE_DRDY_SEL 0x000C
+#define REGMASK_MODE_DRDY_HiZ 0x0002
+#define REGMASK_MODE_DRDY_FMT 0x0001
+
+// Mask Register CLOCK
+#define REGMASK_CLOCK_CH3_EN 0x0800
+#define REGMASK_CLOCK_CH2_EN 0x0400
+#define REGMASK_CLOCK_CH1_EN 0x0200
+#define REGMASK_CLOCK_CH0_EN 0x0100
+#define REGMASK_CLOCK_OSR 0x001C
+#define REGMASK_CLOCK_PWR 0x0003
+
+// Mask Register GAIN
+#define REGMASK_GAIN_PGAGAIN3 0x7000
+#define REGMASK_GAIN_PGAGAIN2 0x0700
+#define REGMASK_GAIN_PGAGAIN1 0x0070
+#define REGMASK_GAIN_PGAGAIN0 0x0007
+
+// Mask Register CFG
+#define REGMASK_CFG_GC_DLY 0x1E00
+#define REGMASK_CFG_GC_EN 0x0100
+#define REGMASK_CFG_CD_ALLCH 0x0080
+#define REGMASK_CFG_CD_NUM 0x0070
+#define REGMASK_CFG_CD_LEN 0x000E
+#define REGMASK_CFG_CD_EN 0x0001
+
+// Mask Register THRSHLD_LSB
+#define REGMASK_THRSHLD_LSB_CD_TH_LSB 0xFF00
+#define REGMASK_THRSHLD_LSB_DCBLOCK 0x000F
+
+// Mask Register CHX_CFG
+#define REGMASK_CHX_CFG_PHASE 0xFFC0
+#define REGMASK_CHX_CFG_DCBLKX_DIS0 0x0004
+#define REGMASK_CHX_CFG_MUX 0x0003
+
+// Mask Register CHX_OCAL_LSB
+#define REGMASK_CHX_OCAL0_LSB 0xFF00
+
+// Mask Register CHX_GCAL_LSB
+#define REGMASK_CHX_GCAL0_LSB 0xFF00
+
+// --------------------------------------------------------------------
+
+// Conversion modes
+#define CONVERSION_MODE_CONT 0
+#define CONVERSION_MODE_SINGLE 1
+
+// Data Format
+#define DATA_FORMAT_TWO_COMPLEMENT 0
+#define DATA_FORMAT_BINARY 1
+
+// Measure Mode
+#define MEASURE_UNIPOLAR 1
+#define MEASURE_BIPOLAR 0
+
+// Clock Type
+#define CLOCK_EXTERNAL 1
+#define CLOCK_INTERNAL 0
+
+// PGA Gain
+#define PGA_GAIN_1 0
+#define PGA_GAIN_2 1
+#define PGA_GAIN_4 2
+#define PGA_GAIN_8 3
+#define PGA_GAIN_16 4
+#define PGA_GAIN_32 5
+#define PGA_GAIN_64 6
+#define PGA_GAIN_128 7
+
+// Input Filter
+#define FILTER_SINC 0
+#define FILTER_FIR 2
+#define FILTER_FIR_IIR 3
+
+// Data Mode
+#define DATA_MODE_24BITS 0
+#define DATA_MODE_32BITS 1
+
+// Data Rate
+#define DATA_RATE_0 0
+#define DATA_RATE_1 1
+#define DATA_RATE_2 2
+#define DATA_RATE_3 3
+#define DATA_RATE_4 4
+#define DATA_RATE_5 5
+#define DATA_RATE_6 6
+#define DATA_RATE_7 7
+#define DATA_RATE_8 8
+#define DATA_RATE_9 9
+#define DATA_RATE_10 10
+#define DATA_RATE_11 11
+#define DATA_RATE_12 12
+#define DATA_RATE_13 13
+#define DATA_RATE_14 14
+#define DATA_RATE_15 15
+
+// Sync Mode
+#define SYNC_CONTINUOUS 1
+#define SYNC_PULSE 0
+
+// DIO Config Mode
+#define DIO_OUTPUT 1
+#define DIO_INPUT 0
+
+#define SPI_MASTER_DUMMY 0xFF
+#define SPI_MASTER_DUMMY16 0xFFFF
+#define SPI_MASTER_DUMMY32 0xFFFFFFFF
+
+class ADS131M04
+{
+public:
+ ADS131M04();
+ uint8_t ADS131M04_CS_PIN;
+ uint8_t ADS131M04_DRDY_PIN;
+ SPISettings SPI_SETTINGS;
+
+ void begin(uint8_t cs_pin, uint8_t drdy_pin, uint32_t spi_speed = 4000000);
+ int8_t isDataReadySoft(byte channel);
+ bool isDataReady(void);
+ bool isResetStatus(void);
+ bool isLockSPI(void);
+ bool setDrdyFormat(uint8_t drdyFormat);
+ bool setDrdyStateWhenUnavailable(uint8_t drdyState);
+ bool setPowerMode(uint8_t powerMode);
+ bool setChannelEnable(uint8_t channel, uint16_t enable);
+ bool setChannelPGA(uint8_t channel, uint16_t pga);
+ void setGlobalChop(uint16_t global_chop);
+ void setGlobalChopDelay(uint16_t delay);
+ bool setInputChannelSelection(uint8_t channel, uint8_t input);
+ bool setChannelOffsetCalibration(uint8_t channel, int32_t offset);
+ bool setChannelGainCalibration(uint8_t channel, uint32_t gain);
+ bool setOsr(uint16_t osr);
+ bool readADC(adcOutput* reading);
+
+private:
+ uint8_t writeRegister(uint8_t address, uint16_t value);
+ void writeRegisterMasked(uint8_t address, uint16_t value, uint16_t mask);
+ uint16_t readRegister(uint8_t address);
+};
+#endif
diff --git a/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/sensor_driver_raw_rmd_ads131m04.ino b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/sensor_driver_raw_rmd_ads131m04.ino
new file mode 100644
index 000000000..48454d301
--- /dev/null
+++ b/bl_force_torque_sensor_ros/arduino/sensor_driver_raw_rmd_ads131m04/sensor_driver_raw_rmd_ads131m04.ino
@@ -0,0 +1,109 @@
+#include
+
+#include
+#include
+#include
+
+#include "ADS131M04.h"
+
+#define ADC_NUM 2
+#define VALID_CH_NUM 3
+ADS131M04 adc[ADC_NUM];
+adcOutput adc_res[ADC_NUM];
+
+unsigned long loop_duration = 0; // loop duration in us, set via rostopic callback
+unsigned long start_time;
+
+ros::NodeHandle nh;
+bl_force_torque_sensor_ros::AmplifierOutputRawArray output_msg;
+ros::Publisher output_pub("~output", &output_msg);
+int32_t output_array[ADC_NUM * VALID_CH_NUM];
+
+void durationCb(const std_msgs::Float32& duration_msg)
+{
+ loop_duration = duration_msg.data * 1000 * 1000; // sec -> us
+}
+
+ros::Subscriber duration_sub("~parameter_getter/loop_duration", &durationCb);
+
+void setup()
+{
+ nh.getHardware()->setBaud(250000);
+ // cf. https://arduino.stackexchange.com/questions/296/how-high-of-a-baud-rate-can-i-go-without-errors
+ // http://wormfood.net/avrbaudcalc.php
+ // I'm not sure why, but 500000 causes "Mismatched protocol version" error
+ nh.initNode();
+ nh.advertise(output_pub);
+ nh.subscribe(duration_sub);
+ while (!nh.connected())
+ {
+ nh.spinOnce();
+ }
+
+ adc[0].begin(9, 7, 4000000);
+ adc[1].begin(8, 6, 4000000); // I'm not sure why, but using D10 as CS doesn't work. Conflicted with SPI.begin()?
+}
+
+void loop()
+{
+ start_time = micros();
+
+ // Wait until data is ready
+ bool is_ok = false;
+ while (!is_ok)
+ {
+ is_ok = true;
+ for (int i = 0; i < ADC_NUM; i++)
+ {
+ if (!adc[i].isDataReady())
+ {
+ is_ok = false;
+ }
+ }
+ }
+
+ is_ok = true;
+ for (int i = 0; i < ADC_NUM; i++)
+ {
+ if (!adc[i].readADC(&adc_res[i]))
+ {
+ is_ok = false;
+ }
+ delayMicroseconds(1); // Should be longer than t_w(CSH)?
+ }
+ for (int i = 0; i < ADC_NUM; i++)
+ {
+ if (adc_res[i].status != 0x050F)
+ {
+ // status is value of STATUS register.
+ // By default, if everything is OK, this value is 0000 0101 0000 1111 (0x050F)
+ is_ok = false;
+ }
+ }
+ if (is_ok)
+ {
+ for (int i = 0; i < ADC_NUM; i++)
+ {
+ output_array[i * VALID_CH_NUM] = adc_res[i].ch0;
+ output_array[i * VALID_CH_NUM + 1] = adc_res[i].ch1;
+ output_array[i * VALID_CH_NUM + 2] = adc_res[i].ch2;
+ }
+ output_msg.array = output_array;
+ output_msg.array_length = ADC_NUM * VALID_CH_NUM;
+ output_pub.publish(&output_msg);
+ }
+
+ nh.spinOnce();
+ // Enforce constant loop time
+ if ((ULONG_MAX - start_time) < loop_duration)
+ {
+ // micros() will overflow while waiting
+ while (micros() >= start_time || micros() < (loop_duration - (ULONG_MAX - start_time)));
+ }
+ else if (micros() < start_time); // micros() already overflowed unexpectedly
+ else
+ {
+ // Normal waiting
+ while ((micros() - start_time) < loop_duration);
+ }
+}
diff --git a/bl_force_torque_sensor_ros/circuits/rmd_ads131m04_arduino/schematic.jpg b/bl_force_torque_sensor_ros/circuits/rmd_ads131m04_arduino/schematic.jpg
new file mode 100644
index 000000000..420c7cc8e
Binary files /dev/null and b/bl_force_torque_sensor_ros/circuits/rmd_ads131m04_arduino/schematic.jpg differ
diff --git a/bl_force_torque_sensor_ros/config/rviz/sample.rviz b/bl_force_torque_sensor_ros/config/rviz/sample.rviz
new file mode 100644
index 000000000..777d94b63
--- /dev/null
+++ b/bl_force_torque_sensor_ros/config/rviz/sample.rviz
@@ -0,0 +1,134 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5
+ Tree Height: 549
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Arrow Width: 0.5
+ Class: rviz/WrenchStamped
+ Enabled: true
+ Force Arrow Scale: 2
+ Force Color: 204; 51; 51
+ Hide Small Values: true
+ History Length: 1
+ Name: WrenchStamped
+ Topic: /bl_force_torque_sensor_driver/output/wrench
+ Torque Arrow Scale: 20
+ Torque Color: 204; 204; 51
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: bl_force_torque_sensor
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 10
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1267
+ X: 60
+ Y: 27
diff --git a/bl_force_torque_sensor_ros/launch/sample.launch b/bl_force_torque_sensor_ros/launch/sample.launch
new file mode 100644
index 000000000..657bf6fc0
--- /dev/null
+++ b/bl_force_torque_sensor_ros/launch/sample.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/bl_force_torque_sensor_ros/launch/sensor_driver_rosserial.launch b/bl_force_torque_sensor_ros/launch/sensor_driver_rosserial.launch
new file mode 100644
index 000000000..185662358
--- /dev/null
+++ b/bl_force_torque_sensor_ros/launch/sensor_driver_rosserial.launch
@@ -0,0 +1,39 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ port: $(arg port)
+ baud: $(arg baud)
+
+
+
+
+
+
+
+ calib_matrix: $(arg calib_matrix)
+ sensor_type: $(arg sensor_type)
+ amplifier_type: $(arg amplifier_type)
+ adc_type: $(arg adc_type)
+ sensor_frame_id: $(arg sensor_frame_id)
+ rate: $(arg rate)
+
+
+
+
diff --git a/bl_force_torque_sensor_ros/msg/AmplifierOutputRawArray.msg b/bl_force_torque_sensor_ros/msg/AmplifierOutputRawArray.msg
new file mode 100644
index 000000000..a23efbbb5
--- /dev/null
+++ b/bl_force_torque_sensor_ros/msg/AmplifierOutputRawArray.msg
@@ -0,0 +1,5 @@
+# Output voltage from amplifiers of strain gauges.
+# This msg targets communication using rosserial.
+# To lighten process on microprocessor, this msg is intended to contain original A/D converter counts.
+
+int32[] array # Output voltage from amplifiers of strain gauges in A/D converter counts
diff --git a/bl_force_torque_sensor_ros/node_scripts/sensor_driver_rosserial.py b/bl_force_torque_sensor_ros/node_scripts/sensor_driver_rosserial.py
new file mode 100755
index 000000000..153b33e34
--- /dev/null
+++ b/bl_force_torque_sensor_ros/node_scripts/sensor_driver_rosserial.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+import rospy
+from bl_force_torque_sensor_ros.msg import AmplifierOutputRawArray
+from geometry_msgs.msg import WrenchStamped
+from std_msgs.msg import Float32
+
+
+class SensorDriverRosserial(object):
+
+ def __init__(self):
+ self.calib_matrix = rospy.get_param('~calib_matrix', None)
+ if self.calib_matrix is None:
+ rospy.logfatal(
+ 'Please specify calibration matrix of your sensor')
+ return
+ self.calib_matrix = np.array(self.calib_matrix)
+
+ self.sensor_type = rospy.get_param(
+ '~sensor_type', 'NANO2.5/2-A')
+ if self.sensor_type == 'NANO1.2/1-A':
+ self.n_per_count = 4.9e-3 # N/count
+ self.nm_per_count = 4.9e-5 # Nm/count
+ elif self.sensor_type == 'NANO2.5/2-A':
+ self.n_per_count = 9.8e-3
+ self.nm_per_count = 9.8e-5
+ elif self.sensor_type == 'NANO5/4-A':
+ self.n_per_count = 24.5e-3
+ self.nm_per_count = 19.6e-5
+ else:
+ rospy.logfatal(
+ 'Unimplemented sensor type: %s', self.sensor_type)
+ return
+
+ self.amplifier_type = rospy.get_param(
+ '~amplifier_type', 'RMD')
+ if self.amplifier_type == 'RMD' or self.amplifier_type == 'RMS':
+ self.amp_effective_range_max = 5.0
+ elif self.amplifier_type == 'RMSH':
+ self.amp_effective_range_max = 2.5
+ else:
+ rospy.logfatal(
+ 'Unimplemented amplifier type: %s', self.amplifier_type)
+ return
+
+ self.adc_type = rospy.get_param(
+ '~adc_type', 'ADS131M04')
+ if self.adc_type == 'ADS131M04':
+ self.voltage_divider = (10.0 + 1.1) / 1.1
+ self.adc_range = 2.4 # From -FSR to +FSR
+ self.adc_resolution = 24 # Number of bits
+ else:
+ rospy.logfatal(
+ 'Unimplemented ADC type: %s', self.adc_type)
+ return
+
+ self.sensor_frame_id = rospy.get_param(
+ '~sensor_frame_id', 'bl_force_torque_sensor')
+
+ self.rate = rospy.get_param('~rate', -1) # Hz
+ if self.rate <= 0:
+ self.loop_duration = 0
+ else:
+ self.loop_duration = 1.0 / self.rate # sec
+ self.duration_pub = rospy.Publisher(
+ '~parameter_setter/loop_duration',
+ Float32,
+ queue_size=1,
+ latch=True
+ )
+ self.duration_pub.publish(Float32(self.loop_duration))
+
+ self.wrench_pub = rospy.Publisher(
+ '~output/wrench', WrenchStamped, queue_size=1)
+ self.input_sub = rospy.Subscriber(
+ '~input', AmplifierOutputRawArray, self.callback)
+
+ def callback(self, in_msg):
+ stamp = rospy.Time.now() # Get timestamp ASAP after message arrival
+
+ # Convert ADC count of amplifier output to force/torque
+ v_raw = np.array(in_msg.array)
+ ## Based on ADS131M04 datasheet Revision C p. 38
+ v_volt = (v_raw *
+ (float(self.adc_range) / (2 ** self.adc_resolution)) *
+ self.voltage_divider)
+ ## Based on NANO sensor manual Rev3.1 p. 8
+ v = 2048 * v_volt / self.amp_effective_range_max
+ ## Based on NANO sensor manual Rev3.1 p. 9
+ f = np.dot(self.calib_matrix, v)
+ real_force = f[:3] * self.n_per_count
+ real_torque = f[3:] * self.nm_per_count
+
+ # Publish force/torque as WrenchStamped
+ wrench_msg = WrenchStamped()
+ wrench_msg.header.stamp = stamp
+ wrench_msg.header.frame_id = self.sensor_frame_id
+ wrench_msg.wrench.force.x = real_force[0]
+ wrench_msg.wrench.force.y = real_force[1]
+ wrench_msg.wrench.force.z = real_force[2]
+ wrench_msg.wrench.torque.x = real_torque[0]
+ wrench_msg.wrench.torque.y = real_torque[1]
+ wrench_msg.wrench.torque.z = real_torque[2]
+ self.wrench_pub.publish(wrench_msg)
+
+
+if __name__ == '__main__':
+ rospy.init_node('bl_force_torque_sensor_driver')
+ app = SensorDriverRosserial()
+ rospy.spin()
diff --git a/bl_force_torque_sensor_ros/package.xml b/bl_force_torque_sensor_ros/package.xml
new file mode 100644
index 000000000..413ace3fa
--- /dev/null
+++ b/bl_force_torque_sensor_ros/package.xml
@@ -0,0 +1,25 @@
+
+
+ bl_force_torque_sensor_ros
+ 2.1.14
+ ROS driver package for BL force torque sensor
+
+ Shun Hasegawa
+ Kei Okada
+ Shun Hasegawa
+
+ BSD
+
+ catkin
+ message_generation
+ message_runtime
+ message_runtime
+ rospy
+ rosserial_arduino
+ rosserial_python
+ std_msgs
+ geometry_msgs
+
+
+
+