diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml index f69481cd..d5d1a85e 100644 --- a/rosbot_controller/config/diff_drive_controller.yaml +++ b/rosbot_controller/config/diff_drive_controller.yaml @@ -4,7 +4,7 @@ simulation_ignition_ros_control: # Separate controller manager used for simulation - only difference is # the use_sim_time parameter (it is the easiest way to do it with ign ros2 control) -simulation_controller_manager: +/**/simulation_controller_manager: ros__parameters: use_sim_time: true update_rate: 20 # Hz @@ -17,7 +17,7 @@ simulation_controller_manager: type: imu_sensor_broadcaster/IMUSensorBroadcaster -controller_manager: +/**/controller_manager: ros__parameters: use_sim_time: false update_rate: 20 # Hz @@ -29,7 +29,7 @@ controller_manager: imu_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster -imu_broadcaster: +/**/imu_broadcaster: ros__parameters: sensor_name: imu frame_id: imu_link @@ -37,7 +37,7 @@ imu_broadcaster: static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally -rosbot_base_controller: +/**/rosbot_base_controller: ros__parameters: left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"] right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"] diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py index f6382f6b..e46ca020 100644 --- a/rosbot_controller/launch/controller.launch.py +++ b/rosbot_controller/launch/controller.launch.py @@ -14,7 +14,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import RegisterEventHandler, DeclareLaunchArgument, GroupAction from launch.conditions import UnlessCondition from launch.event_handlers import OnProcessExit from launch.substitutions import ( @@ -30,6 +30,13 @@ def generate_launch_description(): + namespace = LaunchConfiguration("namespace") + declare_namespace_arg = DeclareLaunchArgument( + "namespace", + default_value="", + description="Namespace for all topics and tfs", + ) + mecanum = LaunchConfiguration("mecanum") declare_mecanum_arg = DeclareLaunchArgument( "mecanum", @@ -69,13 +76,24 @@ def generate_launch_description(): ] ) - controller_manager_name = PythonExpression( + controller_manager_type_name = PythonExpression( [ - "'/simulation_controller_manager' if ", + "'simulation_controller_manager' if ", use_sim, - " else '/controller_manager'", + " else 'controller_manager'", ] ) + controller_manager_name = LaunchConfiguration("controller_manager_name") + namespace_for_controller_name = PythonExpression( + ["''", " if '", namespace, "' == '' ", "else ", "'", namespace, "/'"] + ) + + namespace_for_controller_name + declare_controller_manager_name_arg = DeclareLaunchArgument( + "controller_manager_name", + default_value=[namespace_for_controller_name, controller_manager_type_name], + description="ros2_control controller manager name", + ) # Get URDF via xacro robot_description_content = Command( @@ -114,12 +132,13 @@ def generate_launch_description(): executable="ros2_control_node", parameters=[robot_description, robot_controllers], remappings=[ - ("/imu_sensor_node/imu", "/_imu/data_raw"), + ("imu_sensor_node/imu", "/_imu/data_raw"), ("~/motors_cmd", "/_motors_cmd"), ("~/motors_response", "/_motors_response"), - ("/rosbot_base_controller/cmd_vel_unstamped", "/cmd_vel"), + ("rosbot_base_controller/cmd_vel_unstamped", "cmd_vel"), ], condition=UnlessCondition(use_sim), + namespace=namespace, ) robot_state_pub_node = Node( @@ -137,6 +156,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -149,6 +170,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -169,6 +192,8 @@ def generate_launch_description(): controller_manager_name, "--controller-manager-timeout", "120", + "--namespace", + namespace, ], ) @@ -181,12 +206,16 @@ def generate_launch_description(): ) ) - actions = [ + args_declarations_actions = [ + declare_namespace_arg, declare_mecanum_arg, declare_use_sim_arg, declare_use_gpu_arg, declare_simulation_engine_arg, - SetParameter(name="use_sim_time", value=use_sim), + declare_controller_manager_name_arg, + ] + + nodes_actions = [ control_node, robot_state_pub_node, joint_state_broadcaster_spawner, @@ -194,4 +223,15 @@ def generate_launch_description(): delay_imu_broadcaster_spawner_after_robot_controller_spawner, ] - return LaunchDescription(actions) + return LaunchDescription( + args_declarations_actions + + [ + GroupAction( + actions=[ + # PushRosNamespace(namespace), + SetParameter(name="use_sim_time", value=use_sim), + ] + + nodes_actions + ) + ] + )