Merge remote-tracking branch 'origin/master' into override-rm-change-…

…how-description-is-accessed

gz_ros2_control/src/gz_system.cpp

@@ -235,8 +235,13 @@ bool GazeboSimSystem::initSim(
 
   constexpr double default_gain = 0.1;
 
-  this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter<double>(
-    "position_proportional_gain", default_gain);
+  try {
+    this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter<double>(
+      "position_proportional_gain", default_gain);
+  } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) {
+    this->nh_->get_parameter(
+      "position_proportional_gain", this->dataPtr->position_proportional_gain_);
+  }
 
   RCLCPP_INFO_STREAM(
     this->nh_->get_logger(),

gz_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -12,52 +12,45 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
-
-import xacro
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
-    gz_ros2_control_demos_path = os.path.join(
-        get_package_share_directory('gz_ros2_control_demos'))
-
-    xacro_file = os.path.join(gz_ros2_control_demos_path,
-                              'urdf',
-                              'test_cart_effort.xacro.urdf')
-
-    doc = xacro.parse(open(xacro_file))
-    xacro.process_doc(doc)
-    params = {'robot_description': doc.toxml()}
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("gz_ros2_control_demos"), "urdf", "test_cart_effort.xacro.urdf"]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[params]
+        parameters=[robot_description]
     )
 
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
-        parameters=[{'string': doc.toxml(),
-                     'name': 'cart',
-                     'allow_renaming': True}],
+        arguments=["-topic", "robot_description", "-name", "cart", "-allow_renaming", "true"],
     )
 
     load_joint_state_broadcaster = ExecuteProcess(
@@ -75,8 +68,7 @@ def generate_launch_description():
         # Launch gazebo environment
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_gz_sim'),
-                              'launch', 'gz_sim.launch.py')]),
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]),
             launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(

gz_ros2_control_demos/launch/cart_example_position.launch.py

@@ -12,52 +12,45 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
-
-import xacro
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
-    gz_ros2_control_demos_path = os.path.join(
-        get_package_share_directory('gz_ros2_control_demos'))
-
-    xacro_file = os.path.join(gz_ros2_control_demos_path,
-                              'urdf',
-                              'test_cart_position.xacro.urdf')
-
-    doc = xacro.parse(open(xacro_file))
-    xacro.process_doc(doc)
-    params = {'robot_description': doc.toxml()}
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("gz_ros2_control_demos"), "urdf", "test_cart_position.xacro.urdf"]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[params]
+        parameters=[robot_description]
     )
 
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
-        parameters=[{'string': doc.toxml(),
-                     'name': 'cart',
-                     'allow_renaming': True}],
+        arguments=["-topic", "robot_description", "-name", "cart", "-allow_renaming", "true"],
     )
 
     load_joint_state_broadcaster = ExecuteProcess(
@@ -76,8 +69,7 @@ def generate_launch_description():
         # Launch gazebo environment
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_gz_sim'),
-                              'launch', 'gz_sim.launch.py')]),
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]),
             launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(

gz_ros2_control_demos/launch/cart_example_velocity.launch.py

@@ -12,52 +12,45 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
-
-import xacro
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
-    gz_ros2_control_demos_path = os.path.join(
-        get_package_share_directory('gz_ros2_control_demos'))
-
-    xacro_file = os.path.join(gz_ros2_control_demos_path,
-                              'urdf',
-                              'test_cart_velocity.xacro.urdf')
-
-    doc = xacro.parse(open(xacro_file))
-    xacro.process_doc(doc)
-    params = {'robot_description': doc.toxml()}
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("gz_ros2_control_demos"), "urdf", "test_cart_velocity.xacro.urdf"]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[params]
+        parameters=[robot_description]
     )
 
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
-        parameters=[{'string': doc.toxml(),
-                     'name': 'cart',
-                     'allow_renaming': True}],
+        arguments=["-topic", "robot_description", "-name", "cart", "-allow_renaming", "true"],
     )
 
     load_joint_state_broadcaster = ExecuteProcess(
@@ -71,19 +64,12 @@ def generate_launch_description():
         output='screen'
     )
 
-    load_imu_sensor_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'imu_sensor_broadcaster'],
-        output='screen'
-    )
-
     return LaunchDescription([
         # Launch gazebo environment
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_gz_sim'),
-                              'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]),
+            launch_arguments=[('gz_args', [' -r -v 3 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,
@@ -96,12 +82,6 @@ def generate_launch_description():
                 on_exit=[load_joint_velocity_controller],
             )
         ),
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=load_joint_velocity_controller,
-                on_exit=[load_imu_sensor_broadcaster],
-            )
-        ),
         node_robot_state_publisher,
         gz_spawn_entity,
         # Launch Arguments

gz_ros2_control_demos/launch/diff_drive_example.launch.py

@@ -12,56 +12,48 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
 
 from launch_ros.actions import Node
-
-import xacro
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
     # Launch Arguments
     use_sim_time = LaunchConfiguration('use_sim_time', default=True)
 
-    gz_ros2_control_demos_path = os.path.join(
-        get_package_share_directory('gz_ros2_control_demos'))
-
-    xacro_file = os.path.join(gz_ros2_control_demos_path,
-                              'urdf',
-                              'test_diff_drive.xacro.urdf')
-
-    doc = xacro.parse(open(xacro_file))
-    xacro.process_doc(doc)
-    params = {'robot_description': doc.toxml(), 'use_sim_time': use_sim_time}
-
-    print(params)
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("gz_ros2_control_demos"), "urdf", "test_diff_drive.xacro.urdf"]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
 
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[params],
+        parameters=[robot_description]
     )
 
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
-        parameters=[{'string': doc.toxml(),
-                     'name': 'diff_drive',
-                     'allow_renaming': True}],
+        arguments=["-topic", "robot_description", "-name", "diff_drive", "-allow_renaming", "true"],
     )
 
-    load_joint_state_controller = ExecuteProcess(
+    load_joint_state_broadcaster = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
              'joint_state_broadcaster'],
         output='screen'
@@ -73,31 +65,21 @@ def generate_launch_description():
         output='screen'
     )
 
-    # Bridge
-    bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
-        output='screen'
-    )
-
     return LaunchDescription([
-        bridge,
         # Launch gazebo environment
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_gz_sim'),
-                              'launch', 'gz_sim.launch.py')]),
+                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]),
             launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=gz_spawn_entity,
-                on_exit=[load_joint_state_controller],
+                on_exit=[load_joint_state_broadcaster],
             )
         ),
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=load_joint_state_controller,
+                target_action=load_joint_state_broadcaster,
                 on_exit=[load_diff_drive_controller],
             )
         ),