Unified projects in one repo

hexapod_hardware/.gitignore

@@ -0,0 +1,2 @@
+*~
+.vscode

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 
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+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()

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.

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 <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <stdint.h>
+
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+
+extern "C" {
+    #include <i2c/smbus.h>
+}
+
+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<hardware_interface::StateInterface>
+HexapodRobotHardwareInterface::export_state_interfaces() {
+    std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface>
+HexapodRobotHardwareInterface::export_command_interfaces() {
+    std::vector<hardware_interface::CommandInterface> 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)

hexapod_hardware/hardware/include/hexapod_robot/hexapod_robot_hwi.hpp

@@ -0,0 +1,91 @@
+#ifndef HEXAPOD_ROBOT_HWI_
+#define HEXAPOD_ROBOT_HWI_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_set>
+#include <map>
+
+#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<hardware_interface::StateInterface> export_state_interfaces() override;
+    std::vector<hardware_interface::CommandInterface> 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<std::string, double> joint_commands_;
+    std::map<std::string, PWMOutput> joint_pwm_mapping_;
+
+    bool i2c_available_;
+    int i2c_fd_;
+
+    std::map<uint8_t, bool> 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_

hexapod_hardware/hexapod_hardware.xml

@@ -0,0 +1,7 @@
+<library path="hexapod_robot_hwi">
+    <class name="hexapod_robot/HexapodRobotHardwareInterface"
+           type="hexapod_robot::HexapodRobotHardwareInterface"
+           base_class_type="hardware_interface::SystemInterface">
+        <description>Hexapod Hardware Interface</description>
+    </class>
+</library>

hexapod_hardware/package.xml

@@ -0,0 +1,18 @@
+<?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>hexapod_hardware</name>
+  <version>0.9.0</version>
+  <description>ROS2 Control Hardware Interface Plugin</description>
+  <maintainer email="marcus@grieger.xyz">Marcus Grieger</maintainer>
+  <license>MIT</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>

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

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

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: <Fixed 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

hexapod_moveit/config/hexapod_robot.srdf

@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="hexapod_robot">
+    <group name="e1_group">
+        <chain base_link="base_link" tip_link="e1_link"/>
+    </group> 
+    <group name="e2_group">
+        <chain base_link="base_link" tip_link="e2_link"/>
+    </group> 
+    <group name="e3_group">
+        <chain base_link="base_link" tip_link="e3_link"/>
+    </group> 
+    <group name="e4_group">
+        <chain base_link="base_link" tip_link="e4_link"/>
+    </group> 
+    <group name="e5_group">
+        <chain base_link="base_link" tip_link="e5_link"/>
+    </group> 
+    <group name="e6_group">
+        <chain base_link="base_link" tip_link="e6_link"/>
+    </group> 
+</robot>

hexapod_moveit/config/hexapod_robot.urdf.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import hexapod_robot urdf file -->
+    <xacro:include filename="$(find hexapod_robot)/config/urdf/hexapod_robot.urdf.xacro" />
+
+    <!-- Import control_xacro -->
+    <xacro:include filename="hexapod_robot.ros2_control.xacro" />
+
+
+    <xacro:hexapod_robot_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+
+</robot>

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
+

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

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

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

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

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

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

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

hexapod_moveit/package.xml

@@ -0,0 +1,52 @@
+<?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>hexapod_robot_moveit</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the hexapod_robot with the MoveIt Motion Planning Framework
+  </description>
+  <maintainer email="marcus@grieger.xyz">mg</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit2</url>
+
+  <author email="marcus@grieger.xyz">mg</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
+  <!-- <exec_depend>gazebo_ros_control</exec_depend> -->
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>hexapod_robot</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_common</exec_depend>
+  <exec_depend>rviz_default_plugins</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>warehouse_ros_mongo</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+
+  <export>
+      <build_type>ament_cmake</build_type>
+  </export>
+</package>

hexapod_python/.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+__pycache__

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.

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

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)

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

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

