Add demo to grasp service

ros2_grasp_service/package.xml

@@ -4,7 +4,7 @@
   <name>ros2_grasp_service</name>
   <version>1.0.0</version>
   <description>Simple grasp service</description>
-  <maintainer email="[email protected]">ros</maintainer>
+  <maintainer email="[email protected]">Wiktor Bajor</maintainer>
   <license>Apache-2.0</license>
 
   <test_depend>ament_copyright</test_depend>

ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py

@@ -0,0 +1,48 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+
+
+def generate_launch_description():
+    os.environ["GAZEBO_MODEL_PATH"] = os.path.join(get_package_share_directory("manipulator_simulation"), "models")
+    manipulator_simulation_package = get_package_share_directory("manipulator_simulation")
+    gazebo_ros_package_dir = get_package_share_directory("gazebo_ros")
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                name="world",
+                default_value="default_world.world",
+                description="Name of the world model file to load in ariadna_simulation_bringup/worlds",
+            ),
+            DeclareLaunchArgument(name="gui", default_value="true", description='Set to "false" to run headless.'),
+            DeclareLaunchArgument(
+                name="verbose",
+                default_value="false",
+                description='Set to "true" to run verbose logging.',
+            ),
+            DeclareLaunchArgument(
+                name="server",
+                default_value="true",
+                description='Set to "false" not to run gzserver.',
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(gazebo_ros_package_dir, "launch", "gzserver.launch.py")]),
+                launch_arguments={
+                    "world": PathJoinSubstitution(
+                        [manipulator_simulation_package, "worlds", LaunchConfiguration("world")]
+                    ),
+                    "verbose": LaunchConfiguration("verbose"),
+                }.items(),
+                condition=IfCondition(LaunchConfiguration("server")),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(gazebo_ros_package_dir, "launch", "gzclient.launch.py")]),
+                condition=IfCondition(LaunchConfiguration("gui")),
+            ),
+        ]
+    )

ros2_grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py

@@ -0,0 +1,83 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+
+
+def generate_launch_description():
+    use_open_manipulator = DeclareLaunchArgument(
+        "use_open_manipulator",
+        default_value="False",
+        description="Launch simulation with open manipulator.",
+    )
+
+    use_6dof_manipulator = DeclareLaunchArgument(
+        "use_6dof_manipulator",
+        default_value="False",
+        description="Launch simulation with custom 6DoF manipulator.",
+    )
+    manipulator_simulation = get_package_share_directory("manipulator_simulation")
+
+    manipulator_spawner = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=["-entity", "manipulator", "-topic", "robot_description"],
+        output="screen",
+        emulate_tty=True,
+        condition=IfCondition(
+            PythonExpression(
+                [
+                    LaunchConfiguration("use_open_manipulator"),
+                    " or ",
+                    LaunchConfiguration("use_6dof_manipulator"),
+                ]
+            )
+        ),
+    )
+
+    open_manipulator_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                get_package_share_directory("open_manipulator_bringup"),
+                "/launch/open_manipulator_bringup.launch.py",
+            ]
+        ),
+        condition=IfCondition(LaunchConfiguration("use_open_manipulator")),
+    )
+    manipulator_6dof_bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                get_package_share_directory("manipulator_6dof_bringup"),
+                "/launch/manipulator_6dof_bringup.launch.py",
+            ]
+        ),
+        condition=IfCondition(LaunchConfiguration("use_6dof_manipulator")),
+    )
+
+    return LaunchDescription(
+        [
+            use_open_manipulator,
+            use_6dof_manipulator,
+            DeclareLaunchArgument(
+                name="world",
+                default_value=os.path.join(manipulator_simulation, "worlds", "default_world.world"),
+                description="Full path to the world model file to load",
+            ),
+            DeclareLaunchArgument(name="gui", default_value="true", description='Set to "false" to run headless.'),
+            DeclareLaunchArgument(
+                name="server",
+                default_value="true",
+                description='Set to "false" not to run gzserver.',
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([manipulator_simulation, "/launch/gazebo.launch.py"])
+            ),
+            manipulator_spawner,
+            open_manipulator_bringup,
+            manipulator_6dof_bringup,
+        ]
+    )

ros2_grasp_service_demo/manipulator_simulation/models/asphalt_plane/materials/scripts/asphalt.material

@@ -0,0 +1,20 @@
+material vrc/asphalt
+{
+	technique
+	{
+		pass
+		{
+			ambient 0.5 0.5 0.5 1.0
+			diffuse 0.5 0.5 0.5 1.0
+			specular 0.2 0.2 0.2 1.0 12.5
+
+			texture_unit
+			{
+				texture tarmac.png
+				filtering anistropic
+				max_anisotropy 16
+				scale 0.1 0.1
+			}
+		}
+	}
+}

ros2_grasp_service_demo/manipulator_simulation/models/asphalt_plane/model-1_4.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="asphalt_plane">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>20 20 .1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>20 20 .1</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://asphalt_plane/materials/scripts</uri>
+            <uri>model://asphalt_plane/materials/textures</uri>
+            <name>vrc/asphalt</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

ros2_grasp_service_demo/manipulator_simulation/models/asphalt_plane/model.config

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Asphalt Plane</name>
+  <version>1.0</version>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Thomas Koletschka</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    An asphalt textured plane.
+  </description>
+
+</model>

ros2_grasp_service_demo/manipulator_simulation/models/asphalt_plane/model.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="asphalt_plane">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>20 20 .1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>20 20 .1</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://asphalt_plane/materials/scripts</uri>
+            <uri>model://asphalt_plane/materials/textures</uri>
+            <name>vrc/asphalt</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>