still error in moveit (gripper motion can be planned but not executed)

updated README.md

README.md

@@ -64,7 +64,7 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
 ### use_fake_hardware:=true
 Terminal 1.:
 ```bash
-ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false
+ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller
 ```
 Terminal 2.:
 ```bash

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -131,6 +131,7 @@ forward_position_controller:
       - $(var tf_prefix)wrist_2_joint
       - $(var tf_prefix)wrist_3_joint
 
+
 robotiq_gripper_controller:
   ros__parameters:
     default: true

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -7,6 +7,7 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition, UnlessCondition
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
 
+
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
@@ -44,8 +45,6 @@ def launch_setup(context, *args, **kwargs):
     script_sender_port = LaunchConfiguration("script_sender_port")
     trajectory_port = LaunchConfiguration("trajectory_port")
 
-
-
     # Config Paths UR
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
@@ -191,11 +190,10 @@ def launch_setup(context, *args, **kwargs):
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[
+        parameters=[robot_description,
-            robot_description,
+                    update_rate_config_file,
-            update_rate_config_file,
+                    ParameterFile(initial_joint_controllers, allow_substs=True),
-            ParameterFile(initial_joint_controllers, allow_substs=True),
+                    ],
-        ],
         output="screen",
         condition=IfCondition(use_fake_hardware),
     )
@@ -203,11 +201,10 @@ def launch_setup(context, *args, **kwargs):
     ur_control_node = Node(
         package="ur_robot_driver",
         executable="ur_ros2_control_node",
-        parameters=[
+        parameters=[robot_description,
-            robot_description,
+                    update_rate_config_file,
-            update_rate_config_file,
+                    ParameterFile(initial_joint_controllers, allow_substs=True),
-            ParameterFile(initial_joint_controllers, allow_substs=True),
+                    ],
-        ],
         output="screen",
         condition=UnlessCondition(use_fake_hardware),
     )
@@ -297,13 +294,13 @@ def launch_setup(context, *args, **kwargs):
             package="controller_manager",
             executable="spawner",
             arguments=[
-                name,
+                          name,
-                "--controller-manager",
+                          "--controller-manager",
-                "/controller_manager",
+                          "/controller_manager",
-                "--controller-manager-timeout",
+                          "--controller-manager-timeout",
-                controller_spawner_timeout,
+                          controller_spawner_timeout,
-            ]
+                      ]
-            + inactive_flags,
+                      + inactive_flags,
         )
 
     controller_spawner_names = [
@@ -346,24 +343,24 @@ def launch_setup(context, *args, **kwargs):
     )
 
     nodes_to_start = [
-        control_node,
+                         control_node,
-        ur_control_node,
+                         ur_control_node,
-        robotiq_gripper_controller_spawner,
+                         robotiq_gripper_controller_spawner,
-        robotiq_activation_controller_spawner,
+                         robotiq_activation_controller_spawner,
-        dashboard_client_node,
+                         dashboard_client_node,
-        tool_communication_node,
+                         tool_communication_node,
-        controller_stopper_node,
+                         controller_stopper_node,
-        robot_state_publisher_node,
+                         robot_state_publisher_node,
-        rviz_node,
+                         rviz_node,
-        initial_joint_controller_spawner_stopped,
+                         initial_joint_controller_spawner_stopped,
-        initial_joint_controller_spawner_started,
+                         initial_joint_controller_spawner_started,
-    ] + controller_spawners
+                     ] + controller_spawners
 
     return nodes_to_start
 
 
 def generate_launch_description():
-# Declare Arguments
+    # Declare Arguments
     declared_arguments = []
     # UR specific arguments
     declared_arguments.append(
@@ -606,5 +603,4 @@ def generate_launch_description():
         )
     )
 
-
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -34,7 +34,7 @@ robotiq_gripper_controller:
   action_ns: gripper_cmd
   type: GripperCommand
   default: true
-  joints:
+  controlled_joints:
-  - finger_joint
+    - finger_joint
 
 

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -24,7 +24,7 @@ def load_yaml(package_name, file_path):
         return None
 
 
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
@@ -145,10 +145,12 @@ def generate_launch_description():
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Configuration
-    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
     # the scaled_joint_trajectory_controller does not work on fake hardware
-    change_controllers = use_fake_hardware
+    use_fake_hw = use_fake_hardware.perform(context)
-    if change_controllers == "true":
+    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
+
+    if use_fake_hw == "true":
+        print("Using fake hardware")
         controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
         controllers_yaml["joint_trajectory_controller"]["default"] = True
 
@@ -232,6 +234,12 @@ def generate_launch_description():
         output="screen",
     )
 
+    nodes_to_start = [move_group_node, rviz_node, servo_node]
+
+    return nodes_to_start
+
+
+def generate_launch_description():
     # Declare the Launch arguments
     declared_arguments = []
     # UR specific arguments
@@ -302,7 +310,6 @@ def generate_launch_description():
         DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
     )
 
-    # servo_node,
-    nodes_to_start = [move_group_node, rviz_node, servo_node]
 
-    return LaunchDescription(declared_arguments + nodes_to_start)
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

src/ur_robotiq_moveit_config/package.xml

@@ -50,6 +50,9 @@
   <exec_depend>robotiq_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
 
+  <depend>control_msgs</depend>
+  <depend>rclcpp_action</depend>
+
 
   <export>
       <build_type>ament_cmake</build_type>