hexapod_python/package.xml

@@ -0,0 +1,25 @@
+<?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>hexapod_python</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="marcus@grieger.xyz">wuselfuzz</maintainer>
+  <license>MIT</license>
+
+  <depend>taranis</depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

hexapod_python/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/hexapod_python
+[install]
+install_scripts=$base/lib/hexapod_python

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'
+        ],
+    },
+)

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'

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)

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'

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

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

hexapod_robot/config/urdf/hexapod_robot.urdf.xacro

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
+    <xacro:include filename="$(find hexapod_robot)/config/urdf/hexapod_robot_materials.xacro"/>
+    <xacro:include filename="$(find hexapod_robot)/config/urdf/hexapod_robot_macros.xacro"/>
+
+    <link name="base_link">
+        <xacro:create_frame_plate z_offset="-0.034"/>
+        <xacro:create_frame_plate z_offset=" 0.034"/>
+        
+        <xacro:create_coxa_joint_mount xy_offset="-0.040  0.080"/>
+        <xacro:create_coxa_joint_mount xy_offset="-0.060  0.000"/>
+        <xacro:create_coxa_joint_mount xy_offset="-0.040 -0.080"/>
+
+        <xacro:create_coxa_joint_mount xy_offset=" 0.040 -0.080"/>
+        <xacro:create_coxa_joint_mount xy_offset=" 0.060  0.000"/>
+        <xacro:create_coxa_joint_mount xy_offset=" 0.040  0.080"/>
+    </link>
+
+    <xacro:create_leg number="1" side="l" xy_offset="-0.040   0.080"/>
+    <xacro:create_leg number="2" side="l" xy_offset="-0.060   0.000"/>
+    <xacro:create_leg number="3" side="l" xy_offset="-0.040  -0.080"/>
+    <xacro:create_leg number="4" side="r" xy_offset=" 0.040  -0.080"/>
+    <xacro:create_leg number="5" side="r" xy_offset=" 0.060   0.000"/>
+    <xacro:create_leg number="6" side="r" xy_offset=" 0.040   0.080"/>
+        
+    <xacro:include filename="$(find hexapod_robot)/config/urdf/hexapod_robot_ros2_control.xacro"/>
+
+</robot>

hexapod_robot/config/urdf/hexapod_robot_macros.xacro

