diff --git a/README.md b/README.md index e4f7b02..d43e20d 100644 --- a/README.md +++ b/README.md @@ -174,6 +174,12 @@ its previews. `sim.launch.py` is `world.launch.py` (Gazebo with the world) and `sim_robot.launch.py` (the simulated robot with the same localization, SLAM and navigation as the real one) together, with RViz and joystick teleop; tools such as the GUI start them one by one. + +**Camera.** The simulated robot has a camera on the front of the body, a temporary one until the real robot gets +one: `/camera/image_raw` (also compressed through `image_transport`) and `/camera/camera_info`, 640×480 at 30 Hz, +frame `camera_rgb_frame`. RViz shows it in the *Camera* display (disabled by default). `camera:=false` starts the +model without the camera. + In a simulation shared by several robots each one is started with `sim_robot.launch.py robot_ns:=`: the name is given to the model, its Gazebo topics and its controller manager. The topics of the controller manager move under `/` too (`robot_description`, `joint_states`, `diff_drive_controller/cmd_vel`, `diff_drive_controller/odom`), so diff --git a/src/platform/kpi_rover_bringup/config/gz_bridge.yaml b/src/platform/kpi_rover_bringup/config/gz_bridge.yaml index e4049b8..c806dc6 100644 --- a/src/platform/kpi_rover_bringup/config/gz_bridge.yaml +++ b/src/platform/kpi_rover_bringup/config/gz_bridge.yaml @@ -15,3 +15,9 @@ ros_type_name: "sensor_msgs/msg/Imu" gz_type_name: "gz.msgs.IMU" direction: GZ_TO_ROS + +- ros_topic_name: "/camera/camera_info" + gz_topic_name: "camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS diff --git a/src/platform/kpi_rover_bringup/launch/sim_robot.launch.py b/src/platform/kpi_rover_bringup/launch/sim_robot.launch.py index 1b2e5b1..e52ad99 100644 --- a/src/platform/kpi_rover_bringup/launch/sim_robot.launch.py +++ b/src/platform/kpi_rover_bringup/launch/sim_robot.launch.py @@ -18,6 +18,7 @@ def generate_launch_description(): z_pose = LaunchConfiguration('z_pose') yaw_pose = LaunchConfiguration('yaw_pose') robot_ns = LaunchConfiguration('robot_ns') + camera = LaunchConfiguration('camera') declared_arguments = [ DeclareLaunchArgument( @@ -41,6 +42,9 @@ def generate_launch_description(): DeclareLaunchArgument( 'load_controllers', default_value='true', description='Load the controllers; false when something else loads them'), + DeclareLaunchArgument( + 'camera', default_value='true', + description='Add the camera to the model: /camera/image_raw, /camera/camera_info'), ] # robot_state_publisher with the sim_mode URDF (gz_ros2_control plugin) @@ -52,6 +56,7 @@ def generate_launch_description(): 'sim_mode': 'true', 'wheel_radius': wheel_radius, 'robot_ns': robot_ns, + 'camera': camera, }.items(), ) @@ -81,6 +86,19 @@ def generate_launch_description(): output='screen', ) + # The image goes through ros_gz_image, not gz_bridge.yaml: it adds the image_transport topics (compressed). + # The Gazebo topic is /camera/image_raw, the ROS topics /camera/image_raw[/]: + # image_transport names its topics from the topic before the remapping, so each one is remapped + camera_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=[[robot_ns, '/camera/image_raw']], + remappings=[([robot_ns, '/camera/image_raw' + t], '/camera/image_raw' + t) + for t in ('', '/compressed', '/compressedDepth', '/theora', '/zstd')], + output='screen', + condition=IfCondition(camera), + ) + joint_state_broadcaster = Node( package='controller_manager', executable='spawner', @@ -148,6 +166,7 @@ def generate_launch_description(): description, spawn_robot, gz_bridge, + camera_bridge, spawn_controllers, twist_stamper, localization, diff --git a/src/platform/kpi_rover_bringup/rviz/view_robot.rviz b/src/platform/kpi_rover_bringup/rviz/view_robot.rviz index 2e60388..6a2638b 100644 --- a/src/platform/kpi_rover_bringup/rviz/view_robot.rviz +++ b/src/platform/kpi_rover_bringup/rviz/view_robot.rviz @@ -109,6 +109,20 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Camera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/image_raw + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/src/platform/kpi_rover_description/launch/description.launch.py b/src/platform/kpi_rover_description/launch/description.launch.py index fde3e2c..d5f6dfe 100644 --- a/src/platform/kpi_rover_description/launch/description.launch.py +++ b/src/platform/kpi_rover_description/launch/description.launch.py @@ -17,6 +17,7 @@ def generate_launch_description(): baud_rate = LaunchConfiguration('baud_rate') wheel_radius = LaunchConfiguration('wheel_radius') robot_ns = LaunchConfiguration('robot_ns') + camera = LaunchConfiguration('camera') pkg_share = FindPackageShare('kpi_rover_description') @@ -30,7 +31,8 @@ def generate_launch_description(): ' serial_device:=', serial_device, ' baud_rate:=', baud_rate, ' wheel_radius:=', wheel_radius, - ' robot_ns:=', robot_ns + ' robot_ns:=', robot_ns, + ' camera:=', camera ]) # Launch! @@ -59,6 +61,10 @@ def generate_launch_description(): 'robot_ns', default_value='', description='Name of the robot in a simulation shared by several robots (Gazebo topics, controller manager)'), + DeclareLaunchArgument( + 'camera', + default_value='false', + description='Add the camera to the model'), Node( package='robot_state_publisher', diff --git a/src/platform/kpi_rover_description/urdf/camera.xacro b/src/platform/kpi_rover_description/urdf/camera.xacro index 37be21a..4a8125a 100644 --- a/src/platform/kpi_rover_description/urdf/camera.xacro +++ b/src/platform/kpi_rover_description/urdf/camera.xacro @@ -8,7 +8,8 @@ - + + @@ -35,16 +36,16 @@ true true 30 - camera/image_raw + $(arg robot_ns)/camera/image_raw camera_rgb_frame - camera/camera_info + $(arg robot_ns)/camera/camera_info 1.02974 4294967293 - 1920 - 1080 + 640 + 480 R8G8B8 diff --git a/src/platform/kpi_rover_description/urdf/robot.urdf.xacro b/src/platform/kpi_rover_description/urdf/robot.urdf.xacro index c333926..788971a 100644 --- a/src/platform/kpi_rover_description/urdf/robot.urdf.xacro +++ b/src/platform/kpi_rover_description/urdf/robot.urdf.xacro @@ -8,11 +8,14 @@ + - + + +