Merge branch 'master' into brokenaf

bonk

src/ar3_config/config/ar3_controllers.yaml

@@ -1,7 +1,8 @@
-controller_names:
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+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,4 +1,10 @@
-planner_configs:
+move_group:
+  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()
@@ -121,7 +127,7 @@ planner_configs:
       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
@@ -147,3 +153,4 @@ ar3_arm:
       - LazyPRMstarkConfigDefault
       - SPARSkConfigDefault
       - SPARStwokConfigDefault
+

src/ar3_config/config/planning_scene.yaml

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

src/ar3_config/config/trajectory.yaml

@@ -0,0 +1,4 @@
+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,8 +1,7 @@
 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, ComposableNodeContainer
-from launch_ros.descriptions import ComposableNode
+from launch_ros.actions import Node
 from launch.actions import ExecuteProcess
 import yaml
 
@@ -31,7 +30,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
@@ -39,7 +38,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(
@@ -74,25 +73,30 @@ 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',
         ),
 
@@ -105,9 +109,11 @@ def generate_launch_description():
                 robot_description,
                 robot_description_semantic,
                 robot_description_kinematics,
-                {'move_group': join(config_root, 'ompl_planning.yaml')},
-                {'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
-            ]
+                get_yaml(join(config_root, 'ompl_planning.yaml')),
+                get_yaml(join(config_root, 'trajectory.yaml')),
+                get_yaml(join(config_root, 'planning_scene.yaml')),
+                get_yaml(join(config_root, "ar3_controllers.yaml")),
+            ],
         ),
 
         # Robot state publisher
@@ -115,14 +121,5 @@ def generate_launch_description():
             package='robot_state_publisher',
             executable='robot_state_publisher',
             output='screen',
-            parameters=[robot_description],
-        ),
-
-        # Interface with arduino
-        Node(
-            package='cobot_platform',
-            executable='move',
-            output='screen',
-        ),
+            parameters=[robot_description]),
     ])
-

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. * 2000)
+        self._cur[0] = int(msg.data / pi / 2. * 8000)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 3840.64)
+        self._cur[1] = int(msg.data / pi / 2. * 10000)
         self._move_servo()
 
     def _move_j3_cb(self, msg):
@@ -105,3 +105,4 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
+