NASA_Challenge_[@xfiderek] Add trick_ros2_control package (space-ros#42…

…).

Adds a ROS2 package with ROS2 Control Hardware Plugin that allows to use Trick as Hardware System.
It works similarly to gazebo_ros2_control.

ros_trick/ros_src/trick_ros2_control/CMakeLists.txt

@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 3.8)
+project(trick_ros2_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+
+
+add_library(
+  trick_ros2_control
+  SHARED
+  src/trick_system.cpp
+  src/socket.cpp
+)
+target_include_directories(
+  trick_ros2_control
+  PUBLIC
+  include
+)
+ament_target_dependencies(
+  trick_ros2_control
+  hardware_interface
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+)
+# prevent pluginlib from using boost
+target_compile_definitions(trick_ros2_control PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(
+  hardware_interface trick_ros2_control.xml)
+
+install(
+  TARGETS
+  trick_ros2_control
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  trick_ros2_control
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+ament_package()

ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/socket.hpp

@@ -0,0 +1,51 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Largely inspired by
+// https://github.com/nasa/trick/blob/master/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp
+#ifndef TRICK_ROS2_CONTROL__SOCKET_HPP_
+#define TRICK_ROS2_CONTROL__SOCKET_HPP_
+
+#include <arpa/inet.h>
+#include <cstring>
+#include <iostream>
+#include <netinet/in.h>
+#include <string>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+#define SOCKET_BUF_SIZE 20480
+
+class Socket
+{
+public:
+  Socket();
+  int init(std::string hostname, int port);
+
+  int send(std::string message);
+  int operator<<(std::string message);
+
+  std::string receive();
+  void operator>>(std::string& ret);
+
+private:
+  int _port;
+  std::string _hostname;
+  int _socket_fd;
+  bool _initialized;
+  char carry_on_buffer_[SOCKET_BUF_SIZE] = { '\0' };
+  char trick_delimiter_ = '\n';
+};
+
+#endif

ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/trick_system.hpp

@@ -0,0 +1,142 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_
+#define TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "trick_ros2_control/socket.hpp"
+#include "trick_ros2_control/utils.hpp"
+#include "trick_ros2_control/visibility_control.h"
+#include <limits>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace trick_ros2_control
+{
+
+/**
+ * @brief Data structure representing robot data on ROS side
+ *
+ */
+struct RosJointData
+{
+  std::string name;
+  double joint_position = std::numeric_limits<double>::quiet_NaN();
+  double joint_velocity = std::numeric_limits<double>::quiet_NaN();
+  double joint_acceleration = std::numeric_limits<double>::quiet_NaN();
+  double joint_effort = std::numeric_limits<double>::quiet_NaN();
+  double joint_position_cmd;
+  double joint_velocity_cmd;
+  double joint_acceleration_cmd;
+  double joint_effort_cmd;
+};
+
+/**
+ * @brief Data structure representing data on Trick side
+ * Used in the file to map trick variable to pointer to the data
+ * Used for both sending and receiving data
+ */
+struct TrickData
+{
+  const std::string trick_var_name;
+  double* const data_ptr;
+  TrickData(const std::string& trick_var_name, double* const data_ptr)
+    : trick_var_name(trick_var_name), data_ptr(data_ptr){};
+};
+
+/**
+ * @brief Mapping from the pointer where command is stored to the corresponding trick variable name
+ */
+using CommandDataToTrickVarName = std::pair<double* const, const std::string>;
+/**
+ * @brief Mapping from trick variable name to pointer where its value is stored
+ */
+using TrickVarNameToStateData = std::pair<const std::string, double* const>;
+
+class TrickSystem : public hardware_interface::SystemInterface
+{
+public:
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+  hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+private:
+  /**
+   * @brief Represents data received from trick
+   */
+  std::vector<TrickData> trick_state_variables_;
+  /**
+   * @brief Represents data to be sent to trick
+   */
+  std::vector<TrickData> trick_command_variables_;
+  /**
+   * @brief Thread used for receiving data from trick asynchronously
+   */
+  std::thread incoming_data_thread_;
+  /**
+   * @brief Socket encapsulating the connection with trick simulation
+   */
+  Socket trick_conn_socket_;
+  /**
+   * @brief Buffer for accessing incoming data in realtime-safe manner
+   */
+  realtime_tools::RealtimeBuffer<std::vector<double>> trick_data_incoming_buffer_;
+
+  int trick_server_port_;
+  std::string trick_hostname_;
+  double trick_variable_cycle_rate_;
+
+  const int MAX_NO_OF_RECONNECTS = 5;
+  const int RECONNECT_WAIT_TIME_S = 2;
+  std::vector<struct RosJointData> joints_data_;
+
+  // parameter names used in URDF file for trick-related params
+  const std::string JOINT_PARAM_STATE_INTERFACE_KEY = "state_interface";
+  const std::string JOINT_PARAM_COMMAND_INTERFACE_KEY = "command_interface";
+  const std::string JOINT_PARAM_TRICK_VARIABLE_NAME_KEY = "trick_variable_name";
+};
+
+}  // namespace trick_ros2_control
+
+#endif  // TRICK_ROS2_CONTROL__TRICK_SYSTEM_HPP_

ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/utils.hpp

@@ -0,0 +1,52 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+double trick_string_convert(const std::string& str)
+{
+  std::istringstream ss(str);
+  double num;
+  ss >> num;
+  return num;
+}
+
+std::vector<std::string> trick_split_response(std::string& str, const char delim)
+{
+  std::stringstream ss(str);
+  std::string s;
+  std::vector<std::string> ret;
+  while (ss >> s)
+  {
+    ret.push_back(s);
+  }
+  return ret;
+}
+
+std::vector<double> trick_response_convert(std::string& response)
+{
+  auto responseSplit = trick_split_response(response, '\t');
+  std::vector<double> result;
+  if (responseSplit[0] != "0")
+  {
+    return result;
+  }
+  for (int i = 1; i < responseSplit.size(); i++)
+  {
+    result.push_back(trick_string_convert(responseSplit[i]));
+  }
+  return result;
+}

ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/visibility_control.h

@@ -0,0 +1,56 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_
+#define TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport)
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE                        \
+  TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT                             \
+  __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC                             \
+  __attribute__((visibility("default")))
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL                              \
+  __attribute__((visibility("hidden")))
+#else
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL
+#endif
+#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif // TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_