Ar3HardwareSystem exists

And is pain

src/ar3_config/rviz/ar3.rviz

@@ -0,0 +1,182 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 616
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/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: <Fixed 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: 000000ff00000000fd000000040000000000000156000002f3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 1068
+  Y: 195

src/ar3_config/setup.py

@@ -12,6 +12,7 @@ setup(
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/config', glob('config/*.yaml')),
+        ('share/' + package_name + '/rviz', glob('rviz/*.rviz')),
         ('share/' + package_name, glob('launch/*.py')),
     ],
     install_requires=['setuptools'],

src/ar3_description/ar3_description/state_publisher_net.py

@@ -47,11 +47,13 @@ class StatePublisher(Node):
                         cli.close()
                         print('bailing')
                         break
-                    print(repr(msg))
 
-                    if len(msg) >= 13 and msg[12] == 10:
+                    while len(msg) >= 13:
-                        self.cmd_joints = struct.unpack('hhhhhh', msg[:-1])
+                        if msg[12] == 10:
+                            self.cmd_joints = struct.unpack('hhhhhh', msg[:12])
                             self.cmd_joints = [a / 180. * pi for a in self.cmd_joints]
+                            msg = msg[13:]
+                        print(repr(msg))
                 except BlockingIOError as e:
                     pass
                 except Exception as e:

src/ar3_description/urdf/ar3.urdf

@@ -362,7 +362,7 @@
 
  <ros2_control name="joint_trajectory_controller" type="system">
   <hardware>
-   <plugin>fake_components/GenericSystem</plugin>
+   <plugin>ar3_hardware/Ar3SystemHardware</plugin>
   </hardware>
   <joint name="joint_1">
    <param name="initial_position">0.0</param>

src/ar3_hardware/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.5)
+project(ar3_hardware)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/ar3_system.cpp
+)
+
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+)
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+)
+
+pluginlib_export_plugin_description_file(hardware_interface ar3_hardware.xml)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  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
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+)
+ament_package()

src/ar3_hardware/ar3_hardware.xml

@@ -0,0 +1,5 @@
+<library path="ar3_hardware">
+  <class name="ar3_hardware/Ar3SystemHardware" type="ar3_hardware::Ar3SystemHardware" base_class_type="hardware_interface::SystemInterface">
+    <description>Ar3 hardware system interface</description>
+  </class>
+</library>

src/ar3_hardware/include/ar3_hardware/ar3_system.hpp

@@ -0,0 +1,67 @@
+#ifndef AR3_HARDWARE__AR3_SYSTEM_HPP_
+#define AR3_HARDWARE__AR3_SYSTEM_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <thread>
+
+#include "hardware_interface/base_interface.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_status_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float32.hpp"
+
+namespace ar3_hardware
+{
+class Ar3SystemHardware
+: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(Ar3SystemHardware)
+
+  __attribute__((visibility("default")))
+  hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
+
+  __attribute__((visibility("default")))
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  __attribute__((visibility("default")))
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  __attribute__((visibility("default")))
+  hardware_interface::return_type start() override;
+
+  __attribute__((visibility("default")))
+  hardware_interface::return_type stop() override;
+
+  __attribute__((visibility("default")))
+  hardware_interface::return_type read() override;
+
+  __attribute__((visibility("default")))
+  hardware_interface::return_type write() override;
+
+private:
+  // Store the command for the simulated robot
+  std::vector<double> hw_commands_;
+  std::vector<double> hw_positions_;
+
+  rclcpp::Node::SharedPtr nh_;
+  rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
+  std::thread thread_executor_spin_;
+
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j1_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j2_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j3_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j5_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_pub_;
+};
+
+}  // namespace ar3_hardware
+
+#endif  // AR3_HARDWARE__AR3_SYSTEM_HPP_

src/ar3_hardware/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ar3_hardware</name>
+  <version>1.0.0</version>
+  <description>AR3 Hardware interface</description>
+  <maintainer email="tmuller2017@my.fit.edu">Thomas Muller</maintainer>
+  <maintainer email="farrell2017@my.fit.edu">John Farrell</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ar3_hardware/src/ar3_system.cpp

@@ -0,0 +1,201 @@
+#include "ar3_hardware/ar3_system.hpp"
+
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+namespace ar3_hardware
+{
+
+hardware_interface::return_type Ar3SystemHardware::configure(
+  const hardware_interface::HardwareInfo & info)
+{
+  if (configure_default(info) != hardware_interface::return_type::OK)
+  {
+    return hardware_interface::return_type::ERROR;
+  }
+
+  //hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
+  //hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
+  hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+
+  for (const hardware_interface::ComponentInfo & joint : info_.joints)
+  {
+    // DiffBotSystem has exactly two states and one command interface on each joint
+    if (joint.command_interfaces.size() != 1)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("Ar3SystemHardware"),
+        "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
+        joint.command_interfaces.size());
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("Ar3SystemHardware"),
+        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
+        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.state_interfaces.size() != 1)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("Ar3SystemHardware"),
+        "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(),
+        joint.state_interfaces.size());
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("Ar3SystemHardware"),
+        "Joint '%s' have '%s' as state interface. '%s' expected.",
+        joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
+        hardware_interface::HW_IF_POSITION);
+      return hardware_interface::return_type::ERROR;
+    }
+  }
+
+  status_ = hardware_interface::status::CONFIGURED;
+  return hardware_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  for (auto i = 0u; i < info_.joints.size(); i++)
+  {
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
+  }
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  for (auto i = 0u; i < info_.joints.size(); i++)
+  {
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+  }
+
+  return command_interfaces;
+}
+
+hardware_interface::return_type Ar3SystemHardware::start()
+{
+  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Starting ...please wait...");
+
+  // set some default values
+  for (auto i = 0u; i < hw_positions_.size(); i++)
+  {
+    if (std::isnan(hw_positions_[i]))
+    {
+      hw_positions_[i] = 0;
+      hw_commands_[i] = 0;
+    }
+  }
+
+  executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
+  executor_->add_node(nh_);
+
+  auto spin = [this]() {
+    while(rclcpp::ok()) {
+      executor_->spin_once();
+    }
+  };
+  thread_executor_spin_ = std::thread(spin);
+
+  j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j1", 10);
+  j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j2", 10);
+  j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j3", 10);
+  j4_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j4", 10);
+  j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j5", 10);
+  j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/net_robot/drives/j6", 10);
+
+  status_ = hardware_interface::status::STARTED;
+
+  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type Ar3SystemHardware::stop()
+{
+  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Stopping ...please wait...");
+
+  status_ = hardware_interface::status::STOPPED;
+
+  RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System successfully stopped!");
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type Ar3SystemHardware::read()
+{
+  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
+
+  for (uint i = 0; i < hw_commands_.size(); i++)
+  {
+    hw_positions_[i] = 6.9;
+
+  //  RCLCPP_INFO(
+  //    rclcpp::get_logger("Ar3SystemHardware"),
+  //    "Got position state %.5f for '%s'!", hw_positions_[i],
+  //    info_.joints[i].name.c_str());
+  }
+
+  //RCLCPP_INFO(
+  //  rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",
+  //  hw_commands_[0], hw_commands_[1], hw_commands_[2]);
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
+{
+  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");
+
+  auto message = std_msgs::msg::Float32();
+  message.data = hw_commands_[0];
+  j1_pub_->publish(message);
+  message.data = hw_commands_[1];
+  j2_pub_->publish(message);
+  message.data = hw_commands_[2];
+  j3_pub_->publish(message);
+  message.data = hw_commands_[3];
+  j4_pub_->publish(message);
+  message.data = hw_commands_[4];
+  j5_pub_->publish(message);
+  message.data = hw_commands_[5];
+  j6_pub_->publish(message);
+
+  //for (auto i = 0u; i < hw_commands_.size(); i++)
+  //{
+    // Simulate sending commands to the hardware
+  //  RCLCPP_INFO(
+  //    rclcpp::get_logger("Ar3SystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i],
+  //    info_.joints[i].name.c_str());
+  //}
+  //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully written!");
+
+  return hardware_interface::return_type::OK;
+}
+
+}  // namespace ar3_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+  ar3_hardware::Ar3SystemHardware, hardware_interface::SystemInterface)

src/net_robot/net_robot/net_robot.py

@@ -9,7 +9,7 @@ import struct
 
 class NetRobot(Node):
     def __init__(self):
-        super().__init__('state_publisher')
+        super().__init__('net_robot')
 
         qos = QoSProfile(depth=10)
         self.create_subscription(Float32, '~/drives/j1', self._on_j1, qos)
@@ -24,10 +24,11 @@ class NetRobot(Node):
         self._sock.connect(('127.0.0.1', 9999))
 
         self._joints = [0.] * 6
-        self._prev_joints = [0.] * 6
+        self._prev_joints = [None] * 6
 
     def _send(self):
         if self._joints != self._prev_joints:
+            print('pub')
             self._sock.send(struct.pack('hhhhhh', *[int(j) for j in self._joints]) + b'\n')
             self._prev_joints = self._joints.copy()