stuff

Why do I even subject myself to ros
Launch file aids has been nuked from orbit
Wrong ompl planning default name fixed
Get that spammy print outta my life

src/ar3_config/config/ompl_planning.yaml

@@ -122,7 +122,7 @@ planner_configs:
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
 
 ar3_arm:
-  # default_planner_config: RRTConnect #RRTConnectkConfigDefault
+  default_planner_config: RRTConnectkConfigDefault
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault

src/ar3_config/launch/dev.launch.py

@@ -5,54 +5,23 @@ from launch_ros.actions import Node
 from launch.actions import ExecuteProcess
 import yaml
 
+def get_file(path):
+    with open(path, 'r') as f:
+        return f.read()
+
+def get_yaml(path):
+    with open(path, 'r') as f:
+        return yaml.safe_load(f)
+
+
 def generate_launch_description():
     launch_root = get_package_share_directory('ar3_config')
     description_root = get_package_share_directory('ar3_description')
     config_root = join(launch_root, 'config')
 
-    urdf_path = join(description_root, 'ar3.urdf')
-    with open(urdf_path, 'r') as f:
-        robot_description = {'robot_description': f.read()}
-
-    srdf_path = join(description_root, 'ar3.srdf')
-    with open(srdf_path, 'r') as f:
-        robot_description_semantic = {'robot_description_semantic': f.read()}
-
-    robot_description_kinematics = {}
-    with open(join(config_root, 'kinematics.yaml'), 'r') as f:
-        robot_description_kinematics.update({"robot_description_kinematics": yaml.safe_load(f)})
-
-    # Planning Functionality
-    ompl_planning_pipeline_config = {
-        "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,
-        }
-    }
-
-    with open(join(config_root, "ompl_planning.yaml"), 'r') as f:
-       ompl_planning_pipeline_config["move_group"].update(yaml.safe_load(f))
-
-     # Trajectory Execution Functionality
-    moveit_controllers = {
-        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"
-    }
-    with open(join(config_root, "ar3_controllers.yaml"), 'r') as f:
-        moveit_controllers.update({"moveit_simple_controller_manager": yaml.safe_load(f)})
-
-    trajectory_execution = {
-        "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.01,
-    }
-    planning_scene_monitor_parameters = {
-        "publish_planning_scene": True,
-        "publish_geometry_updates": True,
-        "publish_state_updates": True,
-        "publish_transforms_updates": True,
-    }
+    robot_description  = {'robot_description': get_file(join(description_root, 'ar3.urdf'))}
+    robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))}
+    robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml'))
 
     return LaunchDescription([
         # RViz
@@ -87,6 +56,13 @@ def generate_launch_description():
             shell=True,
             output='screen'),
 
+        # Run platform
+        Node(
+            package='cobot_platform',
+            executable='move',
+            output='screen',
+        ),
+
         # Start the actual move_group node/action server
         Node(
             package="moveit_ros_move_group",
@@ -96,10 +72,8 @@ def generate_launch_description():
                 robot_description,
                 robot_description_semantic,
                 robot_description_kinematics,
-                ompl_planning_pipeline_config,
-                trajectory_execution,
-                moveit_controllers,
-                planning_scene_monitor_parameters
+                {'move_group': join(config_root, 'ompl_planning.yaml')},
+                {'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
             ],
         ),
 

src/cobot_platform/cobot_platform/move.py

@@ -89,7 +89,7 @@ class CobotMove(Node):
 
         self._joint_pub.publish(self._joint_state)
 
-        print(self._joint_state.position)
+#        print(self._joint_state.position)
 
     # def _push ?