🔨 get stable launch move_group plus gazebo

rasms_moveit_actions/CMakeLists.txt

@@ -58,9 +58,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 
 add_executable(rasms_moveit src/run_moveit.cpp)
-#target_include_directories(rasms_moveit PRIVATE include)
+target_include_directories(rasms_moveit PUBLIC include)
 ament_target_dependencies(rasms_moveit 
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+  ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost
 )
 
 install(TARGETS rasms_moveit

rasms_moveit_config/config/rasms.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 814
+    Tree Height: 743
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -21,6 +21,8 @@ Panels:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
 Visualization Manager:
   Class: ""
   Displays:
@@ -106,6 +108,105 @@ Visualization Manager:
         Show Robot Collision: false
         Show Robot Visual: true
       Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        link1:
+          Value: true
+        link2:
+          Value: true
+        link3:
+          Value: true
+        link4:
+          Value: true
+        link5:
+          Value: true
+        link6:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          base:
+            link1:
+              link2:
+                link3:
+                  link4:
+                    link5:
+                      link6:
+                        {}
+      Update Interval: 0
+      Value: true
+    - 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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      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: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: /display_planned_path
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -149,7 +250,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.8626145124435425
+      Distance: 2.2047624588012695
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -164,10 +265,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.84539794921875
+      Pitch: 0.7153979539871216
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.6953980922698975
+      Yaw: 0.7453981041908264
     Saved: ~
 Window Geometry:
   Displays:
@@ -175,11 +276,15 @@ Window Geometry:
   Height: 1043
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f3000003b9fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b2000002290000000000000000000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000472000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
+  RvizVisualToolsGui:
+    collapsed: false
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
+  Trajectory - Trajectory Slider:
+    collapsed: false
   Views:
     collapsed: false
   Width: 1920

rasms_moveit_config/config/rasms_controllers.yml

@@ -1,15 +1,14 @@
-# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
 controller_manager:
   ros__parameters:
-    update_rate: 1000  # Hz
+    update_rate: 500  # Hz
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    position_trajectory_controller:
+    rasms_arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
-position_trajectory_controller:
+rasms_arm_controller:
   ros__parameters:
     joints:
       - joint1

rasms_moveit_config/config/rasms_moveit_controllers.yml

@@ -1,8 +1,8 @@
 controller_names:
-  - position_trajectory_controller
+  - rasms_arm_controller
 
 # http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller
-position_trajectory_controller:
+rasms_arm_controller:
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true

rasms_moveit_config/launch/rasms_bringup.launch.py

@@ -11,16 +11,32 @@ import xacro
 import os
 from ament_index_python import get_package_share_directory
 
+def generate_launch_description():
 
-def launch_setup(context, *args, **kwargs):
+    # Launch arguments
+    launch_args = []
 
-    # Evaluate frequently used variables
+    launch_args.append(DeclareLaunchArgument(
-    model = LaunchConfiguration("model").perform(context)
+            name="robot_name",
+            default_value="rasms",
+            description="Set robot name."
+        )
+    )
+
+    launch_args.append(DeclareLaunchArgument(
+            name="sim",
+            default_value="true",
+            description="Launch robot in simulation or on real setup."
+        )
+    )
+    # get xacro file path
     xacro_file = os.path.join(get_package_share_directory("rasms_description"),"urdf/","rasms_description.urdf.xacro")
+    # get error if xacro file if missing
     assert os.path.exists(xacro_file), "The xacro file of rasms_description doesnt exist"+str(xacro_file)
+    # parse xacro file from file
     robot_description = xacro.process_file(xacro_file)
+    # convert file to xml format
     robot_description_content = robot_description.toxml()
-    #print(robot_description_content)
 
     # Load controls
     control = IncludeLaunchDescription(
@@ -31,10 +47,7 @@ def launch_setup(context, *args, **kwargs):
                 "rasms_control.launch.py"
             ])
         ), launch_arguments=[
-            ("robot_description", robot_description_content),
+            ("robot_description", robot_description_content)
-            ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
-            ("controller_configurations", LaunchConfiguration("controller_configurations")),
-            ("sim", LaunchConfiguration("sim"))
         ]
     )
 
