From 6ba656c384cdd8f73471659ae78c0a664907a581 Mon Sep 17 00:00:00 2001
From: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com>
Date: Thu, 25 Aug 2022 15:20:48 +0800
Subject: [PATCH] add CameraRos to rmf_demos_assets, add humans and camera to
 hotel world

Signed-off-by: Pranay Shirodkar <39498451+PranayShirodkar@users.noreply.github.com>
---
 .../models/CameraRos/model.config             | 16 +++++
 rmf_demos_assets/models/CameraRos/model.sdf   | 60 +++++++++++++++++++
 rmf_demos_maps/maps/hotel/hotel.building.yaml |  4 ++
 3 files changed, 80 insertions(+)
 create mode 100644 rmf_demos_assets/models/CameraRos/model.config
 create mode 100644 rmf_demos_assets/models/CameraRos/model.sdf

diff --git a/rmf_demos_assets/models/CameraRos/model.config b/rmf_demos_assets/models/CameraRos/model.config
new file mode 100644
index 00000000..54d7af16
--- /dev/null
+++ b/rmf_demos_assets/models/CameraRos/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>CameraRos</name>
+  <version>1.0</version>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Nate Koenig</name>
+    <email>nate@osrfoundation.org</email>
+  </author>
+
+  <description>
+    A simple camera with a box for visualization.
+  </description>
+</model>
diff --git a/rmf_demos_assets/models/CameraRos/model.sdf b/rmf_demos_assets/models/CameraRos/model.sdf
new file mode 100644
index 00000000..8eb2afff
--- /dev/null
+++ b/rmf_demos_assets/models/CameraRos/model.sdf
@@ -0,0 +1,60 @@
+<?xml version="1.0"?>
+
+<sdf version="1.7">
+  <model name="CameraRos">
+    <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+    <static>true</static>
+
+    <link name="visual_link">
+      <pose>0.0 0.0 0.05 0.0 0.0 0.0</pose>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.1 0.1 0.1</size>
+          </box>
+        </geometry>
+      </visual>
+      <sensor name="camera" type="camera">
+        <camera>
+          <horizontal_fov>2</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <lens>
+            <intrinsics>
+              <!-- it is up to the user to ensure that:
+              horizontal_fov = 2 * atan2( width, (2*fx) )
+              width = cx * 2
+              height = cy * 2 -->
+              <fx>205.4696</fx>
+              <fy>205.4696</fy>
+              <cx>320</cx>
+              <cy>240</cy>
+            </intrinsics>
+          </lens>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>15</update_rate>
+        <visualize>true</visualize>
+      </sensor>
+    </link>
+    <plugin
+      filename="ignition-gazebo-pose-publisher-system"
+      name="ignition::gazebo::systems::PosePublisher">
+      <publish_model_pose>true</publish_model_pose>
+      <publish_nested_model_pose>true</publish_nested_model_pose>
+      <use_pose_vector_msg>true</use_pose_vector_msg>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre</render_engine>
+    </plugin>
+  </model>
+</sdf>
diff --git a/rmf_demos_maps/maps/hotel/hotel.building.yaml b/rmf_demos_maps/maps/hotel/hotel.building.yaml
index a0b3689c..c4fc9567 100644
--- a/rmf_demos_maps/maps/hotel/hotel.building.yaml
+++ b/rmf_demos_maps/maps/hotel/hotel.building.yaml
@@ -163,6 +163,10 @@ levels:
       - {model_name: OpenRobotics/Toilet, name: OpenRobotics/Toilet, static: true, x: 512.076, y: 712.3680000000001, yaw: 1.5708, z: 0}
       - {model_name: OpenRobotics/Toilet, name: OpenRobotics/Toilet, static: true, x: 537.609, y: 712.199, yaw: 1.5708, z: 0}
       - {model_name: OpenRobotics/Toilet, name: OpenRobotics/Toilet, static: true, x: 559.9299999999999, y: 712.876, yaw: 1.5708, z: 0}
+      - {model_name: OpenRobotics/Standing person, name: OpenRobotics/Standing person, static: true, x: 268.87, y: 597.53700000000003, yaw: 1.5979000000000001, z: 0}
+      - {model_name: OpenRobotics/Standing person, name: OpenRobotics/Standing person, static: true, x: 296.88099999999997, y: 565.24000000000001, yaw: -0.42659999999999998, z: 0}
+      - {model_name: OpenRobotics/Standing person, name: OpenRobotics/Standing person, static: true, x: 325.17700000000002, y: 596.39400000000001, yaw: -2.6819000000000002, z: 0}
+      - {model_name: OpenRobotics/CameraRos, name: camera1, static: true, x: 237.31299999999999, y: 717.54499999999996, yaw: 1, z: 2.2}
     vertices:
       - [127.862, 550.573, 0, ""]
       - [127.1, 350.915, 0, ""]