@@ -0,0 +1,179 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="create_frame_plate" params="z_offset">
+        <visual>
+            <origin xyz="0 0 ${z_offset}"/>
+            <geometry>
+              <box size="0.100 0.180 0.003"/>        
+            </geometry>
+            <material name="black"/>
+        </visual>
+    </xacro:macro>
+  
+    <xacro:macro name="create_coxa_joint_mount" params="xy_offset">
+      <visual>
+          <origin xyz="${xy_offset} -0.0325"/>
+          <geometry>
+              <cylinder radius="0.0125" length="0.006"/>
+          </geometry>
+          <material name="black"/>
+      </visual>
+      <visual>
+          <origin xyz="${xy_offset} 0.0325"/>
+          <geometry>
+              <cylinder radius="0.0125" length="0.006"/>
+          </geometry>
+          <material name="black"/>
+      </visual>
+    </xacro:macro>
+    
+    <xacro:macro name="create_coxa" params="name side xy_offset">
+        <link name="${name}_link">
+            <!-- coxa bounding box -->
+            <visual>
+                <geometry>
+                    <box size="0.045 0.045 0.045"/>
+                </geometry>
+		<xacro:if value="${side == 'l'}"><origin xyz="-0.015 -0.005 0"/></xacro:if>
+		<xacro:if value="${side == 'r'}"><origin xyz=" 0.015 -0.005 0"/></xacro:if>
+                <material name="blue"/>                                
+            </visual>
+            <!-- coxa rotation axis -->
+            <visual>
+                <geometry>
+                    <cylinder radius="0.005" length="0.065"/>
+                </geometry>
+                <material name="blue"/>
+            </visual>
+            <!-- femur rotation axis -->
+            <visual>
+		<xacro:if value="${side == 'l'}"><origin xyz="-0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
+		<xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
+                <geometry>
+                      <cylinder radius="0.005" length="0.065"/>
+                </geometry>
+                <material name="blue"/>
+            </visual>                       
+        </link>
+        <joint name="${name}_joint" type="revolute">
+            <parent link="base_link"/>
+            <child  link="${name}_link"/>
+            <origin xyz="${xy_offset} 0"/>
+            <axis   xyz="0 0 1"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="create_femur" params="name parent_name side">
+        <link name="${name}_link">
+            <visual>
+                <xacro:if value="${side == 'l'}"><origin xyz="-0.040 0 0"/></xacro:if>
+                <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 0 0"/></xacro:if>
+                <geometry>
+                    <box size="0.100 0.003 0.010"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+<!--
+            <visual>
+                <xacro:if value="${side == 'l'}"><origin xyz="-0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
+                <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
+                <geometry>
+                    <cylinder radius="0.0125" length="0.006"/>
+                </geometry>
+                <material name="green"/>
+            </visual>
+-->           
+        </link>
+        <joint name="${name}_joint" type="revolute">
+            <parent link="${parent_name}_link"/>
+            <child  link="${name}_link"/>
+            <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
+            <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
+            <axis xyz="0 -1 0"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="create_tibia" params="name parent_name side">
+        <link name="${name}_link">
+            <!-- servo box -->
+            <visual>
+                <origin xyz="0 -0.005 0.010"/>
+                <geometry>
+                    <box size="0.020 0.035 0.040"/> 
+                </geometry>
+                <material name="red"/>
+            </visual>
+            <!-- tibia top -->
+            <visual>
+                <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 0.0025"/></xacro:if>
+                <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 0.0025"/></xacro:if>
+                <geometry>
+                    <box size="0.040 0.003 0.075"/> 
+                </geometry>
+                <material name="red"/>
+            </visual>
+	    <!-- tibia middle -->
+            <visual>
+                <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 -0.055"/></xacro:if>
+                <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 -0.055"/></xacro:if>
+                <origin xyz="0 0 -0.055"/>
+                <geometry>
+                    <box size="0.030 0.003 0.050"/> 
+                </geometry>
+                <material name="red"/>
+            </visual>
+	    <!-- tibia bottom -->
+            <visual>
+                <origin xyz=" 0.0 0 -0.105"/>
+                <origin xyz="0 0 -0.105"/>
+                <geometry>
+                    <box size="0.010 0.003 0.050"/> 
+                </geometry>
+                <material name="red"/>
+            </visual>
+            <!-- tibia rotation axis -->
+            <visual>
+		<origin xyz="0 0 0" rpy="-1.57 0 0"/>
+                <geometry>
+                      <cylinder radius="0.005" length="0.065"/>
+                </geometry>
+                <material name="red"/>
+            </visual>                       
+
+        </link>
+        <joint name="${name}_joint" type="revolute">
+            <parent link="${parent_name}_link"/>
+            <child  link="${name}_link"/>
+            <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
+            <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
+            <axis xyz="0 1 0"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="create_end" params="name side parent_name">
+        <link name="${name}_link">
+            <visual>
+                <geometry>
+                    <sphere radius="0.004"/>
+                </geometry>
+                <material name="cyan"/>
+            </visual>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent_name}_link"/>
+            <child  link="${name}_link"/>
+            <origin xyz="0 0 -0.130"/>
+        </joint>
+    </xacro:macro>
+    
+    <xacro:macro name="create_leg" params="number side xy_offset">
+        <xacro:create_coxa  name="c${number}" side="${side}" xy_offset="${xy_offset}"/>
+        <xacro:create_femur name="f${number}" side="${side}" parent_name="c${number}"/>
+        <xacro:create_tibia name="t${number}" side="${side}" parent_name="f${number}"/>
+        <xacro:create_end   name="e${number}" side="${side}" parent_name="t${number}"/>
+    </xacro:macro>
+    
+</robot>

