diff --git a/CMakeLists.txt b/CMakeLists.txt index b60463c..0a26f37 100644 --- a/CMakeLists.txt +++ b/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 + $ + $ +) + +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 $ - $ + $ ) 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 diff --git a/bringup/launch/hexapod_robot_gui.launch.py b/bringup/launch/hexapod_robot_gui.launch.py new file mode 100644 index 0000000..c63d7ef --- /dev/null +++ b/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) + \ No newline at end of file diff --git a/hexapod_robot.xml b/hexapod_robot.xml index 7af91b8..141c640 100644 --- a/hexapod_robot.xml +++ b/hexapod_robot.xml @@ -1,4 +1,4 @@ - + diff --git a/kinematics/hexapod_robot_ik_service.cpp b/kinematics/hexapod_robot_ik_service.cpp new file mode 100644 index 0000000..066f590 --- /dev/null +++ b/kinematics/hexapod_robot_ik_service.cpp @@ -0,0 +1,23 @@ +#include "rclcpp/rclcpp.hpp" +#include "hexapod_robot/srv/hexapod_robot_inverse_kinematics.hpp" + +#include + +void ik(const std::shared_ptr request, + std::shared_ptr response) { + + response -> bar = request -> foo; + +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("hexapod_robot_inverse_kinematics_server"); + + rclcpp::Service::SharedPtr service = + node -> create_service("hexapod_robot_inverse_kinematics", &ik); + + rclcpp::spin(node); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/package.xml b/package.xml index d4b7323..d90d97a 100644 --- a/package.xml +++ b/package.xml @@ -8,6 +8,10 @@ MIT ament_cmake + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages ament_lint_auto ament_lint_common diff --git a/srv/HexapodRobotInverseKinematics.srv b/srv/HexapodRobotInverseKinematics.srv new file mode 100644 index 0000000..9ce93a1 --- /dev/null +++ b/srv/HexapodRobotInverseKinematics.srv @@ -0,0 +1,3 @@ +int64 foo +--- +int64 bar