src/ar3_config/config/ar3_controllers.yaml

@@ -1,8 +1,7 @@
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+controller_names:
-moveit_simple_controller_manager:
-  controller_names:
   - joint_trajectory_controller
-  joint_trajectory_controller:
+
+joint_trajectory_controller:
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true

src/ar3_config/config/ompl_planning.yaml

@@ -1,10 +1,4 @@
-move_group:
+planner_configs:
-  planning_plugin: ompl_interface/OMPLPlanner
-  request_adapters:
-    default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints
-  start_state_max_bounds_error: 0.1
-
-  planner_configs:
   SBLkConfigDefault:
     type: geometric::SBL
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -127,7 +121,7 @@ move_group:
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 
-  ar3_arm:
+ar3_arm:
   default_planner_config: RRTConnectkConfigDefault
   planner_configs:
     - SBLkConfigDefault
@@ -153,4 +147,3 @@ move_group:
     - LazyPRMstarkConfigDefault
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
-

src/ar3_config/config/planning_scene.yaml

@@ -1,4 +0,0 @@
-publish_planning_scene: true
-publish_geometry_updates: true
-publish_state_updates: true
-publish_transforms_updates: true

src/ar3_config/config/trajectory.yaml

@@ -1,4 +0,0 @@
-moveit_manage_controllers: true
-trajectory_execution.allowed_execution_duration_scaling: 1.2
-trajectory_execution.allowed_goal_duration_margin: 0.5
-trajectory_execution.allowed_start_tolerance: 0.1

src/ar3_config/launch/dev.launch.py

@@ -1,7 +1,8 @@
 from os.path import join
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 from launch.actions import ExecuteProcess
 import yaml
 
@@ -30,7 +31,7 @@ def generate_launch_description():
             executable='rviz2',
             output='log',
             arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
-            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics],
         ),
 
         # Attatch robot to map
@@ -38,7 +39,7 @@ def generate_launch_description():
             package='tf2_ros',
             executable='static_transform_publisher',
             output='screen',
-            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
+            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link'],
         ),
 
         Node(
@@ -73,30 +74,25 @@ def generate_launch_description():
                 )
             ],
             output="screen",
-        )
+        ),
 
         # Controller
         Node(
             package='controller_manager',
             executable='ros2_control_node',
             output='screen',
-            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
+            parameters=[robot_description, join(config_root, 'controller_manager.yaml')],
         ),
 
         # Start controllers
         ExecuteProcess(
             cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
             shell=True,
-            output='screen'),
+            output='screen',
+        ),
         ExecuteProcess(
             cmd=['ros2 run controller_manager spawner.py joint_state_controller'],
             shell=True,
-            output='screen'),
-
-        # Run platform
-        Node(
-            package='cobot_platform',
-            executable='move',
             output='screen',
         ),
 
@@ -109,11 +105,9 @@ def generate_launch_description():
                 robot_description,
                 robot_description_semantic,
                 robot_description_kinematics,
-                get_yaml(join(config_root, 'ompl_planning.yaml')),
+                {'move_group': join(config_root, 'ompl_planning.yaml')},
-                get_yaml(join(config_root, 'trajectory.yaml')),
+                {'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
-                get_yaml(join(config_root, 'planning_scene.yaml')),
+            ]
-                get_yaml(join(config_root, "ar3_controllers.yaml")),
-            ],
         ),
 
         # Robot state publisher
@@ -121,5 +115,14 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description]),
+            parameters=[robot_description],
+        ),
+
+        # Interface with arduino
+        Node(
+            package='cobot_platform',
+            executable='move',
+            output='screen',
+        ),
     ])
+

src/cobot_platform/cobot_platform/move.py

@@ -46,11 +46,11 @@ class CobotMove(Node):
         print(self._serial.readline())
 
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 8000)
+        self._cur[0] = int(msg.data / pi / 2. * 2000)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 10000)
+        self._cur[1] = int(msg.data / pi / 2. * 3840.64)
         self._move_servo()
 
     def _move_j3_cb(self, msg):
@@ -105,4 +105,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-