From 4230a9c7b42bf68c90d2ae94a64190dadcf602ef Mon Sep 17 00:00:00 2001 From: Marcus Grieger Date: Sun, 11 Feb 2024 16:14:52 +0100 Subject: [PATCH] Unified projects in one repo --- hexapod_hardware/.gitignore | 2 + hexapod_hardware/CMakeLists.txt | 69 ++ hexapod_hardware/LICENSE | 17 + .../hardware/hexapod_robot_hwi.cpp | 213 ++++++ .../hexapod_robot/hexapod_robot_hwi.hpp | 91 +++ hexapod_hardware/hexapod_hardware.xml | 7 + hexapod_hardware/package.xml | 18 + hexapod_moveit/.setup_assistant | 10 + hexapod_moveit/CMakeLists.txt | 11 + hexapod_moveit/COLCON_IGNORE | 0 hexapod_moveit/config/hexapod_robot.rviz | 669 ++++++++++++++++++ hexapod_moveit/config/hexapod_robot.srdf | 21 + .../config/hexapod_robot.urdf.xacro | 14 + hexapod_moveit/config/joint_limits.yaml | 7 + hexapod_moveit/config/kinematics.yaml | 77 ++ hexapod_moveit/config/moveit_controllers.yaml | 81 +++ .../config/pilz_cartesian_limits.yaml | 6 + hexapod_moveit/config/ros2_controllers.yaml | 103 +++ hexapod_moveit/launch/move_group.launch.py | 85 +++ .../launch/move_group_gui.launch.py | 96 +++ hexapod_moveit/launch/rviz.launch.py | 37 + hexapod_moveit/package.xml | 52 ++ hexapod_python/.gitignore | 2 + hexapod_python/LICENSE | 17 + hexapod_python/hexapod_python/__init__.py | 0 hexapod_python/hexapod_python/ik.py | 97 +++ hexapod_python/hexapod_python/ik_node.py | 123 ++++ hexapod_python/hexapod_python/poses.py | 144 ++++ hexapod_python/hexapod_python/taranis_test.py | 132 ++++ hexapod_python/package.xml | 25 + hexapod_python/resource/hexapod_python | 0 hexapod_python/setup.cfg | 4 + hexapod_python/setup.py | 30 + hexapod_python/test/test_copyright.py | 25 + hexapod_python/test/test_flake8.py | 25 + hexapod_python/test/test_pep257.py | 23 + hexapod_robot/CMakeLists.txt | 16 + hexapod_robot/config/ros2_controllers.yaml | 39 + .../config/urdf/hexapod_robot.urdf.xacro | 28 + .../config/urdf/hexapod_robot_macros.xacro | 179 +++++ .../config/urdf/hexapod_robot_materials.xacro | 25 + .../urdf/hexapod_robot_ros2_control.xacro | 78 ++ .../launch/joint_trajectory_control.launch.py | 71 ++ .../launch/taranis_control.launch.py | 85 +++ hexapod_robot/package.xml | 20 + taranis/CMakeLists.txt | 49 ++ taranis/LICENSE | 17 + taranis/include/taranis/taranis_node.hpp | 44 ++ taranis/msg/TaranisPacket.msg | 15 + taranis/package.xml | 25 + taranis/src/taranis_node.cpp | 173 +++++ 51 files changed, 3197 insertions(+) create mode 100644 hexapod_hardware/.gitignore create mode 100644 hexapod_hardware/CMakeLists.txt create mode 100644 hexapod_hardware/LICENSE create mode 100644 hexapod_hardware/hardware/hexapod_robot_hwi.cpp create mode 100644 hexapod_hardware/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp create mode 100644 hexapod_hardware/hexapod_hardware.xml create mode 100644 hexapod_hardware/package.xml create mode 100644 hexapod_moveit/.setup_assistant create mode 100644 hexapod_moveit/CMakeLists.txt create mode 100644 hexapod_moveit/COLCON_IGNORE create mode 100644 hexapod_moveit/config/hexapod_robot.rviz create mode 100644 hexapod_moveit/config/hexapod_robot.srdf create mode 100644 hexapod_moveit/config/hexapod_robot.urdf.xacro create mode 100644 hexapod_moveit/config/joint_limits.yaml create mode 100644 hexapod_moveit/config/kinematics.yaml create mode 100644 hexapod_moveit/config/moveit_controllers.yaml create mode 100644 hexapod_moveit/config/pilz_cartesian_limits.yaml create mode 100644 hexapod_moveit/config/ros2_controllers.yaml create mode 100644 hexapod_moveit/launch/move_group.launch.py create mode 100644 hexapod_moveit/launch/move_group_gui.launch.py create mode 100644 hexapod_moveit/launch/rviz.launch.py create mode 100644 hexapod_moveit/package.xml create mode 100644 hexapod_python/.gitignore create mode 100644 hexapod_python/LICENSE create mode 100644 hexapod_python/hexapod_python/__init__.py create mode 100644 hexapod_python/hexapod_python/ik.py create mode 100644 hexapod_python/hexapod_python/ik_node.py create mode 100644 hexapod_python/hexapod_python/poses.py create mode 100644 hexapod_python/hexapod_python/taranis_test.py create mode 100644 hexapod_python/package.xml create mode 100644 hexapod_python/resource/hexapod_python create mode 100644 hexapod_python/setup.cfg create mode 100644 hexapod_python/setup.py create mode 100644 hexapod_python/test/test_copyright.py create mode 100644 hexapod_python/test/test_flake8.py create mode 100644 hexapod_python/test/test_pep257.py create mode 100644 hexapod_robot/CMakeLists.txt create mode 100644 hexapod_robot/config/ros2_controllers.yaml create mode 100644 hexapod_robot/config/urdf/hexapod_robot.urdf.xacro create mode 100644 hexapod_robot/config/urdf/hexapod_robot_macros.xacro create mode 100644 hexapod_robot/config/urdf/hexapod_robot_materials.xacro create mode 100644 hexapod_robot/config/urdf/hexapod_robot_ros2_control.xacro create mode 100644 hexapod_robot/launch/joint_trajectory_control.launch.py create mode 100644 hexapod_robot/launch/taranis_control.launch.py create mode 100644 hexapod_robot/package.xml create mode 100644 taranis/CMakeLists.txt create mode 100644 taranis/LICENSE create mode 100644 taranis/include/taranis/taranis_node.hpp create mode 100644 taranis/msg/TaranisPacket.msg create mode 100644 taranis/package.xml create mode 100644 taranis/src/taranis_node.cpp diff --git a/hexapod_hardware/.gitignore b/hexapod_hardware/.gitignore new file mode 100644 index 0000000..9beed20 --- /dev/null +++ b/hexapod_hardware/.gitignore @@ -0,0 +1,2 @@ +*~ +.vscode diff --git a/hexapod_hardware/CMakeLists.txt b/hexapod_hardware/CMakeLists.txt new file mode 100644 index 0000000..b30ed0f --- /dev/null +++ b/hexapod_hardware/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(hexapod_hardware) + +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) + +add_library( + hexapod_robot_hwi + SHARED + hardware/hexapod_robot_hwi.cpp +) + +target_compile_features(hexapod_robot_hwi PUBLIC cxx_std_17) +target_include_directories(hexapod_robot_hwi PUBLIC + $ + $ +) + +ament_target_dependencies( + hexapod_robot_hwi PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +target_link_libraries(hexapod_robot_hwi PUBLIC i2c) + +pluginlib_export_plugin_description_file(hardware_interface hexapod_hardware.xml) + +# install(TARGETS hexapod_robot_ik_server +# DESTINATION lib/${PROJECT_NAME} +# ) + +install(TARGETS hexapod_robot_hwi + EXPORT export_hexapod_hardware + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_targets(export_hexapod_hardware HAS_LIBRARY_TARGET) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rcl + cpp_lifecycle + i2c +) + + +ament_package() diff --git a/hexapod_hardware/LICENSE b/hexapod_hardware/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/hexapod_hardware/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/hexapod_hardware/hardware/hexapod_robot_hwi.cpp b/hexapod_hardware/hardware/hexapod_robot_hwi.cpp new file mode 100644 index 0000000..fb82ab0 --- /dev/null +++ b/hexapod_hardware/hardware/hexapod_robot_hwi.cpp @@ -0,0 +1,213 @@ +#include "hexapod_robot/hexapod_robot_hwi.hpp" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +extern "C" { + #include +} + +namespace hexapod_robot { + + +HexapodRobotHardwareInterface::~HexapodRobotHardwareInterface() { + on_deactivate(rclcpp_lifecycle::State()); +} + +hardware_interface::CallbackReturn +HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init"); + + servos_powered_ = false; + + for (const hardware_interface::ComponentInfo &joint: info_.joints) { + + std::string i2c_address_parameter = joint.name + "_i2c_address"; + std::string channel_parameter = joint.name + "_channel"; + std::string counts_intersect_parameter = joint.name + "_counts_intersect"; + std::string counts_slope_parameter = joint.name + "_counts_slope"; + std::string counts_min_parameter = joint.name + "_counts_min"; + std::string counts_max_parameter = joint.name + "_counts_max"; + std::string startup_value_parameter = joint.name + "_startup_value"; + + PWMOutput pwm_output; + pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0); + pwm_output.channel = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0); + pwm_output.counts_intersect = stod(info_.hardware_parameters[counts_intersect_parameter]); + pwm_output.counts_slope = stod(info_.hardware_parameters[counts_slope_parameter]); + pwm_output.counts_min = stod(info_.hardware_parameters[counts_min_parameter]); + pwm_output.counts_max = stod(info_.hardware_parameters[counts_max_parameter]); + joint_commands_[joint.name] = stod(info_.hardware_parameters[startup_value_parameter]); + + pca_available_[pwm_output.i2c_address] = false; + joint_pwm_mapping_[joint.name] = pwm_output; + + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d. intersect: %.4f slope: %.4f", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel, pwm_output.counts_intersect, pwm_output.counts_slope); + } + + + i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]); + if(i2c_available_) pca_initialize_(); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +bool +HexapodRobotHardwareInterface::i2c_open_(const std::string& device) { + i2c_fd_ = open(device.c_str(), O_RDWR); + if (i2c_fd_ < 0) { + RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str()); + return false; + } + return true; +} + +void +HexapodRobotHardwareInterface::pca_initialize_() { + for (const auto& [i2c_address, available]: pca_available_) { + + ioctl(i2c_fd_, I2C_SLAVE, i2c_address); + + if(i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, 0x00) < 0) { + RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Could not open i2c device %02x, marking as unavailable.", i2c_address); + pca_available_[i2c_address] = false; + } else { + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, pca_mode1_sleep_); + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode2_, pca_mode2_outdrv_); + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_prescale_, (uint8_t)pca_prescaler_value_); + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_mode1_, pca_mode1_restart_); + + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "PCA9685 at i2c 0x%02x initialized.", i2c_address); + pca_available_[i2c_address] = true; + } + } +} + +bool +HexapodRobotHardwareInterface::pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts) { + // if(i2c_address == 0x40 && channel_number == 0) { + // RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm(%02x, %02x, %04x)", i2c_address, channel_number, counts); + // } + if(channel_number>15) { + RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: channel %02x >15", channel_number); + return false; + } + if(counts > 0x1000) { + RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: counts %04x >15", counts); + return false; + } + if(!pca_available_[i2c_address]) { + // RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: i2c_address %02x unavailable.", i2c_address); + return false; + } + + if(ioctl(i2c_fd_, I2C_SLAVE, i2c_address) < 0) { + RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "pca_set_pwm: ioctl failed."); + return false; + } + + // adding channel_number * 4 to the base address for LED0 output given by the constants + // yields the addresses of all PWM channels + + if(counts == 0) { + // output always off + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00); // ON_L + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x00); // ON_H + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x10); // OFF_H == 0x10 => LED full OFF + } else if(counts == 0x1000) { + // output always on + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00); // ON_L + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x10); // ON_H == 0x10 => LED full ON + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, 0x00); // OFF_L + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, 0x00); // OFF_H + } else { + // duty cycle from 0 to counts + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_h_ + channel_number * 4, 0x00); + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_on_l_ + channel_number * 4, 0x00); + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_l_ + channel_number * 4, counts & 0xff); // lower byte + i2c_smbus_write_byte_data(i2c_fd_, pca_reg_led_off_h_ + channel_number * 4, counts >> 8); // shift upper byte into lower byte + } + return true; +} + +hardware_interface::CallbackReturn +HexapodRobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_activate"); + servos_powered_ = true; + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn +HexapodRobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) { + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_deactivate"); + servos_powered_ = false; + write(rclcpp::Time(), rclcpp::Duration(std::chrono::nanoseconds(0))); + return hardware_interface::CallbackReturn::SUCCESS; +} + + +std::vector +HexapodRobotHardwareInterface::export_state_interfaces() { + std::vector state_interfaces; + for (const hardware_interface::ComponentInfo &joint: info_.joints) { + state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name])); + } + return state_interfaces; +} + +std::vector +HexapodRobotHardwareInterface::export_command_interfaces() { + std::vector command_interfaces; + for (const hardware_interface::ComponentInfo &joint: info_.joints) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name])); + } + return command_interfaces; +} + + +hardware_interface::return_type +HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) { + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) { + for (const hardware_interface::ComponentInfo &joint: info_.joints) { + const PWMOutput &pwm_output = joint_pwm_mapping_[joint.name]; + if(servos_powered_) { + double pwm_value = pwm_output.counts_intersect + joint_commands_[joint.name] * pwm_output.counts_slope; + if(pwm_value < pwm_output.counts_min) { + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too low, clipped to %.2f", pwm_value, pwm_output.counts_min); + pwm_value = pwm_output.counts_min; + } + if(pwm_value > pwm_output.counts_max) { + RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "write: pwm_value %.2f too high, clipped to %.2f", pwm_value, pwm_output.counts_max); + pwm_value = pwm_output.counts_max; + } + pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, (uint16_t)pwm_value); + } else { + pca_set_pwm_(pwm_output.i2c_address, pwm_output.channel, 0); + } + } + return hardware_interface::return_type::OK; +} + +} // namespace + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(hexapod_robot::HexapodRobotHardwareInterface, hardware_interface::SystemInterface) diff --git a/hexapod_hardware/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp b/hexapod_hardware/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp new file mode 100644 index 0000000..df44042 --- /dev/null +++ b/hexapod_hardware/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp @@ -0,0 +1,91 @@ +#ifndef HEXAPOD_ROBOT_HWI_ +#define HEXAPOD_ROBOT_HWI_ + +#include +#include +#include +#include +#include + +#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 "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace hexapod_robot { + +struct PWMOutput { + uint8_t i2c_address; + uint8_t channel; + double counts_intersect; + double counts_slope; + double counts_min; + double counts_max; +}; + +class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface { + +public: + RCLCPP_SHARED_PTR_DEFINITIONS(HexapodRobotHardwareInterface) + virtual ~HexapodRobotHardwareInterface(); + + hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + bool servos_powered_; + + std::map joint_commands_; + std::map joint_pwm_mapping_; + + bool i2c_available_; + int i2c_fd_; + + std::map pca_available_; + + bool i2c_open_(const std::string& device); + + const uint8_t pca_reg_mode1_ = 0x00; + const uint8_t pca_reg_mode2_ = 0x01; + const uint8_t pca_reg_prescale_ = 0xfe; + + // base address for LED0 following, + // other channels can be calculated by adding + // channel_number * 4 + // to each base address + + const uint8_t pca_reg_led_on_l_ = 0x06; + const uint8_t pca_reg_led_on_h_ = 0x07; + const uint8_t pca_reg_led_off_l_ = 0x08; + const uint8_t pca_reg_led_off_h_ = 0x09; + + const uint8_t pca_mode1_sleep_ = 0x10; + const uint8_t pca_mode1_restart_ = 0x80; + const uint8_t pca_mode2_outdrv_ = 0x04; + + + const double pca_osc_frequency_ = 25000000; // 25 Mhz + const double pca_pwm_frequency_ = 50; // 50 Hz + const double pca_prescaler_value_ = (pca_osc_frequency_ / (4096.0 * pca_pwm_frequency_) - 0.5); // according to data sheet + const double pca_pwm_cycle_usec_ = 1000000 / pca_pwm_frequency_; // microseconds per PWM counter cycle + const double pca_pwm_count_usec_ = pca_pwm_cycle_usec_ / 4096; // microseconds per PWM single count + + void pca_initialize_(); + bool pca_set_pwm_(uint8_t i2c_address, uint8_t channel_number, uint16_t counts); + +}; + +} // namespace + +#endif // HEXAPOD_ROBOT_HWI_ diff --git a/hexapod_hardware/hexapod_hardware.xml b/hexapod_hardware/hexapod_hardware.xml new file mode 100644 index 0000000..141c640 --- /dev/null +++ b/hexapod_hardware/hexapod_hardware.xml @@ -0,0 +1,7 @@ + + + Hexapod Hardware Interface + + diff --git a/hexapod_hardware/package.xml b/hexapod_hardware/package.xml new file mode 100644 index 0000000..eb3093d --- /dev/null +++ b/hexapod_hardware/package.xml @@ -0,0 +1,18 @@ + + + + hexapod_hardware + 0.9.0 + ROS2 Control Hardware Interface Plugin + Marcus Grieger + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hexapod_moveit/.setup_assistant b/hexapod_moveit/.setup_assistant new file mode 100644 index 0000000..b450ed8 --- /dev/null +++ b/hexapod_moveit/.setup_assistant @@ -0,0 +1,10 @@ +moveit_setup_assistant_config: + urdf: + package: hexapod_robot + relative_path: config/urdf/hexapod_robot.urdf.xacro + srdf: + relative_path: config/hexapod_robot.srdf + package_settings: + author_name: Marcus Grieger + author_email: marcus@grieger.xyz + generated_timestamp: 1707046355 \ No newline at end of file diff --git a/hexapod_moveit/CMakeLists.txt b/hexapod_moveit/CMakeLists.txt new file mode 100644 index 0000000..8d0e0d1 --- /dev/null +++ b/hexapod_moveit/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(hexapod_robot_moveit) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/hexapod_moveit/COLCON_IGNORE b/hexapod_moveit/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/hexapod_moveit/config/hexapod_robot.rviz b/hexapod_moveit/config/hexapod_robot.rviz new file mode 100644 index 0000000..1ec916d --- /dev/null +++ b/hexapod_moveit/config/hexapod_robot.rviz @@ -0,0 +1,669 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 378 + - Class: rviz_common/Help + Name: Help + - 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 + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + 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 + c1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.05000000074505806 + Joint Violation Color: 255; 0; 255 + Planning Group: e1_group + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + 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 + c1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + 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 + c1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + c6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + e6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + f6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + t6_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + c1_link: + Value: true + c2_link: + Value: true + c3_link: + Value: true + c4_link: + Value: true + c5_link: + Value: true + c6_link: + Value: true + e1_link: + Value: true + e2_link: + Value: true + e3_link: + Value: true + e4_link: + Value: true + e5_link: + Value: true + e6_link: + Value: true + f1_link: + Value: true + f2_link: + Value: true + f3_link: + Value: true + f4_link: + Value: true + f5_link: + Value: true + f6_link: + Value: true + t1_link: + Value: true + t2_link: + Value: true + t3_link: + Value: true + t4_link: + Value: true + t5_link: + Value: true + t6_link: + Value: true + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + c1_link: + f1_link: + t1_link: + e1_link: + {} + c2_link: + f2_link: + t2_link: + e2_link: + {} + c3_link: + f3_link: + t3_link: + e3_link: + {} + c4_link: + f4_link: + t4_link: + e4_link: + {} + c5_link: + f5_link: + t5_link: + e5_link: + {} + c6_link: + f6_link: + t6_link: + e6_link: + {} + Update Interval: 0 + Value: 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 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.6329568028450012 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.07758110761642456 + Y: 0.08254023641347885 + Z: 0.09184139221906662 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5400001406669617 + Target Frame: base_link + Value: Orbit (rviz_default_plugins) + Yaw: 2.3151493072509766 + Saved: ~ +Window Geometry: + " - Trajectory Slider": + collapsed: false + Displays: + collapsed: false + Height: 1287 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002b4000004a9fc0200000006fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004400fffffffb000000100044006900730070006c006100790073010000003f00000200000000cc00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000245000001a10000019600fffffffb0000000800480065006c0070000000029a0000006e0000006f00fffffffb0000000a0056006900650077007301000003ec000000fc000000a900fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004400ffffff0000063e000004a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 2296 + X: 0 + Y: 32 diff --git a/hexapod_moveit/config/hexapod_robot.srdf b/hexapod_moveit/config/hexapod_robot.srdf new file mode 100644 index 0000000..df01027 --- /dev/null +++ b/hexapod_moveit/config/hexapod_robot.srdf @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/hexapod_moveit/config/hexapod_robot.urdf.xacro b/hexapod_moveit/config/hexapod_robot.urdf.xacro new file mode 100644 index 0000000..70734f8 --- /dev/null +++ b/hexapod_moveit/config/hexapod_robot.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/hexapod_moveit/config/joint_limits.yaml b/hexapod_moveit/config/joint_limits.yaml new file mode 100644 index 0000000..33387dc --- /dev/null +++ b/hexapod_moveit/config/joint_limits.yaml @@ -0,0 +1,7 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + diff --git a/hexapod_moveit/config/kinematics.yaml b/hexapod_moveit/config/kinematics.yaml new file mode 100644 index 0000000..aa4f5c6 --- /dev/null +++ b/hexapod_moveit/config/kinematics.yaml @@ -0,0 +1,77 @@ +e1_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +e2_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +e3_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +e4_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +e5_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +e6_group: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + position_scale: 1.0 + rotation_scale: 0.0 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 diff --git a/hexapod_moveit/config/moveit_controllers.yaml b/hexapod_moveit/config/moveit_controllers.yaml new file mode 100644 index 0000000..1f4dfde --- /dev/null +++ b/hexapod_moveit/config/moveit_controllers.yaml @@ -0,0 +1,81 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - e1_group_controller + - e2_group_controller + - e3_group_controller + - e4_group_controller + - e5_group_controller + - e6_group_controller + + e1_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c1_joint + - f1_joint + - t1_joint + action_ns: follow_joint_trajectory + default: true + + e2_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c2_joint + - f2_joint + - t2_joint + action_ns: follow_joint_trajectory + default: true + + e3_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c3_joint + - f3_joint + - t3_joint + action_ns: follow_joint_trajectory + default: true + + e4_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c4_joint + - f4_joint + - t4_joint + action_ns: follow_joint_trajectory + default: true + + e5_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c5_joint + - f5_joint + - t5_joint + action_ns: follow_joint_trajectory + default: true + + e6_group_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - c6_joint + - f6_joint + - t6_joint + action_ns: follow_joint_trajectory + default: true + + + \ No newline at end of file diff --git a/hexapod_moveit/config/pilz_cartesian_limits.yaml b/hexapod_moveit/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/hexapod_moveit/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/hexapod_moveit/config/ros2_controllers.yaml b/hexapod_moveit/config/ros2_controllers.yaml new file mode 100644 index 0000000..c0ae99d --- /dev/null +++ b/hexapod_moveit/config/ros2_controllers.yaml @@ -0,0 +1,103 @@ +controller_manager: + ros__parameters: + update_rate: 50 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + e1_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + e2_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + e3_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + e4_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + e5_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + e6_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +e1_group_controller: + ros__parameters: + joints: + - c1_joint + - f1_joint + - t1_joint + + command_interfaces: + - position + + state_interfaces: + - position + +e2_group_controller: + ros__parameters: + joints: + - c2_joint + - f2_joint + - t2_joint + + command_interfaces: + - position + + state_interfaces: + - position + +e3_group_controller: + ros__parameters: + joints: + - c3_joint + - f3_joint + - t3_joint + + command_interfaces: + - position + + state_interfaces: + - position + +e4_group_controller: + ros__parameters: + joints: + - c4_joint + - f4_joint + - t4_joint + + command_interfaces: + - position + + state_interfaces: + - position + +e5_group_controller: + ros__parameters: + joints: + - c5_joint + - f5_joint + - t5_joint + + command_interfaces: + - position + + state_interfaces: + - position + +e6_group_controller: + ros__parameters: + joints: + - c6_joint + - f6_joint + - t6_joint + + command_interfaces: + - position + + state_interfaces: + - position diff --git a/hexapod_moveit/launch/move_group.launch.py b/hexapod_moveit/launch/move_group.launch.py new file mode 100644 index 0000000..7ca0180 --- /dev/null +++ b/hexapod_moveit/launch/move_group.launch.py @@ -0,0 +1,85 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + moveit_config = ( + MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") + + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines(pipelines = ["ompl"]) + .to_moveit_configs() + ) + + move_group_node = Node( + package = "moveit_ros_move_group", + executable = "move_group", + output = "screen", + parameters = [moveit_config.to_dict()] + ) + + ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) + + robot_state_publisher_node = Node( + package = "robot_state_publisher", + executable = "robot_state_publisher", + parameters = [moveit_config.robot_description], + output = "both" + ) + + ros2_control_node = Node( + package = "controller_manager", + executable = "ros2_control_node", + parameters = [moveit_config.robot_description, ros2_controllers], + output="both" + ) + + joint_state_broadcaster_spawner = Node ( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] + ) + + joint_trajectory_controller_spawner = Node( + package = "controller_manager", + executable = "spawner", + arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"] + ) + + + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [rviz_node] + ) + ) + + + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [joint_trajectory_controller_spawner] + ) + ) + + + nodes = [ + robot_state_publisher_node, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner + ] + + return LaunchDescription(nodes) + \ No newline at end of file diff --git a/hexapod_moveit/launch/move_group_gui.launch.py b/hexapod_moveit/launch/move_group_gui.launch.py new file mode 100644 index 0000000..7bd9990 --- /dev/null +++ b/hexapod_moveit/launch/move_group_gui.launch.py @@ -0,0 +1,96 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + moveit_config = ( + MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") + + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines(pipelines = ["ompl"]) + .to_moveit_configs() + ) + + move_group_node = Node( + package = "moveit_ros_move_group", + executable = "move_group", + output = "screen", + parameters = [moveit_config.to_dict()] + ) + + ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) + + robot_state_publisher_node = Node( + package = "robot_state_publisher", + executable = "robot_state_publisher", + parameters = [moveit_config.robot_description], + output = "both" + ) + + ros2_control_node = Node( + package = "controller_manager", + executable = "ros2_control_node", + parameters = [moveit_config.robot_description, ros2_controllers], + output="both" + ) + + rviz_node = Node( + package = "rviz2", + executable = "rviz2", + arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])], + name = "rviz2", + output = "screen", + parameters = [moveit_config.planning_pipelines, moveit_config.robot_description_kinematics] + ) + + joint_state_broadcaster_spawner = Node ( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] + ) + + joint_trajectory_controller_spawner = Node( + package = "controller_manager", + executable = "spawner", + arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"] + ) + + + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [rviz_node] + ) + ) + + + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [joint_trajectory_controller_spawner] + ) + ) + + + nodes = [ + robot_state_publisher_node, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner, + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner + ] + + return LaunchDescription(nodes) + \ No newline at end of file diff --git a/hexapod_moveit/launch/rviz.launch.py b/hexapod_moveit/launch/rviz.launch.py new file mode 100644 index 0000000..e629c08 --- /dev/null +++ b/hexapod_moveit/launch/rviz.launch.py @@ -0,0 +1,37 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + moveit_config = ( + MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot") + + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines(pipelines = ["ompl"]) + .to_moveit_configs() + ) + + rviz_node = Node( + package = "rviz2", + executable = "rviz2", + arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])], + name = "rviz2", + output = "screen", + parameters = [moveit_config.planning_pipelines, moveit_config.robot_description_kinematics] + ) + nodes = [ + rviz_node + ] + + return LaunchDescription(nodes) + \ No newline at end of file diff --git a/hexapod_moveit/package.xml b/hexapod_moveit/package.xml new file mode 100644 index 0000000..fed522f --- /dev/null +++ b/hexapod_moveit/package.xml @@ -0,0 +1,52 @@ + + + + hexapod_robot_moveit + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the hexapod_robot with the MoveIt Motion Planning Framework + + mg + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + mg + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + hexapod_robot + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + + diff --git a/hexapod_python/.gitignore b/hexapod_python/.gitignore new file mode 100644 index 0000000..a295864 --- /dev/null +++ b/hexapod_python/.gitignore @@ -0,0 +1,2 @@ +*.pyc +__pycache__ diff --git a/hexapod_python/LICENSE b/hexapod_python/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/hexapod_python/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/hexapod_python/hexapod_python/__init__.py b/hexapod_python/hexapod_python/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hexapod_python/hexapod_python/ik.py b/hexapod_python/hexapod_python/ik.py new file mode 100644 index 0000000..e6b2a3b --- /dev/null +++ b/hexapod_python/hexapod_python/ik.py @@ -0,0 +1,97 @@ +import math + +from geometry_msgs.msg import Point, Pose, Quaternion +from trajectory_msgs.msg import JointTrajectoryPoint + + +leg_offsets = { + 'e1': Point(x = -40.0, y = 80.0, z = 25.0), + 'e2': Point(x = -60.0, y = 0.0, z = 25.0), + 'e3': Point(x = -40.0, y = -80.0, z = 25.0), + 'e4': Point(x = 40.0, y = -80.0, z = 25.0), + 'e5': Point(x = 60.0, y = 0.0, z = 25.0), + 'e6': Point(x = 40.0, y = 80.0, z = 25.0) +} + +leg_sides = { + 'e1': 'l', + 'e2': 'l', + 'e3': 'l', + 'e4': 'r', + 'e5': 'r', + 'e6': 'r', +} + + +def ik(body_pose): + # todo apply rotation and translation of body + + joint_values = {} + for l in body_pose['legs']: + joint_values[l] = leg_ik( + leg_sides[l], + Point( + x = body_pose['legs'][l].x - leg_offsets[l].x - body_pose['body'].position.x, + y = body_pose['legs'][l].y - leg_offsets[l].y - body_pose['body'].position.y, + z = body_pose['legs'][l].z - leg_offsets[l].z - body_pose['body'].position.z, + ) + ) + + return joint_values + +def leg_ik(side, point): + + x = point.x + y = point.y + z = point.z + + c = 20 + f = 80 + t = 130 + + ff = f*f + tt = t*t + + if side == 'l': + x = -x + + coxa_angle = math.atan2(y,x) + + r = math.sqrt(x*x + y * y) - c + + rr = r*r + zz = z*z + dd = rr + zz + d = math.sqrt(dd) + + femur_angle = math.acos((ff + dd - tt) / (2 * f * d)) + math.acos(-z/d) - math.pi/2 + tibia_angle = math.acos((tt + ff - dd) / (2 * t * f)) - math.pi/2 + + if(side == 'l'): + coxa_angle *= -1 + femur_angle *= -1 + else: + tibia_angle *= -1 + + + return([coxa_angle, femur_angle, tibia_angle]) + +if __name__ == "__main__": + print(leg_ik('l', Point(x = -100.0, y = 0.0, z = -130.0))) + print(leg_ik('r', Point(x = 100.0, y = 0.0, z = -130.0))) + print(leg_ik('l', Point(x = -120.0, y = 0.0, z = -130.0))) + print(leg_ik('r', Point(x = 120.0, y = 0.0, z = -130.0))) + + for o in leg_offsets: + print(leg_offsets[o]) + + + print(ik(standing_pose)) + print(ik(shutdown_pose)) + + + poses = interpolate_pose(shutdown_pose, standing_pose , 10) + for p in poses: + print(p) + print(ik(p)) + print() diff --git a/hexapod_python/hexapod_python/ik_node.py b/hexapod_python/hexapod_python/ik_node.py new file mode 100644 index 0000000..05b21b1 --- /dev/null +++ b/hexapod_python/hexapod_python/ik_node.py @@ -0,0 +1,123 @@ +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +from hexapod_python.poses import shutdown_pose, interpolate_pose +from hexapod_python.ik import ik + +import time +import copy +import functools + +class IKNode(Node): + def __init__(self, node_name = "ik_node"): + super().__init__(node_name) + + self.leg_groups_ = [ "e1", "e2", "e3", "e4", "e5", "e6"] + self.legs_active_ = False + + self.joint_names = [] + for leg in self.leg_groups_: + self.joint_names += [ f"c{leg[1:]}_joint", f"f{leg[1:]}_joint", f"t{leg[1:]}_joint" ] + + + self.action_client_ = ActionClient(self, FollowJointTrajectory, "/joint_group_controller/follow_joint_trajectory") + + self.current_pose_ = copy.deepcopy(shutdown_pose) + self.move_to_pose(shutdown_pose) + + def is_moving(self): + return self.legs_active_ + + def move_to_pose(self, target_pose, interpolation_steps = 50, modulation_function = {}): + if(self.is_moving()): + print("move_to_pose: already moving") + return + + self.follow_poses(interpolate_pose(self.current_pose_, target_pose, interpolation_steps, modulation_function)) + + def move_to_poses(self, target_poses, interpolation_steps = 50, modulation_function = {}): + if(self.is_moving()): + print("move_to_pose: already moving") + return + + poses = [] + t = self.current_pose_ + for p in target_poses: + poses += interpolate_pose(t, p, interpolation_steps, modulation_function) + t = p + + self.follow_poses(poses) + + + def follow_poses(self, poses): + self.target_pose_ = poses[-1] + ik_values = [ik(pose) for pose in poses] + trajectory = self.create_joint_trajectory(ik_values) + + goal = FollowJointTrajectory.Goal(trajectory = trajectory) + self.action_client_.wait_for_server() + + self.legs_active_ = True + self.response_future = self.action_client_.send_goal_async(goal) + self.response_future.add_done_callback(self.response_callback) + + def response_callback(self, future): + goal_handle = future.result() + if goal_handle.accepted: + self.result_futures = future.result().get_result_async() + self.result_futures.add_done_callback(self.result_callback) + + def result_callback(self, future): + self.legs_active_ = False + self.current_pose_ = self.target_pose_ + + + def create_joint_trajectory(self, ik_values): + + t = 0 + joint_trajectory_points = [] + for iv in ik_values: + joint_trajectory_positions = [] + for leg in self.leg_groups_: + joint_trajectory_positions += iv[leg] + + joint_trajectory_points.append( + JointTrajectoryPoint( + positions = joint_trajectory_positions, + time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50)) + ) + ) + t += 1 + + + return JointTrajectory( + joint_names = self.joint_names, + points = joint_trajectory_points + ) + + + def publish_ik_values(self, ik_values): + for leg in self.leg_groups_: + t = 0 + joint_trajectory_points = [] + for i in ik_values: + joint_trajectory_points.append( + JointTrajectoryPoint( + positions = i[leg], + time_from_start = Duration(nanosec = t%50 * 20000000 , sec = int(t/50)) + ) + ) + t += 1 + + + group_num = leg[1:] + trajectory = JointTrajectory( + joint_names = [ f"c{group_num}_joint", f"f{group_num}_joint", f"t{group_num}_joint" ], + points = joint_trajectory_points + ) + self.publisher_[leg].publish(trajectory) diff --git a/hexapod_python/hexapod_python/poses.py b/hexapod_python/hexapod_python/poses.py new file mode 100644 index 0000000..b442335 --- /dev/null +++ b/hexapod_python/hexapod_python/poses.py @@ -0,0 +1,144 @@ +from geometry_msgs.msg import Point, Pose, Quaternion +from hexapod_python.ik import leg_offsets + +import copy +import math + +standing_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = 80.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0) + ), + 'legs': { + 'e1': Point(x = leg_offsets['e1'].x + -50.0, y = leg_offsets['e1'].y + 86.0, z = 0.0), + 'e2': Point(x = leg_offsets['e2'].x + -100.0, y = leg_offsets['e2'].y + 0.0, z = 0.0), + 'e3': Point(x = leg_offsets['e3'].x + -50.0, y = leg_offsets['e3'].y - 86.0, z = 0.0), + 'e4': Point(x = leg_offsets['e4'].x + 50.0, y = leg_offsets['e4'].y - 86.0, z = 0.0), + 'e5': Point(x = leg_offsets['e5'].x + 100.0, y = leg_offsets['e5'].y + 0.0, z = 0.0), + 'e6': Point(x = leg_offsets['e6'].x + 50.0, y = leg_offsets['e6'].y + 86.0, z = 0.0) + } +} + +stand_up_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = 80.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0) + ), + 'legs': { + 'e1': Point(x = leg_offsets['e1'].x + -70.0, y = leg_offsets['e1'].y + 121.0, z = 0.0), + 'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y + 0.0, z = 0.0), + 'e3': Point(x = leg_offsets['e3'].x + -70.0, y = leg_offsets['e3'].y - 121.0, z = 0.0), + 'e4': Point(x = leg_offsets['e4'].x + 70.0, y = leg_offsets['e4'].y - 121.0, z = 0.0), + 'e5': Point(x = leg_offsets['e5'].x + 140.0, y = leg_offsets['e5'].y + 0.0, z = 0.0), + 'e6': Point(x = leg_offsets['e6'].x + 70.0, y = leg_offsets['e6'].y + 121.0, z = 0.0) + } +} + + + +shutdown_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = -5.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0) + ), + 'legs': { + 'e1': Point(x = leg_offsets['e1'].x + -70.0, y = leg_offsets['e1'].y + 121.0, z = 0.0), + 'e2': Point(x = leg_offsets['e2'].x + -140.0, y = leg_offsets['e2'].y + 0.0, z = 0.0), + 'e3': Point(x = leg_offsets['e3'].x + -70.0, y = leg_offsets['e3'].y - 121.0, z = 0.0), + 'e4': Point(x = leg_offsets['e4'].x + 70.0, y = leg_offsets['e4'].y - 121.0, z = 0.0), + 'e5': Point(x = leg_offsets['e5'].x + 140.0, y = leg_offsets['e5'].y + 0.0, z = 0.0), + 'e6': Point(x = leg_offsets['e6'].x + 70.0, y = leg_offsets['e6'].y + 121.0, z = 0.0) + } +} + +calibration_pose = { + 'body': Pose( + position = Point(x = 0.0, y = 0.0, z = 0.0 ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0) + ), + 'legs': { + 'e1': Point(x = -200.0, y = 150.0, z = 0.0), + 'e2': Point(x = -200.0, y = 0.0, z = 0.0), + 'e3': Point(x = -200.0, y = -150.0, z = 0.0), + + 'e4': Point(x = 200.0, y = -150.0, z = 0.0), + 'e5': Point(x = 200.0, y = 0.0, z = 0.0), + 'e6': Point(x = 200.0, y = 150.0, z = 0.0), + + } + +} + + +def interpolate_pose(start, end, steps, fn = {}): + + body_step_x = (end['body'].position.x - start['body'].position.x) / (steps-1) + body_step_y = (end['body'].position.y - start['body'].position.y) / (steps-1) + body_step_z = (end['body'].position.z - start['body'].position.z) / (steps-1) + + leg_step_x = {} + leg_step_y = {} + leg_step_z = {} + + for leg in start['legs']: + leg_step_x[leg] = (end['legs'][leg].x - start['legs'][leg].x) / (steps-1) + leg_step_y[leg] = (end['legs'][leg].y - start['legs'][leg].y) / (steps-1) + leg_step_z[leg] = (end['legs'][leg].z - start['legs'][leg].z) / (steps-1) + + poses = [] + for i in range(0, steps): + pose = { + 'body': Pose( + position = Point( + x = start['body'].position.x + body_step_x * i, + y = start['body'].position.y + body_step_y * i, + z = start['body'].position.z + body_step_z * i + ), + orientation = Quaternion(x = 0.0, y = 0.0, z = 0.0, w = 1.0) + ), + 'legs': {} + } + + for leg in start['legs']: + pose['legs'][leg] = Point( + x = start['legs'][leg].x + leg_step_x[leg] * i, + y = start['legs'][leg].y + leg_step_y[leg] * i, + z = start['legs'][leg].z + leg_step_z[leg] * i + ) + if leg in fn: + (fn_x, fn_y, fn_z) = fn[leg](i) + pose['legs'][leg].x += fn_x + pose['legs'][leg].y += fn_y + pose['legs'][leg].z += fn_z + + poses.append(pose) + + return poses + +def step_poses(start_pose, target_pose, legs_at_once, leg_index_offset, interpolation_steps, step_height): + poses = [] + current_pose = start_pose + + n_legs = len(start_pose['legs']) + n_cycles = int(n_legs / legs_at_once) + + for cycle in range(0, n_cycles): + intermediate_pose = copy.deepcopy(current_pose) + intermediate_pose['body'].position.x += (target_pose['body'].position.x - start_pose['body'].position.x) / n_cycles + intermediate_pose['body'].position.y += (target_pose['body'].position.y - start_pose['body'].position.y) / n_cycles + intermediate_pose['body'].position.z += (target_pose['body'].position.z - start_pose['body'].position.z) / n_cycles + step_fn = {} + for l in range(0, legs_at_once): # 0 .. 1 + leg_num = (cycle + 1 + n_cycles * l + leg_index_offset) + + while(leg_num > n_legs): + leg_num -= n_legs + + leg = f'e{leg_num}' + intermediate_pose['legs'][leg] = target_pose['legs'][leg] + step_fn[leg] = lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi)) + + poses += interpolate_pose(current_pose, intermediate_pose, interpolation_steps, step_fn) + current_pose = intermediate_pose + + return poses diff --git a/hexapod_python/hexapod_python/taranis_test.py b/hexapod_python/hexapod_python/taranis_test.py new file mode 100644 index 0000000..49a9201 --- /dev/null +++ b/hexapod_python/hexapod_python/taranis_test.py @@ -0,0 +1,132 @@ +import rclpy +from rclpy.node import Node + +from hexapod_python.ik_node import IKNode +from hexapod_python.poses import standing_pose, stand_up_pose, shutdown_pose, interpolate_pose, step_poses + +from taranis.msg import TaranisPacket + +import math +import time +import copy +import functools + + + +class TaranisTestNode(IKNode): + MODE_SHUTDOWN = 0 + MODE_STANDING = 1 + MODE_WALKING = 2 + + def __init__(self): + super().__init__('taranis_test') + + self.subscription = self.create_subscription( + TaranisPacket, + '/taranis_output', + self.taranis_callback, + 10 + ) + + self.mode = self.MODE_SHUTDOWN + + def taranis_callback(self, msg): + if self.is_moving(): + return + + if self.mode == self.MODE_SHUTDOWN: + if(msg.se == 1): + self.follow_poses( + interpolate_pose(self.current_pose_, stand_up_pose, 50) + + step_poses(stand_up_pose, standing_pose, 2, 0, 25, 20) + ) + self.mode = self.MODE_STANDING + print("-> MODE_STANDING") + + if self.mode == self.MODE_STANDING: + if(msg.se == 0): + self.follow_poses( + step_poses(self.current_pose_, stand_up_pose, 2, 0, 25, 20) + + interpolate_pose(stand_up_pose, shutdown_pose, 50) + ) + self.mode = self.MODE_SHUTDOWN + print("-> MODE_SHUTDOWN") + + if(msg.se == 2): + self.mode = self.MODE_WALKING + self.second_step = False + self.last_delta = 0 + self.walking_base_pose = copy.deepcopy(self.current_pose_) + print("-> MODE_WALKING") + + if(msg.se == 1): + if(msg.sf == 1): + target_pose = copy.deepcopy(self.current_pose_) + target_pose['body'].position.x = msg.aileron * 20.0 + target_pose['body'].position.y = msg.elevator * 20.0 + target_pose['body'].position.z = standing_pose['body'].position.z + msg.throttle * 20 + + delta = math.sqrt( + (target_pose['body'].position.x - self.current_pose_['body'].position.x)**2 + + (target_pose['body'].position.y - self.current_pose_['body'].position.y)**2 + + (target_pose['body'].position.z - self.current_pose_['body'].position.z)**2 + ) + + if delta > 0.2: + delta_steps = int(delta) * 2 + if(delta_steps < 2): + delta_steps = 2; + self.move_to_pose(target_pose, delta_steps) + + + if self.mode == self.MODE_WALKING: + if(msg.se != 2 and self.last_delta == 0): + self.mode = self.MODE_STANDING + print("-> MODE_STANDING") + return + + delta_x = msg.aileron * 100.0 + delta_y = msg.elevator * 100.0 + + delta = math.sqrt(delta_x**2 + delta_y**2) + + if delta < 1: + delta_x = 0.0 + delta_y = 0.0 + + if delta > 10 or self.last_delta > 0: + step_height = int(50 + 50 * msg.s1) + interpolation_steps = int(60 + 50 * msg.s2) + + fn = lambda i : (0, 0, step_height * math.sin(i/(interpolation_steps-1) * math.pi)) + + lift_leg_group = ['e1', 'e3', 'e5'] + step_pose = copy.deepcopy(self.walking_base_pose) + step_fn = {} + + for leg in self.leg_groups_: + if (leg in lift_leg_group) != self.second_step : + step_pose['legs'][leg].x += delta_x / 2 + step_pose['legs'][leg].y += delta_y / 2 + step_fn[leg] = fn + else: + step_pose['legs'][leg].x -= delta_x / 2 + step_pose['legs'][leg].y -= delta_y / 2 + + poses = interpolate_pose(self.current_pose_, step_pose, interpolation_steps, step_fn) + self.follow_poses(poses) + self.second_step = not self.second_step + self.last_delta = delta + + + + + +def main(args = None): + rclpy.init(args=args) + + ik_publisher = TaranisTestNode() + rclpy.spin(ik_publisher) + ik_publisher.destroy_node() + rclpy.shutdown() + \ No newline at end of file diff --git a/hexapod_python/package.xml b/hexapod_python/package.xml new file mode 100644 index 0000000..d948cf3 --- /dev/null +++ b/hexapod_python/package.xml @@ -0,0 +1,25 @@ + + + + hexapod_python + 0.0.0 + TODO: Package description + wuselfuzz + MIT + + taranis + + rclpy + std_msgs + trajectory_msgs + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/hexapod_python/resource/hexapod_python b/hexapod_python/resource/hexapod_python new file mode 100644 index 0000000..e69de29 diff --git a/hexapod_python/setup.cfg b/hexapod_python/setup.cfg new file mode 100644 index 0000000..e870c2c --- /dev/null +++ b/hexapod_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/hexapod_python +[install] +install_scripts=$base/lib/hexapod_python diff --git a/hexapod_python/setup.py b/hexapod_python/setup.py new file mode 100644 index 0000000..594e752 --- /dev/null +++ b/hexapod_python/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = 'hexapod_python' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='wuselfuzz', + maintainer_email='marcus@grieger.xyz', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'jtc_test = hexapod_python.jtc_test:main', + 'ik_test = hexapod_python.ik_test:main', + 'ik_calibration = hexapod_python.ik_calibration:main', + 'step_test = hexapod_python.step_test:main', + 'taranis_test = hexapod_python.taranis_test:main' + ], + }, +) diff --git a/hexapod_python/test/test_copyright.py b/hexapod_python/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/hexapod_python/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 +# +# http://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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/hexapod_python/test/test_flake8.py b/hexapod_python/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/hexapod_python/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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 +# +# http://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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/hexapod_python/test/test_pep257.py b/hexapod_python/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/hexapod_python/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 +# +# http://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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/hexapod_robot/CMakeLists.txt b/hexapod_robot/CMakeLists.txt new file mode 100644 index 0000000..d0777b7 --- /dev/null +++ b/hexapod_robot/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(hexapod_robot) + +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) + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/hexapod_robot/config/ros2_controllers.yaml b/hexapod_robot/config/ros2_controllers.yaml new file mode 100644 index 0000000..65ae792 --- /dev/null +++ b/hexapod_robot/config/ros2_controllers.yaml @@ -0,0 +1,39 @@ +controller_manager: + ros__parameters: + update_rate: 50 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_group_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_group_controller: + ros__parameters: + joints: + - c1_joint + - f1_joint + - t1_joint + - c2_joint + - f2_joint + - t2_joint + - c3_joint + - f3_joint + - t3_joint + - c4_joint + - f4_joint + - t4_joint + - c5_joint + - f5_joint + - t5_joint + - c6_joint + - f6_joint + - t6_joint + + command_interfaces: + - position + + state_interfaces: + - position + + interpolation_method: none diff --git a/hexapod_robot/config/urdf/hexapod_robot.urdf.xacro b/hexapod_robot/config/urdf/hexapod_robot.urdf.xacro new file mode 100644 index 0000000..532aab9 --- /dev/null +++ b/hexapod_robot/config/urdf/hexapod_robot.urdf.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hexapod_robot/config/urdf/hexapod_robot_macros.xacro b/hexapod_robot/config/urdf/hexapod_robot_macros.xacro new file mode 100644 index 0000000..802a899 --- /dev/null +++ b/hexapod_robot/config/urdf/hexapod_robot_macros.xacro @@ -0,0 +1,179 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hexapod_robot/config/urdf/hexapod_robot_materials.xacro b/hexapod_robot/config/urdf/hexapod_robot_materials.xacro new file mode 100644 index 0000000..1825e16 --- /dev/null +++ b/hexapod_robot/config/urdf/hexapod_robot_materials.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hexapod_robot/config/urdf/hexapod_robot_ros2_control.xacro b/hexapod_robot/config/urdf/hexapod_robot_ros2_control.xacro new file mode 100644 index 0000000..bb7533c --- /dev/null +++ b/hexapod_robot/config/urdf/hexapod_robot_ros2_control.xacro @@ -0,0 +1,78 @@ + + + + + + + + -1.57 + 1.57 + + + + + + + -1.57 + 1.57 + + + + + + -1.57 + 1.57 + + + + + + + ${i2c_address} + ${channel} + ${counts_intersect} + ${counts_slope} + ${counts_min} + ${counts_max} + ${startup_value} + + + + + + hexapod_robot/HexapodRobotHardwareInterface + /dev/i2c-1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hexapod_robot/launch/joint_trajectory_control.launch.py b/hexapod_robot/launch/joint_trajectory_control.launch.py new file mode 100644 index 0000000..ba829c4 --- /dev/null +++ b/hexapod_robot/launch/joint_trajectory_control.launch.py @@ -0,0 +1,71 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit + + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("hexapod_robot"), + "config", + "urdf", + "hexapod_robot.urdf.xacro" + ] + ) + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) + + robot_state_publisher_node = Node( + package = "robot_state_publisher", + executable = "robot_state_publisher", + parameters = [robot_description], + output = "both" + ) + + control_node = Node( + package = "controller_manager", + executable = "ros2_control_node", + parameters = [robot_description, robot_controllers], + output="both" + ) + + joint_state_broadcaster_spawner = Node ( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] + ) + + joint_trajectory_controller_spawner = Node( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_group_controller", "--controller-manager", "/controller_manager"] + ) + + + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [joint_trajectory_controller_spawner] + ) + ) + + nodes = [ + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner + ] + + return LaunchDescription(nodes) + \ No newline at end of file diff --git a/hexapod_robot/launch/taranis_control.launch.py b/hexapod_robot/launch/taranis_control.launch.py new file mode 100644 index 0000000..314f0a7 --- /dev/null +++ b/hexapod_robot/launch/taranis_control.launch.py @@ -0,0 +1,85 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.event_handlers import OnProcessExit + + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("hexapod_robot"), + "config", + "urdf", + "hexapod_robot.urdf.xacro" + ] + ) + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"]) + + robot_state_publisher_node = Node( + package = "robot_state_publisher", + executable = "robot_state_publisher", + parameters = [robot_description], + output = "both" + ) + + control_node = Node( + package = "controller_manager", + executable = "ros2_control_node", + parameters = [robot_description, robot_controllers], + output = "both" + ) + + taranis_node = Node( + package = "taranis", + executable = "taranis_node", + output = "both" + ) + + ik_node = Node( + package = "hexapod_python", + executable = "taranis_test", + output = "both" + ) + + joint_state_broadcaster_spawner = Node ( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"] + ) + + joint_trajectory_controller_spawner = Node( + package = "controller_manager", + executable = "spawner", + arguments = ["joint_group_controller", "--controller-manager", "/controller_manager"] + ) + + + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler = OnProcessExit( + target_action = joint_state_broadcaster_spawner, + on_exit = [joint_trajectory_controller_spawner] + ) + ) + + nodes = [ + control_node, + taranis_node, + ik_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner + ] + + return LaunchDescription(nodes) + \ No newline at end of file diff --git a/hexapod_robot/package.xml b/hexapod_robot/package.xml new file mode 100644 index 0000000..0cc794d --- /dev/null +++ b/hexapod_robot/package.xml @@ -0,0 +1,20 @@ + + + + hexapod_robot + 0.9.0 + ROS2 Robot Description and configuration + Marcus Grieger + MIT + + ament_cmake + + hexapod_hardware + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/taranis/CMakeLists.txt b/taranis/CMakeLists.txt new file mode 100644 index 0000000..1742db7 --- /dev/null +++ b/taranis/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.8) +project(taranis) + +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(rclcpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Build Taranis interfaces + +set(msg_files + "msg/TaranisPacket.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} +) + +# Build Taranis publisher node + +add_executable(taranis_node src/taranis_node.cpp) +ament_target_dependencies(taranis_node rclcpp) +rosidl_target_interfaces(taranis_node ${PROJECT_NAME} "rosidl_typesupport_cpp") + +target_include_directories(taranis_node PUBLIC + $ + $) +target_compile_features(taranis_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS taranis_node + DESTINATION lib/${PROJECT_NAME}) + +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() +endif() + +ament_package() diff --git a/taranis/LICENSE b/taranis/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/taranis/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/taranis/include/taranis/taranis_node.hpp b/taranis/include/taranis/taranis_node.hpp new file mode 100644 index 0000000..704edad --- /dev/null +++ b/taranis/include/taranis/taranis_node.hpp @@ -0,0 +1,44 @@ +#ifndef TARANIS_NODE_HPP_ +#define TARANIS_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "taranis/msg/taranis_packet.hpp" + +#include +#include +#include +#include +#include +#include + +namespace taranis { + +class TaranisNode : public rclcpp::Node { + public: + TaranisNode(); + + private: + int serial_fd_; + termios2 termios2_; + + bool initialize_serial_(); + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + void timer_callback_(); + void publish_packet_(); + + int empty_read_ = 0; + uint8_t buffer_index_ = 0; + + uint8_t buffer_[25]; + + double normalize_channel_(uint16_t); + uint8_t decode_switch_(uint16_t); +}; + + +} // namespace + +#endif // TARANIS_NODE_HPP_ \ No newline at end of file diff --git a/taranis/msg/TaranisPacket.msg b/taranis/msg/TaranisPacket.msg new file mode 100644 index 0000000..fff30d0 --- /dev/null +++ b/taranis/msg/TaranisPacket.msg @@ -0,0 +1,15 @@ +float64 aileron +float64 elevator +float64 throttle +float64 rudder + +float64 s1 +float64 s2 + +int8 se +int8 sf +int8 sg +int8 sh + +bool failsafe_active +bool signal_loss diff --git a/taranis/package.xml b/taranis/package.xml new file mode 100644 index 0000000..56b8797 --- /dev/null +++ b/taranis/package.xml @@ -0,0 +1,25 @@ + + + + taranis + 0.0.0 + TODO: Package description + Marcus Grieger + MIT + + rclcpp + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/taranis/src/taranis_node.cpp b/taranis/src/taranis_node.cpp new file mode 100644 index 0000000..c17382e --- /dev/null +++ b/taranis/src/taranis_node.cpp @@ -0,0 +1,173 @@ +#include "taranis/taranis_node.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +namespace taranis { + +TaranisNode::TaranisNode() : Node("taranis_node") { + publisher_ = this->create_publisher("/taranis_output", 10); + if(!initialize_serial_()) { + RCLCPP_ERROR(rclcpp::get_logger("TaranisNode"), "serial could not be initialized, exiting."); + return; + } else { + RCLCPP_INFO(rclcpp::get_logger("TaranisNode"), "serial port initialized successfully."); + } + + timer_ = this->create_wall_timer( + 2ms, + std::bind(&TaranisNode::timer_callback_, this) + ); +} + +bool +TaranisNode::initialize_serial_() { + + serial_fd_ = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NONBLOCK ); + if (serial_fd_ < 0) { + RCLCPP_WARN(rclcpp::get_logger("TaranisNode"), "cannot open serial."); + return false; + } + + if(ioctl(serial_fd_, TCGETS2, &termios2_) < 0) { + RCLCPP_WARN(rclcpp::get_logger("TaranisNode"), "ioctl TCGETS2 failed."); + return false; + } + + // Set up 8-O-2 at 100000 baud, no flow-control, no pi-pa-po + + termios2_.c_iflag &= ~(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF); + termios2_.c_iflag |= IGNBRK | IGNPAR; + + termios2_.c_oflag &= ~(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY); + termios2_.c_oflag |= NL0 | VT0; + + termios2_.c_cflag &= ~(CSIZE | CBAUD | (CBAUD << IBSHIFT)); + termios2_.c_cflag |= CS8 | CSTOPB | PARENB | PARODD | CREAD | CLOCAL | BOTHER | (BOTHER << IBSHIFT); + + termios2_.c_lflag &= ~(ISIG | ICANON | ECHO | TOSTOP | IEXTEN); + + termios2_.c_ispeed = 100000; + termios2_.c_ospeed = 100000; + + termios2_.c_cc[VTIME] = 0; + termios2_.c_cc[VMIN] = 0; + + if(ioctl(serial_fd_, TCSETS2, &termios2_) < 0) { + RCLCPP_WARN(rclcpp::get_logger("TaranisNode"), "ioctl TCSETS2 failed."); + return false; + } + + return true; +} + +void +TaranisNode::timer_callback_() { + int bytes_read; + bool did_read = false; + + do { + + if(empty_read_ > 2) { + buffer_index_ = 0; + empty_read_ = 0; + } + + bytes_read = read(serial_fd_, &buffer_[buffer_index_], 25-buffer_index_); + + if(bytes_read) { + buffer_index_ += bytes_read; + did_read = true; + if(buffer_index_ == 25) { + publish_packet_(); + buffer_index_ = 0; + } + } else { + if(!did_read) empty_read_ ++; + } + } + while(bytes_read); +} + +double +TaranisNode::normalize_channel_(uint16_t value) { + double float_value = value; + + float_value -= 992.0; // (shift center to 0.0) + float_value /= 820.0; + + if(float_value > 1.0) float_value = 1.0; + if(float_value < -1.0) float_value = -1.0; + + return float_value; +} + +uint8_t +TaranisNode::decode_switch_(uint16_t value) { + if(value < 582) return 0; + if(value < 1402) return 1; + return 2; +} + +void +TaranisNode::publish_packet_() { + if(buffer_[0] != 0x0f || buffer_[24] != 0x00) { + RCLCPP_WARN(rclcpp::get_logger("TaranisNode"), "invalid sbus frame received, dropping."); + return; + } + + uint16_t channels[16]; + + channels[ 0] = (buffer_[ 2] & 0b00000111) << 8 | buffer_[ 1]; + channels[ 1] = (buffer_[ 3] & 0b00111111) << 5 | (buffer_[ 2] & 0b11111000) >> 3; + channels[ 2] = (buffer_[ 5] & 0b00000001) << 10 | (buffer_[ 4] & 0b11111111) << 2 | (buffer_[3] & 0b11000000) >> 6; + channels[ 3] = (buffer_[ 6] & 0b00001111) << 7 | (buffer_[ 5] & 0b11111110) >> 1; + channels[ 4] = (buffer_[ 7] & 0b01111111) << 4 | (buffer_[ 6] & 0b11110000) >> 4; + channels[ 5] = (buffer_[ 9] & 0b00000011) << 9 | (buffer_[ 8] & 0b11111111) << 1 | (buffer_[7] & 0b10000000) >> 7; + channels[ 6] = (buffer_[10] & 0b00011111) << 6 | (buffer_[ 9] & 0b11111100) >> 2; + channels[ 7] = (buffer_[11] & 0b11111111) << 3 | (buffer_[10] & 0b11100000) >> 5; + + channels[ 8] = (buffer_[13] & 0b00000111) << 8 | buffer_[12]; + channels[ 9] = (buffer_[14] & 0b00111111) << 5 | (buffer_[13] & 0b11111000) >> 3; + channels[10] = (buffer_[16] & 0b00000001) << 10 | (buffer_[15] & 0b11111111) << 2 | (buffer_[14] & 0b11000000) >> 6; + channels[11] = (buffer_[17] & 0b00001111) << 7 | (buffer_[16] & 0b11111110) >> 1; + channels[12] = (buffer_[18] & 0b01111111) << 4 | (buffer_[17] & 0b11110000) >> 4; + channels[13] = (buffer_[20] & 0b00000011) << 9 | (buffer_[19] & 0b11111111) << 1 | (buffer_[18] & 0b10000000) >> 7; + channels[14] = (buffer_[21] & 0b00011111) << 6 | (buffer_[20] & 0b11111100) >> 2; + channels[15] = (buffer_[22] & 0b11111111) << 3 | (buffer_[21] & 0b11100000) >> 5; + + taranis::msg::TaranisPacket message = taranis::msg::TaranisPacket(); + + message.aileron = normalize_channel_(channels[0]); + message.elevator = normalize_channel_(channels[1]); + message.throttle = normalize_channel_(channels[2]); + message.rudder = normalize_channel_(channels[3]); + + message.se = decode_switch_(channels[4]); + message.sf = decode_switch_(channels[5]); + message.sg = decode_switch_(channels[6]); + message.sh = decode_switch_(channels[7]); + + message.s1 = normalize_channel_(channels[8]); + message.s2 = normalize_channel_(channels[9]); + + if(buffer_[23] & (1<<2)) message.signal_loss = true; + if(buffer_[23] & (1<<3)) message.failsafe_active = true; + + publisher_->publish(message); +} + +} // namespace + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +}