Added moveit stuff

.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

CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+
+find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 # Build hexapod_robot_interfaces
@@ -21,6 +23,7 @@ set(srv_files
 
 rosidl_generate_interfaces(${PROJECT_NAME}
     ${srv_files}
+    DEPENDENCIES geometry_msgs
 )
 
 # Build hexapod_robot_ik_server
@@ -70,17 +73,17 @@ pluginlib_export_plugin_description_file(hardware_interface hexapod_robot.xml)
 
 
 install(
-   DIRECTORY   description
-   DESTINATION share/hexapod_robot
+   DIRECTORY   config launch
+   DESTINATION share/${PROJECT_NAME}
 )
 
 install(
-    DIRECTORY   bringup/launch bringup/config
-    DESTINATION share/hexapod_robot
+    FILES .setup_assistant DESTINATION share/${PROJECT_NAME}
 )
 
+
 install(TARGETS hexapod_robot_ik_server
-    DESTINATION lib/hexapod_robot
+    DESTINATION lib/${PROJECT_NAME}
 )
 
 install(TARGETS hexapod_robot_hwi

bringup/config/controllers.yaml

@@ -1,58 +0,0 @@
-controller_manager:
-    ros__parameters:
-        update_rate: 50
-        
-        joint_state_broadcaster:
-            type: joint_state_broadcaster/JointStateBroadcaster
-
-        joint_trajectory_controller:
-            type: joint_trajectory_controller/JointTrajectoryController
-
-        forward_position_controller:
-            type: forward_command_controller/ForwardCommandController
-
-
-            
-forward_position_controller:
-    ros__parameters:
-        joints:
-            - t3_joint
-        interface_name: position
-
-joint_trajectory_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
-
-        action_monitor_rate: 20.0
-
-        allow_partial_joints_goal: false
-

bringup/config/hexapod_robot.rviz

@@ -1,327 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 70
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /TF1
-      Splitter Ratio: 0.5
-    Tree Height: 438
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 0.800000011920929
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        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
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: 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.30000001192092896
-      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/MoveCamera
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 0.8691349625587463
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.559999942779541
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 4.6249775886535645
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 666
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001560000023cfc0200000002fb000000100044006900730070006c006100790073010000003f0000023c000000cc00fffffffb0000000a0056006900650077007300000001d2000000a9000000a900ffffff000001ff0000023c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Views:
-    collapsed: false
-  Width: 859
-  X: 0
-  Y: 32

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

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>

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
+

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

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

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

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

description/urdf/hexapod_robot.urdf.xacro → config/urdf/hexapod_robot.urdf.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
-    <xacro:include filename="$(find hexapod_robot)/description/urdf/hexapod_robot_materials.xacro"/>
-    <xacro:include filename="$(find hexapod_robot)/description/urdf/hexapod_robot_macros.xacro"/>
+    <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"/>
@@ -23,6 +23,6 @@
     <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)/description/urdf/hexapod_robot_ros2_control.xacro"/>
+    <xacro:include filename="$(find hexapod_robot)/config/urdf/hexapod_robot_ros2_control.xacro"/>
 
 </robot>

description/urdf/hexapod_robot_macros.xacro → config/urdf/hexapod_robot_macros.xacro

@@ -60,7 +60,7 @@
             <child  link="${name}_link"/>
             <origin xyz="${xy_offset} 0"/>
             <axis   xyz="0 0 1"/>
-            <limit effort="30" velocity="100" lower="-1" upper="1"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
         </joint>
     </xacro:macro>
 
@@ -91,7 +91,7 @@
             <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" upper="1"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
         </joint>
     </xacro:macro>
 
@@ -149,7 +149,7 @@
             <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" upper="1"/>
+            <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
         </joint>
     </xacro:macro>
 

description/urdf/hexapod_robot_ros2_control.xacro → config/urdf/hexapod_robot_ros2_control.xacro

@@ -5,23 +5,23 @@
 
         <joint name="c${number}_joint">
             <command_interface name="position">
-                <param name="min">-1</param>
-                <param name="max">1</param>
+                <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</param>
-                <param name="max">1</param>
+                <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</param>
-                <param name="max">1</param>
+                <param name="min">-1.57</param>
+                <param name="max">1.57</param>
             </command_interface>
             <state_interface name="position"/>
         </joint>

kinematics/hexapod_robot_ik_service.cpp

@@ -6,8 +6,8 @@
 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;
-
+    response -> angles.push_back(42);
+     
 }
 
 int main(int argc, char **argv) {

bringup/launch/hexapod_robot.launch.py → launch/hexapod_robot.launch.py

@@ -7,6 +7,9 @@ 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():
 
     robot_description_content = Command([
@@ -15,7 +18,7 @@ def generate_launch_description():
             PathJoinSubstitution(
                 [
                     FindPackageShare("hexapod_robot"),
-                    "description",
+                    "config",
                     "urdf",
                     "hexapod_robot.urdf.xacro"
                 ]
@@ -24,7 +27,7 @@ def generate_launch_description():
     )
     robot_description = {"robot_description": robot_description_content}
 
-    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
+    ros2_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"])
 
     robot_state_publisher_node = Node(
         package = "robot_state_publisher",
@@ -33,10 +36,10 @@ def generate_launch_description():
         output = "both"
     )            
 
-    control_node = Node(
+    ros2_control_node = Node(
         package = "controller_manager",
         executable = "ros2_control_node",
-        parameters = [robot_description, robot_controllers],
+        parameters = [robot_description, ros2_controllers],
         output="both"
     )
 
@@ -63,7 +66,7 @@ def generate_launch_description():
     joint_trajectory_controller_spawner = Node(
         package = "controller_manager",
         executable = "spawner",
-        arguments = ["joint_trajectory_controller", "--controller-manager", "/controller_manager"]
+        arguments = ["e1_group_controller", "--controller-manager", "/controller_manager"]
     )
 
 
@@ -90,7 +93,7 @@ def generate_launch_description():
 
 
     nodes = [
-        control_node,
+        ros2_control_node,
         robot_state_publisher_node,
         joint_state_broadcaster_spawner,
         #delay_rviz_after_joint_state_broadcaster_spawner,

bringup/launch/hexapod_robot_gui.launch.py → launch/hexapod_robot_gui.launch.py

@@ -3,40 +3,44 @@ 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():
 
-    robot_description_content = Command([
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("hexapod_robot"),
-                    "description",
-                    "urdf",
-                    "hexapod_robot.urdf.xacro"
-                ]
-            )
-        ]
+    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()
     )
-    robot_description = {"robot_description": robot_description_content}
 
-    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
+    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 = [robot_description],
+        parameters = [moveit_config.robot_description],
         output = "both"
     )            
 
-    control_node = Node(
+    ros2_control_node = Node(
         package = "controller_manager",
         executable = "ros2_control_node",
-        parameters = [robot_description, robot_controllers],
+        parameters = [moveit_config.robot_description, ros2_controllers],
         output="both"
     )
 
@@ -46,6 +50,7 @@ def generate_launch_description():
         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 (
@@ -53,17 +58,11 @@ def generate_launch_description():
         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"]
+        arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"]
     )
 
 
@@ -73,13 +72,7 @@ def generate_launch_description():
             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(
@@ -90,8 +83,9 @@ def generate_launch_description():
 
 
     nodes = [
-        control_node,
         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,

package.xml

@@ -9,7 +9,11 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+
   <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 

srv/HexapodRobotInverseKinematics.srv

@@ -1,3 +1,4 @@
-int64 foo
+geometry_msgs/Pose body
+geometry_msgs/Vector3[] leg_tips
 ---
-int64 bar
+float64[] angles