From 754d5d5d4c2b11b0f7dd93c3135528722bfe554e Mon Sep 17 00:00:00 2001 From: Thomas Muller Date: Tue, 7 Sep 2021 05:51:00 +0000 Subject: [PATCH] Ar3HardwareSystem exists And is pain --- src/ar3_config/rviz/ar3.rviz | 182 ++++++++++++++++ src/ar3_config/setup.py | 1 + .../ar3_description/state_publisher_net.py | 10 +- src/ar3_description/urdf/ar3.urdf | 2 +- src/ar3_hardware/CMakeLists.txt | 78 +++++++ src/ar3_hardware/ar3_hardware.xml | 5 + .../include/ar3_hardware/ar3_system.hpp | 67 ++++++ src/ar3_hardware/package.xml | 19 ++ src/ar3_hardware/src/ar3_system.cpp | 201 ++++++++++++++++++ src/net_robot/net_robot/net_robot.py | 5 +- 10 files changed, 563 insertions(+), 7 deletions(-) create mode 100644 src/ar3_config/rviz/ar3.rviz create mode 100644 src/ar3_hardware/CMakeLists.txt create mode 100644 src/ar3_hardware/ar3_hardware.xml create mode 100644 src/ar3_hardware/include/ar3_hardware/ar3_system.hpp create mode 100644 src/ar3_hardware/package.xml create mode 100644 src/ar3_hardware/src/ar3_system.cpp diff --git a/src/ar3_config/rviz/ar3.rviz b/src/ar3_config/rviz/ar3.rviz new file mode 100644 index 0000000..16a5970 --- /dev/null +++ b/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: + 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: + 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 diff --git a/src/ar3_config/setup.py b/src/ar3_config/setup.py index 9b81377..5d41d78 100644 --- a/src/ar3_config/setup.py +++ b/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'], diff --git a/src/ar3_description/ar3_description/state_publisher_net.py b/src/ar3_description/ar3_description/state_publisher_net.py index 75e0a02..1506cb0 100644 --- a/src/ar3_description/ar3_description/state_publisher_net.py +++ b/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: - self.cmd_joints = struct.unpack('hhhhhh', msg[:-1]) - self.cmd_joints = [a / 180. * pi for a in self.cmd_joints] + while len(msg) >= 13: + 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: diff --git a/src/ar3_description/urdf/ar3.urdf b/src/ar3_description/urdf/ar3.urdf index 98cd593..2fdac2c 100644 --- a/src/ar3_description/urdf/ar3.urdf +++ b/src/ar3_description/urdf/ar3.urdf @@ -362,7 +362,7 @@ - fake_components/GenericSystem + ar3_hardware/Ar3SystemHardware 0.0 diff --git a/src/ar3_hardware/CMakeLists.txt b/src/ar3_hardware/CMakeLists.txt new file mode 100644 index 0000000..c4d142e --- /dev/null +++ b/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( 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() diff --git a/src/ar3_hardware/ar3_hardware.xml b/src/ar3_hardware/ar3_hardware.xml new file mode 100644 index 0000000..9de8813 --- /dev/null +++ b/src/ar3_hardware/ar3_hardware.xml @@ -0,0 +1,5 @@ + + + Ar3 hardware system interface + + diff --git a/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp b/src/ar3_hardware/include/ar3_hardware/ar3_system.hpp new file mode 100644 index 0000000..498b659 --- /dev/null +++ b/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 +#include +#include +#include + +#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 +{ +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 export_state_interfaces() override; + + __attribute__((visibility("default"))) + std::vector 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 hw_commands_; + std::vector hw_positions_; + + rclcpp::Node::SharedPtr nh_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + std::thread thread_executor_spin_; + + rclcpp::Publisher::SharedPtr j1_pub_; + rclcpp::Publisher::SharedPtr j2_pub_; + rclcpp::Publisher::SharedPtr j3_pub_; + rclcpp::Publisher::SharedPtr j4_pub_; + rclcpp::Publisher::SharedPtr j5_pub_; + rclcpp::Publisher::SharedPtr j6_pub_; +}; + +} // namespace ar3_hardware + +#endif // AR3_HARDWARE__AR3_SYSTEM_HPP_ diff --git a/src/ar3_hardware/package.xml b/src/ar3_hardware/package.xml new file mode 100644 index 0000000..188328c --- /dev/null +++ b/src/ar3_hardware/package.xml @@ -0,0 +1,19 @@ + + + + ar3_hardware + 1.0.0 + AR3 Hardware interface + Thomas Muller + John Farrell + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp new file mode 100644 index 0000000..f16abad --- /dev/null +++ b/src/ar3_hardware/src/ar3_system.cpp @@ -0,0 +1,201 @@ +#include "ar3_hardware/ar3_system.hpp" + +#include +#include +#include +#include +#include + +#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::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::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 Ar3SystemHardware::export_state_interfaces() +{ + std::vector 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 Ar3SystemHardware::export_command_interfaces() +{ + std::vector 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(); + nh_ = std::make_shared("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("/net_robot/drives/j1", 10); + j2_pub_ = nh_->create_publisher("/net_robot/drives/j2", 10); + j3_pub_ = nh_->create_publisher("/net_robot/drives/j3", 10); + j4_pub_ = nh_->create_publisher("/net_robot/drives/j4", 10); + j5_pub_ = nh_->create_publisher("/net_robot/drives/j5", 10); + j6_pub_ = nh_->create_publisher("/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) diff --git a/src/net_robot/net_robot/net_robot.py b/src/net_robot/net_robot/net_robot.py index 5902232..3222d6a 100644 --- a/src/net_robot/net_robot/net_robot.py +++ b/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()