Adding Inverse Kinematics Service

CMakeLists.txt

@@ -11,37 +11,64 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# Build hexapod_robot_interfaces
+
+set(srv_files
+    "srv/HexapodRobotInverseKinematics.srv"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+    ${srv_files}
+)
+
+# Build hexapod_robot_ik_server
+
+add_executable(hexapod_robot_ik_server
+    "kinematics/hexapod_robot_ik_service.cpp"
+)
+
+target_compile_features(hexapod_robot_ik_server PUBLIC cxx_std_17)
+target_include_directories(hexapod_robot_ik_server PUBLIC 
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/kinematics/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(
+    hexapod_robot_ik_server
+    rclcpp
+)
+
+rosidl_target_interfaces(hexapod_robot_ik_server ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# Build hexapod_robot_hwi
 
 add_library(
-    hexapod_robot
+    hexapod_robot_hwi
     SHARED
     hardware/hexapod_robot_hwi.cpp
 )
 
-target_compile_features(hexapod_robot PUBLIC cxx_std_17)
-target_include_directories(hexapod_robot PUBLIC 
+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/hexapod_robot>
+    $<INSTALL_INTERFACE:include>
 )
 
 ament_target_dependencies(
-    hexapod_robot PUBLIC
+    hexapod_robot_hwi PUBLIC
     hardware_interface
     pluginlib
     rclcpp
     rclcpp_lifecycle
 )
 
-target_link_libraries(hexapod_robot PUBLIC i2c)
+target_link_libraries(hexapod_robot_hwi PUBLIC i2c)
 
 pluginlib_export_plugin_description_file(hardware_interface hexapod_robot.xml)
 
 
-install(
-    DIRECTORY hardware/include/
-    DESTINATION include/hexapod_robot
-)
-
 install(
    DIRECTORY   description
    DESTINATION share/hexapod_robot
@@ -52,7 +79,11 @@ install(
     DESTINATION share/hexapod_robot
 )
 
-install(TARGETS hexapod_robot
+install(TARGETS hexapod_robot_ik_server
+    DESTINATION lib/hexapod_robot
+)
+
+install(TARGETS hexapod_robot_hwi
     EXPORT export_hexapod_robot
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
@@ -74,6 +105,7 @@ endif()
 
 ament_export_targets(export_hexapod_robot HAS_LIBRARY_TARGET)
 ament_export_dependencies(
+    rosidl_default_runtime
     hardware_interface 
     pluginlib
     rclcpp

bringup/launch/hexapod_robot_gui.launch.py

@@ -0,0 +1,102 @@
+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"),
+                    "description",
+                    "urdf",
+                    "hexapod_robot.urdf.xacro"
+                ]
+            )
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "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"
+    )
+
+    rviz_node = Node(
+        package = "rviz2",
+        executable = "rviz2",
+        arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
+        name = "rviz2",
+        output = "screen",
+    )
+
+    joint_state_broadcaster_spawner = Node (
+        package = "controller_manager",
+        executable = "spawner",
+        arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
+    )
+    
+    # forward_position_controller_spawner = Node(
+    #     package = "controller_manager",
+    #     executable = "spawner",
+    #     arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
+    # )
+
+    joint_trajectory_controller_spawner = Node(
+        package = "controller_manager",
+        executable = "spawner",
+        arguments = ["joint_trajectory_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_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    #     event_handler = OnProcessExit(
+    #         target_action = joint_state_broadcaster_spawner,
+    #         on_exit = [forward_position_controller_spawner]
+    #     )
+    # )
+
+    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_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_robot.xml

@@ -1,4 +1,4 @@
-<library path="hexapod_robot">
+<library path="hexapod_robot_hwi">
     <class name="hexapod_robot/HexapodRobotHardwareInterface"
            type="hexapod_robot::HexapodRobotHardwareInterface"
            base_class_type="hardware_interface::SystemInterface">

kinematics/hexapod_robot_ik_service.cpp

@@ -0,0 +1,23 @@
+#include "rclcpp/rclcpp.hpp"
+#include "hexapod_robot/srv/hexapod_robot_inverse_kinematics.hpp"
+
+#include <memory>
+
+void ik(const std::shared_ptr<hexapod_robot::srv::HexapodRobotInverseKinematics::Request> request,
+        std::shared_ptr<hexapod_robot::srv::HexapodRobotInverseKinematics::Response> response) {
+
+    response -> bar = request -> foo;
+
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+
+    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("hexapod_robot_inverse_kinematics_server");
+
+    rclcpp::Service<hexapod_robot::srv::HexapodRobotInverseKinematics>::SharedPtr service = 
+        node -> create_service<hexapod_robot::srv::HexapodRobotInverseKinematics>("hexapod_robot_inverse_kinematics", &ik);
+
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+}

package.xml

@@ -8,6 +8,10 @@
   <license>MIT</license>
 
   <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>

srv/HexapodRobotInverseKinematics.srv

@@ -0,0 +1,3 @@
+int64 foo
+---
+int64 bar