hexapod_robot/config/urdf/hexapod_robot_materials.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <material name="black">
+      <color rgba="0 0 0 1"/>
+  </material>
+
+  <material name="blue">
+      <color rgba="0 0 1 1"/>
+  </material>
+
+  <material name="green">
+      <color rgba="0 1 0 1"/>
+  </material>
+
+  <material name="red">
+      <color rgba="1 0 0 1"/>
+  </material>
+
+  <material name="cyan">
+      <color rgba="1 0 1 1"/>
+  </material>
+
+
+</robot>

hexapod_robot/config/urdf/hexapod_robot_ros2_control.xacro

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="create_leg_joints" params="number">
+
+        <joint name="c${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1.57</param>
+                <param name="max">1.57</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="f${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1.57</param>
+                <param name="max">1.57</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="t${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1.57</param>
+                <param name="max">1.57</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max startup_value">
+        <param name="${name}_joint_i2c_address">${i2c_address}</param>
+        <param name="${name}_joint_channel">${channel}</param>
+        <param name="${name}_joint_counts_intersect">${counts_intersect}</param>
+        <param name="${name}_joint_counts_slope">${counts_slope}</param>
+        <param name="${name}_joint_counts_min">${counts_min}</param>
+        <param name="${name}_joint_counts_max">${counts_max}</param>
+        <param name="${name}_joint_startup_value">${startup_value}</param>
+    </xacro:macro>
+
+    <ros2_control name="Hexapod" type="system">
+        
+        <hardware>
+            <plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
+            <param name="i2c_device">/dev/i2c-1</param>
+            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="389" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="329" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.2"/>
+            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="202" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+
+            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="329" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="334" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.2"/>
+            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="232" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>        
+            
+            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="238" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="320" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.2"/>
+            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="279" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
+            
+            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="448" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="371" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.2"/>
+            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="479" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
+            
+            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="355" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="363" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.2"/>
+            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="424" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
+            
+            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="252" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.2"/>
+            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="447" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+        </hardware>
+
+        <xacro:create_leg_joints number="1"/>    
+        <xacro:create_leg_joints number="2"/>    
+        <xacro:create_leg_joints number="3"/>    
+        <xacro:create_leg_joints number="4"/>    
+        <xacro:create_leg_joints number="5"/>    
+        <xacro:create_leg_joints number="6"/>    
+
+    </ros2_control>
+</robot>

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

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

hexapod_robot/package.xml

@@ -0,0 +1,20 @@
+<?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>hexapod_robot</name>
+  <version>0.9.0</version>
+  <description>ROS2 Robot Description and configuration</description>
+  <maintainer email="marcus@grieger.xyz">Marcus Grieger</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>hexapod_hardware</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+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()

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.

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 <asm/termbits.h>
+#include <fcntl.h> 
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdio.h>
+
+namespace taranis {
+
+class TaranisNode : public rclcpp::Node {
+    public:
+        TaranisNode();
+
+    private:    
+        int serial_fd_;
+        termios2 termios2_;
+
+        bool initialize_serial_();
+
+        rclcpp::Publisher<taranis::msg::TaranisPacket>::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_

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

taranis/package.xml

@@ -0,0 +1,25 @@
+<?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>taranis</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="marcus@grieger.xyz">Marcus Grieger</maintainer>
+  <license>MIT</license>
+
+  <depend>rclcpp</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

taranis/src/taranis_node.cpp

@@ -0,0 +1,173 @@
+#include "taranis/taranis_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include <functional>
+#include <memory>
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+namespace taranis {
+
+TaranisNode::TaranisNode() : Node("taranis_node") {
+    publisher_ = this->create_publisher<taranis::msg::TaranisPacket>("/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<taranis::TaranisNode>());
+  rclcpp::shutdown();
+
+  return 0;
+}