diff --git a/rae_description/launch/rsp.launch.py b/rae_description/launch/rsp.launch.py index b021d04..8e55fd5 100644 --- a/rae_description/launch/rsp.launch.py +++ b/rae_description/launch/rsp.launch.py @@ -11,6 +11,7 @@ def launch_setup(context, *args, **kwargs): use_sim_time = LaunchConfiguration('use_sim_time') + ns = LaunchConfiguration('namespace').perform(context) pkg_path = os.path.join(get_package_share_directory('rae_description')) xacro_path = os.path.join(pkg_path, 'urdf', 'rae.urdf.xacro') @@ -21,8 +22,10 @@ def launch_setup(context, *args, **kwargs): name='robot_state_publisher', namespace=LaunchConfiguration('namespace'), parameters=[{ - 'robot_description': Command(['xacro ', xacro_path, ' sim_mode:=', use_sim_time]), - 'use_sim_time': use_sim_time + 'robot_description': Command(['xacro ', xacro_path, + ' sim_mode:=', use_sim_time, + ' namespace:=', ns]), + 'use_sim_time:=': use_sim_time, }] ) ] diff --git a/rae_description/urdf/camera.urdf.xacro b/rae_description/urdf/camera.urdf.xacro index ca4db3d..5342543 100644 --- a/rae_description/urdf/camera.urdf.xacro +++ b/rae_description/urdf/camera.urdf.xacro @@ -1,57 +1,94 @@ - - - - ogre2 - - - - - - - - - - - + + + + + + + - - - - + + + + - - - - + + + + + + + + + + + + + 15.0 + ${ns}/rae/${camera_name}${topic_postfix} + ${ns}_rae_${camera_name}_sim_camera_optical_frame + + ${105.0/180.0*pi} + + 640 + 400 + + + 0.2 + 6.0 + + + + + + ogre2 + + + + + + + + + - - - - + + + + - - - - 1.047 - - 320 - 240 - - - 0.1 - 100 - - - rgb_camera_link_optical_frame - 1 - 30 - true - /camera/image - + + + true + 30.0 + ${ns}/rae/rgb/image_raw + ${ns}/rae_${camera_name}_sim_optical_frame + + ${127.0/180.0*pi} + + 720 + 480 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + + + + ogre2 + - + + \ No newline at end of file diff --git a/rae_description/urdf/rae.urdf.xacro b/rae_description/urdf/rae.urdf.xacro index a3e7b54..6bfbafd 100644 --- a/rae_description/urdf/rae.urdf.xacro +++ b/rae_description/urdf/rae.urdf.xacro @@ -2,8 +2,10 @@ + + @@ -24,7 +26,7 @@ 1 10.0 true - rae/imu/data + /rae/imu/data rae_imu_frame rae_imu_frame @@ -238,7 +240,7 @@ - + @@ -251,7 +253,7 @@ - + @@ -277,7 +279,7 @@ - + @@ -303,7 +305,7 @@ - + @@ -329,7 +331,7 @@ - + @@ -353,6 +355,14 @@ - + + + + + + + + + diff --git a/rae_description/urdf/rae_ros2_control.urdf.xacro b/rae_description/urdf/rae_ros2_control.urdf.xacro index 266fc6c..b5a08aa 100644 --- a/rae_description/urdf/rae_ros2_control.urdf.xacro +++ b/rae_description/urdf/rae_ros2_control.urdf.xacro @@ -1,7 +1,7 @@ - + @@ -68,7 +68,8 @@ $(find rae_hw)/config/controller.yaml - /diff_controller/cmd_vel_unstamped:=/cmd_vel + ${namespace} + diff_controller/cmd_vel_unstamped:=cmd_vel diff --git a/rae_gazebo/config/gz_bridge.yaml b/rae_gazebo/config/gz_bridge.yaml index b04f7a7..73b89cd 100644 --- a/rae_gazebo/config/gz_bridge.yaml +++ b/rae_gazebo/config/gz_bridge.yaml @@ -3,34 +3,50 @@ gz_type_name: "ignition.msgs.Clock" direction: GZ_TO_ROS -- topic_name: "/camera/image" +- ros_topic_name: "/rae/right/image_raw" + gz_topic_name: "/rae/right/image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" direction: GZ_TO_ROS -- topic_name: "/camera/depth_image" +- topic_name: "/rae/right/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "ignition.msgs.CameraInfo" + direction: GZ_TO_ROS + +- ros_topic_name: "/rae/stereo_front/image_raw" + gz_topic_name: "/rae/right/depth_image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" direction: GZ_TO_ROS -- topic_name: "/camera/camera_info" +- ros_topic_name: "/rae/stereo_front/camera_info" + gz_topic_name: "/rae/right/camera_info" ros_type_name: "sensor_msgs/msg/CameraInfo" gz_type_name: "ignition.msgs.CameraInfo" direction: GZ_TO_ROS -- topic_name: "/image_out" +- ros_topic_name: "/rae/left_back/image_raw" + gz_topic_name: "/rae/left_back/image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" direction: GZ_TO_ROS -- topic_name: "/image_tuning" +- topic_name: "/rae/left_back/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "ignition.msgs.CameraInfo" + direction: GZ_TO_ROS + +- ros_topic_name: "/rae/stereo_back/image_raw" + gz_topic_name: "/rae/left_back/depth_image" ros_type_name: "sensor_msgs/msg/Image" gz_type_name: "ignition.msgs.Image" direction: GZ_TO_ROS -- topic_name: "/scan" - ros_type_name: "sensor_msgs/msg/LaserScan" - gz_type_name: "ignition.msgs.LaserScan" +- ros_topic_name: "/rae/stereo_back/camera_info" + gz_topic_name: "/rae/left_back/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "ignition.msgs.CameraInfo" direction: GZ_TO_ROS - topic_name: "/rae/imu/data" diff --git a/rae_gazebo/launch/gazebo.launch.py b/rae_gazebo/launch/gazebo.launch.py index 9a5b31b..c5cdfe8 100644 --- a/rae_gazebo/launch/gazebo.launch.py +++ b/rae_gazebo/launch/gazebo.launch.py @@ -8,15 +8,16 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter def launch_setup(context, *args, **kwargs): rae_description_pkg = get_package_share_directory('rae_description') ros_gz_sim_pkg = get_package_share_directory('ros_gz_sim') - enable_localization = LaunchConfiguration( - 'enable_localization', default='true') + namespace = LaunchConfiguration('namespace') + enable_localization = LaunchConfiguration('enable_localization') world_file = LaunchConfiguration('sdf_file').perform(context) print(f'world_file: {world_file}') + return [ SetEnvironmentVariable( name='IGN_GAZEBO_RESOURCE_PATH', @@ -42,15 +43,18 @@ def launch_setup(context, *args, **kwargs): os.path.join(rae_description_pkg, 'launch', 'rsp.launch.py') ]), launch_arguments={ - 'use_sim_time': 'True' + 'use_sim_time': 'True', + 'namespace': namespace }.items() ), # ros_gz_bridge Node( package='ros_gz_bridge', + namespace=LaunchConfiguration('namespace'), executable='parameter_bridge', parameters=[{ + 'expand_gz_topic_names': True, 'use_sim_time': True, 'config_file': os.path.join(get_package_share_directory( 'rae_gazebo'),'config', 'gz_bridge.yaml') @@ -61,8 +65,11 @@ def launch_setup(context, *args, **kwargs): # Ignition Gazebo - Spawn Entity Node( package='ros_gz_sim', + namespace=LaunchConfiguration('namespace'), executable='create', arguments=[ + "-name", "rea", + "-allow_renaming", "true", '-topic', 'robot_description', ], output='screen' @@ -73,7 +80,8 @@ def launch_setup(context, *args, **kwargs): package='controller_manager', namespace=LaunchConfiguration('namespace'), executable='spawner', - arguments=['diff_controller', '-c', '/controller_manager'], + arguments=['diff_controller', + '--controller-manager', f'/controller_manager'], ), # Activate joint state broadcaster @@ -82,7 +90,7 @@ def launch_setup(context, *args, **kwargs): executable='spawner', namespace=LaunchConfiguration('namespace'), arguments=['joint_state_broadcaster', - '--controller-manager', '/controller_manager'], + '--controller-manager', f'/controller_manager'], ), # Activate EKF @@ -93,9 +101,13 @@ def launch_setup(context, *args, **kwargs): name='ekf_filter_node', namespace=LaunchConfiguration('namespace'), output='screen', - parameters=[os.path.join(get_package_share_directory( - 'rae_hw'), 'config', 'ekf.yaml')], - ) + parameters=[{'use_sim_time': True}, + os.path.join(get_package_share_directory( + 'rae_hw'), 'config', 'ekf.yaml')], + ), + + # Set all nodes to use simulation time + SetParameter(name='use_sim_time', value=True) ] @@ -103,6 +115,8 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): rae_gazebo_pkg = get_package_share_directory('rae_gazebo') declared_arguments = [ + DeclareLaunchArgument('namespace', default_value=''), + DeclareLaunchArgument('enable_localization', default_value='true'), DeclareLaunchArgument('sdf_file', default_value=f"-r {os.path.join(rae_gazebo_pkg, 'worlds', 'world_demo.sdf')}"), ]