Implement PTZ action server interface (#65)

* Add remappings & bridge configuration for the PTZ joint states

* Remap the raw velocity topics from Gazebo, implement the PTZ action server interface. Add a 5x digital zoom to simulate real-world zoom control

clearpath_generator_gz/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(ament_cmake_python REQUIRED)
 
 install(PROGRAMS
   ${PROJECT_NAME}/launch/generate_launch
+  ${PROJECT_NAME}/nodes/ptz_controller_node
   ${PROJECT_NAME}/param/generate_param
   DESTINATION lib/${PROJECT_NAME}
 )

clearpath_generator_gz/clearpath_generator_gz/launch/sensors.py

@@ -55,6 +55,7 @@ class SensorLaunch():
     GZ_TO_ROS_CAMERA_INFO = '@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo'
     GZ_TO_ROS_IMU = '@sensor_msgs/msg/Imu[gz.msgs.IMU'
     GZ_TO_ROS_NAVSAT = '@sensor_msgs/msg/NavSatFix[gz.msgs.NavSat'
+    GZ_TO_ROS_JOINTSTATE = '@sensor_msgs/msg/JointState[gz.msgs.Model'
 
     ROS_TO_GZ_FLOAT = '@std_msgs/msg/Float64]gz.msgs.Double'
 
@@ -105,7 +106,10 @@ def __init__(
         self.extra_gz_nodes = []
 
         # Cameras
-        if self.sensor.SENSOR_TYPE == BaseCamera.SENSOR_TYPE:
+        if (
+            self.sensor.SENSOR_TYPE == BaseCamera.SENSOR_TYPE and
+            self.sensor.SENSOR_MODEL not in self.PTZ_CAMERAS
+        ):
             image_ns = '/' + self.namespace + self.name
             image_topic = image_ns + '/image'
 
@@ -150,20 +154,69 @@ def __init__(
             self.extra_gz_nodes.append(depth_bridge_node)
 
         if self.sensor.SENSOR_MODEL in self.PTZ_CAMERAS:
+            image_ns = '/' + self.namespace + self.name
+            image_topic = image_ns + '/image'
+
+            image_bridge_node = LaunchFile.Node(
+                name=self.name + '_gz_image_bridge',
+                namespace=self.namespace,
+                package='ros_gz_image',
+                executable='image_bridge',
+                parameters=[{
+                    'use_sim_time': True,
+                }],
+                arguments=[image_topic],
+                remappings=[
+                    (image_topic, image_ns + '/_/image_raw'),
+                    (image_topic + '/compressed', image_ns + '/_/compressed'),
+                    (image_topic + '/compressedDepth', image_ns + '/_/compressedDepth'),
+                    (image_topic + '/theora', image_ns + '/_/theora'),
+                ]
+            )
+            self.extra_gz_nodes.append(image_bridge_node)
+
             cmd_ns = '/' + self.namespace + self.name
             cmd_bridge_node = LaunchFile.Node(
-                name=self.name + '_gz_cmd_node',
+                name=self.name + '_gz_cmd_bridge',
                 namespace=self.namespace,
                 package='ros_gz_bridge',
                 executable='parameter_bridge',
                 parameters=[{'use_sim_time': True}],
                 arguments = [
                     cmd_ns + '/cmd_pan_vel' + self.ROS_TO_GZ_FLOAT,
                     cmd_ns + '/cmd_tilt_vel' + self.ROS_TO_GZ_FLOAT,
-                ]
+                    cmd_ns + '/pan_joint_state' + self.GZ_TO_ROS_JOINTSTATE,
+                    cmd_ns + '/tilt_joint_state' + self.GZ_TO_ROS_JOINTSTATE,
+                ],
+                remappings=[
+                    (cmd_ns + '/pan_joint_state', '/' + self._robot_namespace + '/platform/joint_states'),
+                    (cmd_ns + '/tilt_joint_state', '/' + self._robot_namespace + '/platform/joint_states'),
+                    (cmd_ns + '/cmd_pan_vel', cmd_ns + '/_/cmd_pan_vel'),
+                    (cmd_ns + '/cmd_tilt_vel', cmd_ns + '/_/cmd_tilt_vel'),
+                ],
             )
             self.extra_gz_nodes.append(cmd_bridge_node)
 
+            ptz_node = LaunchFile.Node(
+                name='ptz_action_server_node',
+                namespace=image_ns,
+                package='clearpath_generator_gz',
+                executable='ptz_controller_node',
+                parameters=[
+                    {'use_sim_time': True},
+                    {'camera_name': self.name},
+                ],
+                remappings=[
+                    ('image_in', image_ns + '/_/image_raw'),
+                    ('image_out', image_ns + '/color/image'),
+                    ('cmd/velocity', image_ns + '/cmd/velocity'),
+                    ('joint_states', '/' + self._robot_namespace + '/platform/joint_states'),
+                    ('cmd_pan_vel', cmd_ns + '/_/cmd_pan_vel'),
+                    ('cmd_tilt_vel', cmd_ns + '/_/cmd_tilt_vel'),
+                ],
+            )
+            self.extra_gz_nodes.append(ptz_node)
+
     def generate(self):
         sensor_writer = LaunchWriter(self.launch_file)
         # Add sensor bridge and tf nodes