Add RGBD example

[EricCousineau-TRI]

Supersedes #323

For colcon testing:

ros2 launch drake_ros_examples rgbd_launch.py

---

This change is 

[EricCousineau-TRI]

Reviewable status: 0 of 27 files reviewed, 8 unresolved discussions (waiting on @EricCousineau-TRI)

a discussion (no related file):
Working: User consumption should follow something closer to Drake's camera_config and camera_config_functions.

---

---

drake_ros/core/camera_info_system.cc line 21 at r1 (raw file):

  now += rclcpp::Duration::from_seconds(context.get_time());
  output_value->header.stamp = now;
  output_value->header.frame_id = "CartPole/camera_optical";

Working: We should either not set this, or make it configurable.

---

drake_ros/core/rgbd_system.h line 31 at r1 (raw file):

  /** A constructor for the rbgd system.
   */
  RGBDSystem();

Working: This should be named RgbdSystem.

---

drake_ros/core/rgbd_system.h line 35 at r1 (raw file):

  ~RGBDSystem() override;

  const InputPort<double>& DeclareImageInputPort(PixelType pixel_type,

Working: Too much of this is public.

---

drake_ros/core/rgbd_system.cc line 33 at r1 (raw file):

void RGBDSystem::CalcColorImage(const drake::systems::Context<double>& context,
                                sensor_msgs::msg::Image* color_value) const {
  color_value->header.frame_id = "CartPole/camera_optical";

Working: Frame id should not be hard-coded.

---

drake_ros/core/rgbd_system.cc line 50 at r1 (raw file):

    const drake::systems::Context<double>& /*context*/,
    sensor_msgs::msg::Image* depth_value) const {
  depth_value->header.frame_id = "CartPole/camera_optical";

Working: Frame id should not be hard-coded.

---

drake_ros/core/rgbd_system.cc line 62 at r1 (raw file):

  float* depth_row_value = reinterpret_cast<float*>(&depth_value->data[0]);

  for (size_t x = 0; x < depth_value->data.size() / 4; ++x) {

nit Usage of 4 unclear.

---

drake_ros/core/rgbd_system.cc line 85 at r1 (raw file):

      break;
    case PixelType::kDepth16U: {
      return this->template DeclareDepthInputPort<PixelType::kDepth32F>(

Working: This doesn't seem to actually match? 32F <=> 16U.

[EricCousineau-TRI]

Reviewable status: 0 of 27 files reviewed, 9 unresolved discussions

a discussion (no related file):
Working: Given current state, will prototype in Anzu directly, and possibly hoist here once it stabilizes.

---