Add SLATE Base ROS 1 Drivers (#34)

* [ros_slate] Add the Interbotix Slate ROS 1 driver

* Fix node name

* Minor logging improvements

* Clean up messages CMakeLists

* Add logging to service callbacks

* Swap base_node and base files

* Add header guards

* Update some comments

* Add SLATE noetic workflow

* Clean up interface package

* Add dependencies to slate driver cmakelists

* Remove launch install

.github/workflows/slate-noetic.yaml

@@ -0,0 +1,34 @@
+name: build-slate-noetic
+
+on:
+  push:
+    branches:
+      - main
+      - devel
+  pull_request:
+    branches:
+      - main
+      - devel
+  workflow_dispatch:
+
+defaults:
+  run:
+    shell: bash
+
+jobs:
+  slate-noetic:
+    strategy:
+      matrix:
+        env:
+          - {ROS_DISTRO: noetic,  ROS_REPO: main, BUILDER: catkin_tools}
+          - {ROS_DISTRO: noetic,  ROS_REPO: main, BUILDER: catkin_make}
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Create src directory for slate
+        run: |
+          rm interbotix_ros_slate/CATKIN_IGNORE
+          mkdir src
+          mv interbotix_ros_slate src
+      - uses: 'ros-industrial/industrial_ci@master'
+        env: ${{matrix.env}}

interbotix_ros_slate/interbotix_ros_slate/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(interbotix_ros_slate)
+find_package(catkin REQUIRED)
+catkin_metapackage()

interbotix_ros_slate/interbotix_ros_slate/README.md

@@ -0,0 +1,3 @@
+# interbotix_ros_slate
+
+This metapackage groups together the core ROS Packages for the Interbotix Slate mobile base.

interbotix_ros_slate/interbotix_ros_slate/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>interbotix_ros_slate</name>
+  <version>0.0.0</version>
+  <description>The interbotix_ros_slate metapackage</description>
+  <maintainer email="trsupport@trossenrobotics.com">Trossen Robotics</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <author email="trsupport@trossenrobotics.com">Trossen Robotics</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>interbotix_slate_driver</exec_depend>
+  <exec_depend>interbotix_slate_msgs</exec_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.10.0)
+project(interbotix_slate_driver)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-Wall -g -std=c++11 -O0 ")
+
+set(serial_driver "chassis_driver")
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  interbotix_slate_msgs
+  nav_msgs
+  roscpp
+  sensor_msgs
+  std_srvs
+  tf
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    geometry_msgs
+    interbotix_slate_msgs
+    nav_msgs
+    roscpp
+    sensor_msgs
+    std_srvs
+    tf
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib)
+
+add_library(slate_base
+  src/slate_base.cpp
+  src/base_driver.cpp
+)
+add_dependencies(slate_base ${slate_base_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(slate_base
+  ${serial_driver}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(slate_base_node
+  src/slate_base_node.cpp
+)
+add_dependencies(slate_base_node ${slate_base_node_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(slate_base_node
+  slate_base
+)
+
+add_custom_command(
+	OUTPUT copy_lib
+	COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_SOURCE_DIR}/lib/* ${CATKIN_DEVEL_PREFIX}/lib
+)
+add_custom_target(COPY_CHASSIS_LIB ALL DEPENDS copy_lib slate_base_node)
+
+install(
+  TARGETS
+    slate_base_node
+  RUNTIME DESTINATION
+    ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  FILES
+    lib/lib${serial_driver}.so
+  DESTINATION
+    ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

interbotix_ros_slate/interbotix_slate_driver/README.md

@@ -0,0 +1 @@
+# interbotix_slate_driver

interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.h

@@ -0,0 +1,44 @@
+#ifndef INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_
+#define INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_
+
+#include <string>
+
+#include "od_index.h"
+
+namespace base_driver
+{
+
+bool chassisInit(std::string &dev);
+bool chassisControl(float aim_x_vel, float aim_z_omega);
+bool getChassisInfo(float &x_vel, float &z_omega);
+bool getChassisOdom(float &odom_x, float &odom_y, float &odom_theta);
+
+bool getBatteryInfo(float &vol, float &cur, int &percent);
+bool getChassisState(SystemState &state);
+bool getChassisCollision(int &collision);
+bool getVersion(char *text);
+bool getJoyState(int &state);
+
+// Limited to 100 characters
+bool setText(const char *text);
+
+// 0 / 1
+bool setCharge(int charge);
+
+// 0 / 1
+bool setAlarm(int alarm);
+
+bool motorCtrl(int v);
+
+bool setIo(int io);
+
+bool getIo(int &io_state);
+
+// 0 ~ 100
+bool setLight(int light);
+
+bool setStateLight(int light);
+
+}  // namespace base_driver
+
+#endif  // INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_H_

interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/od_index.h

@@ -0,0 +1,58 @@
+#ifndef INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_
+#define INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_
+
+#define PORT "chassis"
+
+typedef enum
+{
+  SYS_INIT = 0x00,
+  SYS_NORMAL,
+  SYS_REMOTE,
+  SYS_ESTOP,
+  SYS_CALIB,
+  SYS_TEST,
+  SYS_CHARGING,
+
+  SYS_ERR = 0x10,
+  SYS_ERR_ID,
+  SYS_ERR_COM,
+  SYS_ERR_ENC,
+  SYS_ERR_COLLISION,
+  SYS_ERR_LOW_VOLTAGE,
+  SYS_ERR_OVER_VOLTAGE,
+  SYS_ERR_OVER_CURRENT,
+  SYS_ERR_OVER_TEMP,
+
+  SYS_STATE_LEN,
+} SystemState;
+
+typedef enum
+{
+  INDEX_SYS_STATE = 0,
+  INDEX_SYS_POWER_PERCENTAGE = 1,
+
+  INDEX_CHASSIS_VEL = 2,
+  INDEX_CHASSIS_POS_OR_OMEGA = 3,
+  INDEX_CHASSIS_ODOM_X = 4,
+  INDEX_CHASSIS_ODOM_Y = 5,
+  INDEX_CHASSIS_ODOM_THETA = 6,
+
+  INDEX_SYS_VOLTAGE = 7,
+  INDEX_SYS_CURRENT = 8,
+
+  INDEX_AIM_CHASSIS_VEL = 9,
+  INDEX_AIM_CHASSIS_POS_OR_OMEGA = 10,
+
+  INDEX_SYS_CHARGE = 11,
+  INDEX_SYS_ALARM = 12,
+  INDEX_SYS_TEXT = 13,
+  INDEX_SYS_LIGHT = 14,
+  INDEX_SYS_COLLISION = 15,
+  INDEX_SYS_VERSION = 16,
+  INDEX_STATE_LIGHT = 17,
+  INDEX_STATE_JOY = 18,
+  INDEX_STATE_IO = 21,
+  INDEX_SYS_CMD = 22,
+} OdIndex;
+
+#endif  // INTERBOTIX_SLATE_DRIVER__OD_INDEX_H_

interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.h

@@ -0,0 +1,31 @@
+#ifndef INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_
+#define INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_
+
+#include <string>
+#define ASSERT(expr, fail) (static_cast<bool>(expr) ? void(0) : (fail))
+
+class SerialDriver
+{
+ public:
+  bool init(const std::string& portname, std::string& dev, int baudRate);
+  void close();
+
+  bool setEntry(int index, const float* data_address);
+  bool setEntry(int index, const int* data_address);
+  bool setEntry(int index, const char* data_address);
+
+  bool getEntry(int index, float* data_address);
+  bool getEntry(int index, int* data_address);
+  bool getEntry(int index, char* data_address);
+
+ private:
+  bool handleTimeout(int);
+
+  int fd_;
+  bool isOpened_;
+  int conn_timeout_cnt_;
+  int baud_rate_ = 0;
+  std::string portname_;
+};
+
+#endif  // INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_H_