thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Ar3HardwareSystem exists

Browse source 
And is pain
This commit is contained in:
Thomas Muller 2021-09-07 05:51:00 +00:00
parent bb0cfd40f4
commit 754d5d5d4c
10 changed files with 563 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

182
src/ar3_config/rviz/ar3.rviz Normal file
View file

@ -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

1
src/ar3_config/setup.py
View file

@ -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'],

8
src/ar3_description/ar3_description/state_publisher_net.py
View file

@ -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])
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:

2
src/ar3_description/urdf/ar3.urdf
View file

@ -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>

78
src/ar3_hardware/CMakeLists.txt Normal file
View file

@ -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()

5
src/ar3_hardware/ar3_hardware.xml Normal file
View file

@ -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>

67
src/ar3_hardware/include/ar3_hardware/ar3_system.hpp Normal file
View file

@ -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_

19
src/ar3_hardware/package.xml Normal file
View file

@ -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>

201
src/ar3_hardware/src/ar3_system.cpp Normal file
View file

@ -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)

5
src/net_robot/net_robot/net_robot.py
View file

@ -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()