Migrate demo to use modern gazebo

Wiktor-99 · Aug 18, 2024 · c2d0402 · c2d0402
1 parent b6af8cb
commit c2d0402
Show file tree

Hide file tree

Showing 7 changed files with 804 additions and 157 deletions.
diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
@@ -1,3 +1,5 @@
+# flake8: noqa
+
 # auto-generated DO NOT EDIT
 
 from rcl_interfaces.msg import ParameterDescriptor
@@ -35,6 +37,7 @@ class __MapPositionsToTargetList:
             def add_entry(self, name):
                 if not hasattr(self, name):
                     setattr(self, name, self.__map_type())
+                return getattr(self, name)
 
             def get_entry(self, name):
                 return getattr(self, name)
@@ -51,6 +54,7 @@ class __MapPositionsToHomeList:
             def add_entry(self, name):
                 if not hasattr(self, name):
                     setattr(self, name, self.__map_type())
+                return getattr(self, name)
 
             def get_entry(self, name):
                 return getattr(self, name)
@@ -85,10 +89,11 @@ def refresh_dynamic_parameters(self):
             # TODO remove any destroyed dynamic parameters
 
             # declare any new dynamic parameters
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -107,10 +112,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -130,10 +136,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.time_to_target = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -152,10 +159,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -241,42 +249,44 @@ def update(self, parameters):
 
             # update dynamic parameters
             for param in parameters:
-                for value in updated_params.positions_to_target_list:
-                    param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+                for value_1 in updated_params.positions_to_target_list:
+                    param_name = f"{self.prefix_}positions_to_target{value_1}.positions"
                     if param.name == param_name:
                         validation_result = ParameterValidators.size_gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_target.positions_to_target_list_map[value].positions = param.value
+
+                        updated_params.positions_to_target.get_entry(value_1).positions = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_target_list:
-                    param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+                for value_1 in updated_params.positions_to_target_list:
+                    param_name = f"{self.prefix_}positions_to_target{value_1}.time_to_target"
                     if param.name == param_name:
                         validation_result = ParameterValidators.gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_target.positions_to_target_list_map[value].time_to_target = (
-                            param.value
-                        )
+
+                        updated_params.positions_to_target.get_entry(value_1).time_to_target = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_home_list:
-                    param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+                for value_1 in updated_params.positions_to_home_list:
+                    param_name = f"{self.prefix_}positions_to_home{value_1}.positions"
                     if param.name == param_name:
                         validation_result = ParameterValidators.size_gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_home.positions_to_home_list_map[value].positions = param.value
+
+                        updated_params.positions_to_home.get_entry(value_1).positions = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_home_list:
-                    param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+                for value_1 in updated_params.positions_to_home_list:
+                    param_name = f"{self.prefix_}positions_to_home{value_1}.time_to_target"
                     if param.name == param_name:
                         validation_result = ParameterValidators.gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_home.positions_to_home_list_map[value].time_to_target = param.value
+
+                        updated_params.positions_to_home.get_entry(value_1).time_to_target = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
             updated_params.stamp_ = self.clock_.now()
@@ -432,10 +442,11 @@ def declare_params(self):
             updated_params.gripper_open = param.value
 
             # declare and set all dynamic parameters
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -454,10 +465,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -477,10 +489,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.time_to_target = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -499,10 +512,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())

diff --git a/ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py b/ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py
diff --git a/...grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py b/...grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py
@@ -4,45 +4,59 @@
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
-    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,
-    )
-
     open_manipulator_bringup = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
                 get_package_share_directory("open_manipulator_bringup"),
                 "/launch/open_manipulator_bringup.launch.py",
             ]
+        )
+    )
+
+    world_argument = DeclareLaunchArgument(
+        "world",
+        default_value=os.path.join(
+            get_package_share_directory("manipulator_simulation"), "worlds", "default_world.sdf"
         ),
+        description="Robot controller to start.",
+    )
+
+    gazebo = IncludeLaunchDescription(
+        os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"),
+        launch_arguments=[
+            (
+                "gz_args",
+                ["-r -v 4 ", LaunchConfiguration("world"), " --physics-engine gz-physics-bullet-featherstone-plugin"],
+            )
+        ],
+    )
+
+    ign_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="gz_bridge",
+        arguments=["/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock"],
+        output="screen",
+    )
+
+    gazebo_spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        name="spawn_manipulator",
+        arguments=["-name", "manipulator", "-topic", "robot_description"],
+        output="screen",
     )
 
     return LaunchDescription(
         [
-            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,
+            world_argument,
+            gazebo,
+            ign_bridge,
+            gazebo_spawn_robot,
             open_manipulator_bringup,
         ]
     )
diff --git a/ros2_grasp_service_demo/manipulator_simulation/setup.py b/ros2_grasp_service_demo/manipulator_simulation/setup.py
@@ -12,7 +12,7 @@
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
         (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
-        (os.path.join("share", package_name, "worlds"), glob("worlds/*.world")),
+        (os.path.join("share", package_name, "worlds"), glob("worlds/*.sdf")),
         (
             os.path.join("share", package_name, "models/ground_plane"),
             glob("models/ground_plane/model.sdf"),