@@ -64,76 +77,18 @@ def launch_setup(context, *args, **kwargs):
         ), 
         launch_arguments=[
             ("robot_description", robot_description_content),
-            ("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")),
-            ("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")),
-            ("model", LaunchConfiguration("model")),
             ("sim", LaunchConfiguration("sim"))
         ]
     )
 
-    return [
+    launch_nodes = []
-        simulation,
+    launch_nodes.append(control)
-        control,
+    launch_nodes.append(simulation)
-        move_group
+    launch_nodes.append(move_group)
-    ]
 
-
+    
-def generate_launch_description():
-
-    # Launch arguments
-    launch_args = []
-
-    launch_args.append(DeclareLaunchArgument(
-        name="model",
-        default_value="rasms_description",
-        description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
-    ))
-
-    launch_args.append(DeclareLaunchArgument(
-        name="robot_name",
-        default_value="rasms",
-        description="Set robot name."
-    ))
-
-    launch_args.append(DeclareLaunchArgument(
-        name="sim",
-        default_value="true",
-        description="Launch robot in simulation or on real setup."
-    ))
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="controller_configurations_package",
-            default_value="rasms_moveit_config",
-            description="Package that contains controller configurations."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="controller_configurations",
-            default_value="config/rasms_controllers.yml",
-            description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="moveit_controller_configurations_package",
-            default_value="rasms_moveit_config",
-            description="Package that contains MoveIt! controller configurations."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="moveit_controller_configurations",
-            default_value="config/rasms_moveit_controllers.yml",
-            description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file."
-        )
-    )
 
     return LaunchDescription(
-        launch_args + [
+        launch_args +
-        OpaqueFunction(function=launch_setup)
+        launch_nodes
-    ])
+    )

rasms_moveit_config/launch/rasms_control.launch.py

@@ -16,22 +16,6 @@ def generate_launch_description():
     # Launch arguments
     launch_args = []
 
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="controller_configurations_package",
-            default_value="rasms_moveit_config",
-            description="Package that contains controller configurations."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="controller_configurations",
-            default_value="config/rasms_controllers.yml",
-            description="Relative path to controller configurations YAML file."
-        )
-    )
-
     launch_args.append(
         DeclareLaunchArgument(
             name="robot_description",
@@ -42,27 +26,15 @@ def generate_launch_description():
     launch_args.append(
         DeclareLaunchArgument(
             name="controller",
-            default_value="position_trajectory_controller",
+            default_value="rasms_arm_controller",
             description="Robot controller."
         )
     )
 
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="sim",
-            default_value="true",
-            description="Launch robot in simulation or on real setup."
-        )
-    )
-
     # Configure robot_description
     robot_description = {"robot_description": LaunchConfiguration("robot_description")}
 
     # Load controllers from YAML configuration file
-    """controller_configurations_path = PathJoinSubstitution([
-        FindPackageShare(LaunchConfiguration("controller_configurations_package")),
-        LaunchConfiguration("controller_configurations")
-    ])"""
     ros2_controllers_path = os.path.join(
         get_package_share_directory("rasms_moveit_config"),
         "config",
@@ -95,21 +67,21 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"],
     )
 
-    """joint_state_broadcaster = Node(
+    joint_state_broadcaster = Node(
         package="controller_manager",
         executable="spawner.py",
         arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
     )
 
-    controller = Node(
+    rasms_arm_controller = Node(
         package="controller_manager",
         executable="spawner.py",
-        arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"],
+        arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"],
-    )"""
+    )
 
-    load_controllers = []
+    """load_controllers = []
     for controller in [
-        "position_trajectory_controller",
+        "rasms_arm_controller",
         "joint_state_broadcaster",
     ]:
         load_controllers += [
@@ -118,14 +90,16 @@ def generate_launch_description():
                 shell=True,
                 output="screen",
             )
-        ]
+        ]"""
 
     return LaunchDescription(
         launch_args + 
-        load_controllers +
+        #load_controllers +
         [
             controller_manager,
             robot_state_publisher,
-            static_tf
+            static_tf,
+            joint_state_broadcaster,
+            rasms_arm_controller
         ] 
         )

rasms_moveit_config/launch/rasms_moveit_interface.launch.py

@@ -32,24 +32,45 @@ def load_file(package_name: str, file_path: str) -> str:
         return None
 
 
