roboclone scene and launch for single rl enviroment

rbs_bringup/launch/launch_env.launch.py

@@ -0,0 +1,254 @@
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+from nav2_common.launch import RewrittenYaml
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
+    # General arguments
+    with_gripper_condition = LaunchConfiguration("with_gripper")
+    controllers_file = LaunchConfiguration("controllers_file")
+    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    robot_name = LaunchConfiguration("robot_name")
+    start_joint_controller = LaunchConfiguration("start_joint_controller")
+    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
+    launch_simulation = LaunchConfiguration("launch_sim")
+    launch_moveit = LaunchConfiguration("launch_moveit")
+    launch_task_planner = LaunchConfiguration("launch_task_planner")
+    launch_perception = LaunchConfiguration("launch_perception")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    sim_gazebo = LaunchConfiguration("sim_gazebo")
+    hardware = LaunchConfiguration("hardware")
+    env_manager = LaunchConfiguration("env_manager")
+    launch_controllers = LaunchConfiguration("launch_controllers")
+    gazebo_gui = LaunchConfiguration("gazebo_gui")
+    gripper_name = LaunchConfiguration("gripper_name")
+
+
+    sim_gazebo = LaunchConfiguration("sim_gazebo")
+    launch_simulation = LaunchConfiguration("launch_sim")
+
+    configured_params = RewrittenYaml(
+        source_file=os.path.join(
+            get_package_share_directory(
+                description_package.perform(context)), 
+            "config", 
+            controllers_file.perform(context)),
+        root_key=robot_name.perform(context),
+        param_rewrites={},
+        convert_types=True,
+    )
+    initial_joint_controllers_file_path = os.path.join(
+        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
+    )
+    controller_paramfile = configured_params.perform(context)
+    namespace = "/" + robot_name.perform(context) 
+
+    # spawner = Node(
+    #     package="gz_enviroment_python",
+    #     executable="spawner.py",
+    #     namespace=namespace
+    # )
+
+    single_robot_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([
+            PathJoinSubstitution([
+                FindPackageShare('rbs_bringup'),
+                "launch",
+                "rbs_robot.launch.py"
+            ])
+        ]),
+        launch_arguments={
+            "env_manager": env_manager,
+            "with_gripper": with_gripper_condition,
+            "gripper_name": gripper_name,
+            "controllers_file": controllers_file,
+            "robot_type": robot_type,
+            "controllers_file": initial_joint_controllers_file_path,
+            "cartesian_controllers": cartesian_controllers,
+            "description_package": description_package,
+            "description_file": description_file,
+            "robot_name": robot_name,
+            "start_joint_controller": start_joint_controller,
+            "initial_joint_controller": initial_joint_controller,
+            "launch_simulation": launch_simulation,
+            "launch_moveit": launch_moveit,
+            "launch_task_planner": launch_task_planner,
+            "launch_perception": launch_perception,
+            "moveit_config_package": moveit_config_package,
+            "moveit_config_file": moveit_config_file,
+            "use_sim_time": use_sim_time,
+            "sim_gazebo": sim_gazebo,
+            "hardware": hardware,
+            "launch_controllers": "true",
+            "gazebo_gui": gazebo_gui,
+        }.items()
+    )
+    
+    nodes_to_start = [
+        # spawner,
+        single_robot_setup
+    ]
+    return nodes_to_start
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_type",
+            description="Type of robot by name",
+            choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            default_value="rbs_arm",
+        )
+    )
+    # General arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controllers_file",
+            default_value="rbs_arm_controllers_gazebosim.yaml",
+            description="YAML file with the controllers configuration.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="rbs_arm",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="rbs_arm_modular.xacro",
+            description="URDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_name",
+            default_value="arm0",
+            description="Name for robot, used to apply namespace for specific robot in multirobot setup",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "start_joint_controller",
+            default_value="false",
+            description="Enable headless mode for robot control",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "initial_joint_controller",
+            default_value="joint_trajectory_controller",
+            description="Robot controller to start.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="rbs_arm",
+            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            default_value="rbs_arm.srdf.xacro",
+            description="MoveIt SRDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="Make MoveIt to use simulation time.\
+            This is needed for the trajectory planing in simulation.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gripper_name",
+            default_value="rbs_gripper",
+            choices=["rbs_gripper", ""],
+            description="choose gripper by name (leave empty if hasn't)",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("with_gripper", 
+                              default_value="true", 
+                              description="With gripper or not?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("sim_gazebo", 
+                              default_value="true", 
+                              description="Gazebo Simulation")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("env_manager", 
+                              default_value="false", 
+                              description="Launch env_manager?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_sim", 
+                              default_value="true", 
+                              description="Launch simulator (Gazebo)?\
+                              Most general arg")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_moveit", 
+                              default_value="false", 
+                              description="Launch moveit?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_perception", 
+                              default_value="false", 
+                              description="Launch perception?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_task_planner", 
+                              default_value="false", 
+                              description="Launch task_planner?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("cartesian_controllers", 
+                              default_value="true", 
+                              description="Load cartesian\
+                              controllers?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("hardware", 
+                              choices=["gazebo", "mock"],
+                              default_value="gazebo", 
+                              description="Choose your harware_interface")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_controllers", 
+                              default_value="true", 
+                              description="Launch controllers?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("gazebo_gui", 
+                              default_value="true", 
+                              description="Launch gazebo with gui?")
+    )
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_bringup/launch/rbs_robot.launch.py

@@ -15,6 +15,8 @@ import xacro
 import os
 from ament_index_python.packages import get_package_share_directory
 
+from rbs_arm import RbsBuilder
+
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
@@ -62,10 +64,19 @@ def launch_setup(context, *args, **kwargs):
             "namespace": namespace,
             "x": x_pos.perform(context),
             "y": y_pos.perform(context),
-            "z": z_pos.perform(context)
+            "z": z_pos.perform(context),
+            "roll": roll.perform(context),
+            "pitch": pitch.perform(context),
+            "yaw": yaw.perform(context)
             #TODO: add rotation and add probably via dict
         }
     )
+
+    # robot = RbsBuilder(6, "arm0", "world", "rbs_gripper")
+    # robot.base()
+    # robot.gripper()
+    # robot.ros2_control()
+    # robot.moveit()
     robot_description_content = robot_description_doc.toprettyxml(indent="  ")
 
     robot_description = {"robot_description": robot_description_content}
@@ -381,6 +392,21 @@ def generate_launch_description():
                               default_value="0.0", 
                               description="Position of robot in world by Z")
     )
+    declared_arguments.append(
+        DeclareLaunchArgument("roll", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("pitch", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("yaw", 
+                              default_value="0.0", 
+                              description="Position of robot in world by Z")
+    )
 
 
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_simulation/worlds/asm2.sdf

@@ -10,6 +10,7 @@
     <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
     <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
     <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
+    <plugin filename="ignition-gazebo-forcetorque-system" name="ignition::gazebo::systems::ForceTorque"/>
     <plugin name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
         <render_engine>ogre2</render_engine>
       </plugin>  
@@ -92,15 +93,16 @@
         </visual>
       </link>
     </model>
-    Manipulating objects
+    <!-- Manipulating objects -->
-			<include>
+			<!-- <include> -->
-				<name>board</name>
+			<!-- 	<name>board</name> -->
-				<uri>model://board</uri>
+			<!-- 	<uri>model://board</uri> -->
-        <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose>
+   <!--      <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose> -->
-			</include>
+			<!-- </include> -->
 			<!-- <include> -->
 			<!-- 	<name>bishop</name> -->
 			<!-- 	<uri>model://bishop</uri> -->
    <!--      <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose> -->
 			<!-- </include> -->
   </world>
+</sdf>