-def launch_setup(context, *args, **kwargs):
+def generate_launch_description():
 
-    # Evaluate frequently used variables
+    #=============
-    model = LaunchConfiguration("model").perform(context)
+    #================================================================= Start initialize launch args ==========================================================
+    #=============
 
-    # Configure robot_description
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="robot_description",
+            description="Robot description XML file."
+        )
+    )
+
+    launch_args.append(DeclareLaunchArgument(
+        name="sim",
+        default_value="true",
+        description="Launch robot in simulation or on real setup."
+    ))
+
+    #=============
+    #================================================================= Start launching nodes ==========================================================
+    #=============
+
+        # Configure robot_description
     robot_description = {"robot_description": LaunchConfiguration("robot_description")}
 
     # Robot semantics SRDF
     robot_description_semantic = {
-        "robot_description_semantic": load_file("rasms_moveit_config", "srdf/{}.srdf".format(model))
+        "robot_description_semantic": load_file("rasms_moveit_config", "srdf/rasms_description.srdf")
     }
 
     # Kinematics
     kinematics_yaml = load_yaml("rasms_moveit_config", "config/kinematics.yml")
     
     # Update group name
-    kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"]
+    kinematics_yaml["rasms_description_arm"] = kinematics_yaml["group_name"]
     del kinematics_yaml["group_name"]
 
     # Joint limits
@@ -79,8 +100,8 @@ def launch_setup(context, *args, **kwargs):
 
     # Controllers
     moveit_simple_controller_yml = load_yaml(
-        LaunchConfiguration("moveit_controller_configurations_package").perform(context),
+        "rasms_moveit_config",
-        LaunchConfiguration("moveit_controller_configurations").perform(context)
+        "config/rasms_moveit_controllers.yml"
     )
 
     moveit_controllers = {"moveit_simple_controller_manager": moveit_simple_controller_yml,
@@ -110,7 +131,7 @@ def launch_setup(context, *args, **kwargs):
     # Time configuration
     use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}
 
-    # Prepare move group node
+    # Simple move_group interface
     move_group_node = Node(
         package="moveit_ros_move_group",
         executable="move_group",
@@ -129,8 +150,8 @@ def launch_setup(context, *args, **kwargs):
             use_sim_time
         ]
     )
-    
+    # Node for test moveit_cpp interface
-    """moveit_cpp_node = Node(
+    moveit_cpp_node = Node(
         name="moveit_cpp_actions",
         package="rasms_moveit_actions",
         executable="rasms_moveit",
@@ -143,20 +164,6 @@ def launch_setup(context, *args, **kwargs):
             ompl_planning_pipeline_config,
             moveit_controllers,
         ],
-    )"""
-    
-    """robot_state_publisher = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="screen",
-        parameters=[robot_description]
-    )"""
-
-    spawn_controller = Node(
-        package="controller_manager",
-        executable="spawner.py",
-        arguments=["joint_state_broadcaster"],
-        output="screen",
     )
 
     # RViz
@@ -176,57 +183,13 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
-    return [
+    launch_nodes = []
-        move_group_node,
+    launch_nodes.append(rviz)
-        #moveit_cpp_node,
+    launch_nodes.append(move_group_node)
-        rviz,
+    #launch_nodes.append(moveit_cpp_node)
-        #robot_state_publisher,
-        spawn_controller
-    ]
 
-def generate_launch_description():
-
-    # Launch arguments
-    launch_args = []
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="robot_description",
-            description="Robot description XML file."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="model",
-            default_value="rasms_description",
-            description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="moveit_controller_configurations_package",
-            default_value="rasms_moveit_config",
-            description="Package that contains MoveIt! controller configurations."
-        )
-    )
-
-    launch_args.append(
-        DeclareLaunchArgument(
-            name="moveit_controller_configurations",
-            default_value="config/rasms_moveit_controllers.yml",
-            description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
-        )
-    )
-
-    launch_args.append(DeclareLaunchArgument(
-        name="sim",
-        default_value="true",
-        description="Launch robot in simulation or on real setup."
-    ))
 
     return LaunchDescription(
-        launch_args + [
+        launch_args +
-        OpaqueFunction(function=launch_setup)
+        launch_nodes
-    ])
+    )