Daisukes/dic description dev (#99)

* implement function to request surrounding information

Signed-off-by: Daisuke Sato <[email protected]>

* implement description with live images

Signed-off-by: Daisuke Sato <[email protected]>

* change holddown down to holddown right to get description

Signed-off-by: Daisuke Sato <[email protected]>

* use holddown duration and plan length for request

Signed-off-by: Daisuke Sato <[email protected]>

* fix a bug in description event mapping

Signed-off-by: Masayuki Murata <[email protected]>

* bugfix in geoutil

Signed-off-by: Daisuke Sato <[email protected]>

* fix lint errors

Signed-off-by: Daisuke Sato <[email protected]>

* temporary fix to rotate camera images for cabot3-kx

Signed-off-by: Masayuki Murata <[email protected]>

* add environment variables for image description function

Signed-off-by: Daisuke Sato <[email protected]>

---------

Signed-off-by: Daisuke Sato <[email protected]>
Signed-off-by: Masayuki Murata <[email protected]>
Co-authored-by: Masayuki Murata <[email protected]>

README.md

@@ -87,6 +87,14 @@ RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
   CABOT_SIDE           # left: user stands on the right, right: user stands on the left
   CYCLONEDDS_NETWORK_INTERFACE_NAME # to specify network interface name for Cyclone DDS
   ```
+- Options for image description (hold down right button for 1-3 seconds)
+  ```
+  CABOT_IMAGE_DESCRIPTION_SERVER   # image description server (default=http://localhost:8000)
+  CABOT_IMAGE_DESCRIPTION_ENABLED  # enable image description (default=false)
+  CABOT_IMAGE_DESCRIPTION_ROTATE_LEFT   # rotate left image (default=false)
+  CABOT_IMAGE_DESCRIPTION_ROTATE_FRONT  # rotate front image (default=false)
+  CABOT_IMAGE_DESCRIPTION_ROTATE_RIGHT  # rotate right image (default=false)
+  ```
 - Options for debug/test
   ```
   CABOT_GAMEPAD              # (default=gamepad) gamepad type for remote controll (ex. PS4 controller)

cabot_ui/cabot_ui/description.py

@@ -0,0 +1,187 @@
+# Copyright (c) 2024  Carnegie Mellon University
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
+import rclpy.logging
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from nav_msgs.msg import Path
+from cv_bridge import CvBridge
+import cv2
+import base64
+import requests
+import json
+import os
+import math
+
+
+class Description:
+    DESCRIPTION_API = 'description'
+    DESCRIPTION_WITH_IMAGES_API = 'description_with_live_image'
+
+    def __init__(self, node: Node):
+        self.node = node
+        self.bridge = CvBridge()
+        self.max_size = 512
+        self.max_distance = node.declare_parameter("description_max_distance", 100).value
+        self.last_images = {'left': None, 'front': None, 'right': None}
+        self.last_plan_distance = None
+        self.host = os.environ.get("CABOT_IMAGE_DESCRIPTION_SERVER", "http://localhost:8000")
+        self.enabled = os.environ.get("CABOT_IMAGE_DESCRIPTION_ENABLED", False)
+        self._logger = rclpy.logging.get_logger("cabot_ui_manager.description")
+
+        self.subscriptions = {
+            'left': self.node.create_subscription(
+                Image,
+                '/rs3/color/image_raw',  # Change this to your left image topic name
+                lambda msg: self.image_callback('left', msg),
+                10
+            ),
+            'front': self.node.create_subscription(
+                Image,
+                '/rs1/color/image_raw',  # Change this to your front image topic name
+                lambda msg: self.image_callback('front', msg),
+                10
+            ),
+            'right': self.node.create_subscription(
+                Image,
+                '/rs2/color/image_raw',  # Change this to your right image topic name
+                lambda msg: self.image_callback('right', msg),
+                10
+            )
+        }
+
+        # rotate modes
+        rotate_left = os.environ.get("CABOT_IMAGE_DESCRIPTION_ROTATE_LEFT", False)
+        rotate_front = os.environ.get("CABOT_IMAGE_DESCRIPTION_ROTATE_FRONT", False)
+        rotate_right = os.environ.get("CABOT_IMAGE_DESCRIPTION_ROTATE_RIGHT", False)
+        self.image_rotate_modes = {
+            'left': cv2.ROTATE_180 if rotate_left else None,
+            'front': cv2.ROTATE_180 if rotate_front else None,
+            'right': cv2.ROTATE_180 if rotate_right else None,
+        }
+
+        self.plan_sub = self.node.create_subscription(Path, '/plan', self.plan_callback, 10)
+
+    def image_callback(self, position: str, msg: Image):
+        try:
+            # Store the raw ROS Image message
+            self.last_images[position] = msg
+        except Exception as e:
+            self._logger.error(f'Error processing {position} image: {e}')
+
+    def plan_callback(self, msg):
+        prev = None
+        dist = 0
+        for pose in msg.poses:
+            if prev is None:
+                prev = pose
+                continue
+            dx = pose.pose.position.x - prev.pose.position.x
+            dy = pose.pose.position.y - prev.pose.position.y
+            dist += math.sqrt(dx * dx + dy * dy)
+            prev = pose
+        self.last_plan_distance = dist
+
+    def request_description(self, gp):
+        self._logger.info(F"Request Description at {gp}")
+        try:
+            req = requests.get(
+                F"{self.host}/{Description.DESCRIPTION_API}?lat={gp.lat}&lng={gp.lng}&rotation={gp.r}&max_distance={self.max_distance}"
+            )
+            data = json.loads(req.text)
+            if req.status_code != requests.codes.ok:
+                self._logger.error(F"Request Error {data=}")
+                return
+            self._logger.info(F"Request result {data['description']=}")
+        except Exception as error:
+            self._logger.error(F"Request Error {error=}")
+
+    def request_description_with_images(self, gp, length_index=0):
+        if not self.enabled:
+            return None
+        self._logger.info(F"Request Description with images at {gp}")
+        image_data_list = []
+        distance_to_travel = 100
+        if self.last_plan_distance:
+            distance_to_travel = self.last_plan_distance
+        for position, img_msg in self.last_images.items():
+            if img_msg is not None:
+                try:
+                    # Convert ROS Image message to OpenCV image
+                    cv_image_raw = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
+
+                    # Rotate image if necessary
+                    rotate_mode = self.image_rotate_modes[position]
+                    if rotate_mode is not None:
+                        cv_image = cv2.rotate(cv_image_raw, rotate_mode)
+                        self._logger.info(f'Rotating {position} image: with {rotate_mode}')
+                    else:
+                        cv_image = cv_image_raw
+
+                    # Resize image while keeping the aspect ratio
+                    h, w = cv_image.shape[:2]
+                    if max(h, w) > self.max_size:
+                        scale = self.max_size / float(max(h, w))
+                        new_w = int(w * scale)
+                        new_h = int(h * scale)
+                        resized_image = cv2.resize(cv_image, (new_w, new_h), interpolation=cv2.INTER_AREA)
+                    else:
+                        resized_image = cv_image
+
+                    # Convert OpenCV image to JPEG
+                    _, buffer = cv2.imencode('.jpg', resized_image)
+
+                    # Encode JPEG to base64 and create image URI
+                    base64_image = base64.b64encode(buffer).decode('utf-8')
+                    image_uri = f'data:image/jpeg;base64,{base64_image}'
+
+                    # Prepare JSON data
+                    image_data_list.append({
+                        'position': position,
+                        'image_uri': image_uri
+                    })
+                except Exception as e:
+                    self._logger.error(f'Error converting {position} image: {e}')
+            else:
+                self._logger.warn(f'No {position} image available to process.')
+
+        # Send HTTP request with the image data
+        try:
+            headers = {'Content-Type': 'application/json'}
+            json_data = json.dumps(image_data_list)
+            self._logger.debug(F"Request data: {image_data_list}")
+            lat = gp.lat
+            lng = gp.lng
+            rotation = gp.r
+            max_distance = self.max_distance
+            response = requests.post(
+                F"{self.host}/{Description.DESCRIPTION_WITH_IMAGES_API}?{lat=}&{lng=}&{rotation=}&{max_distance=}&{length_index=}&{distance_to_travel=}",
+                headers=headers,
+                data=json_data
+            )
+            if response.status_code == 200:
+                response_json = response.json()
+                self._logger.info('Successfully sent images to server.')
+                return response_json
+            else:
+                self._logger.error(f'Failed to send images to server. Status code: {response.status_code}')
+        except Exception as e:
+            self._logger.error(f'Error sending HTTP request: {e}')
+        return None

cabot_ui/cabot_ui/geoutil.py

@@ -356,6 +356,16 @@ def distance_to(self, obj):
         raise RuntimeError(F"need to pass a Latlng object ({type(obj)})")
 
 
+class LatlngPose(Latlng):
+    def __init__(self, latlng, r):
+        self.lat = latlng.lat
+        self.lng = latlng.lng
+        self.r = r
+
+    def __repr__(self):
+        return F"[{self.lat:.7f}, {self.lng:.7f}] @ {self.r:.2f}"
+
+
 class Anchor(Latlng):
     """represent an anchor point. init with lat, lng, and rotate"""
     def __init__(self, **kwargs):
@@ -436,7 +446,16 @@ def local2global(xy, anchor):
     anchor.y = temp.y
     mer = xy2mercator(xy, anchor)
     latlng = mercator2latlng(mer)
-    return latlng
+    if hasattr(xy, 'r'):
+        r = xy.r - (90 + anchor.rotate) / 180.0 * math.pi
+        while r > math.pi:
+            r -= math.pi*2
+        while r < -math.pi:
+            r += math.pi*2
+        latlngr = LatlngPose(latlng, r)
+        return latlngr
+    else:
+        return latlng
 
 
 def get_anchor(anchor_file=None):

cabot_ui/cabot_ui/interface.py

@@ -364,3 +364,11 @@ def please_follow_behind(self):
     def please_return_position(self):
         self._activity_log("cabot/interface", "navigation", "please_return_position")
         self.speak(i18n.localized_string("RETURN_TO_POSITION_PLEASE"))
+
+    def requesting_describe_surround(self):
+        self._activity_log("cabot/interface", "requesting_describe_surround", "")
+        self.speak(i18n.localized_string("REQUESTING_DESCRIBE_SURROUND"))
+
+    def describe_surround(self, description):
+        self._activity_log("cabot/interface", "describe_surround", description)
+        self.speak(description)

cabot_ui/cabot_ui/navigation.py

@@ -446,6 +446,7 @@ def __init__(self, node: Node, tf_node: Node, srv_node: Node, act_node: Node, so
         self._start_loop()
 
     def _localize_status_callback(self, msg):
+        self._logger.info(F"_localize_status_callback {msg}")
         self.localize_status = msg.status
 
     def process_event(self, event):
@@ -824,6 +825,7 @@ def _check_loop(self):
         try:
             self.current_pose = self.current_local_pose()
             self.delegate.update_pose(ros_pose=self.current_ros_pose(),
+                                      current_pose=self.current_pose,
                                       global_position=self.current_global_pose(),
                                       current_floor=self.current_floor,
                                       global_frame=self._global_map_name

cabot_ui/i18n/en.yaml

@@ -114,3 +114,6 @@ IN_FRONT: "in front of you"
 RIGHT_SIDE: "on your right"
 LEFT_SIDE: "on your left"
 APPROACEHD_TO_FACILITY: "{0} is {1}"
+
+## Describe Surround
+REQUESTING_DESCRIBE_SURROUND: "Requesting surrounding situation"

cabot_ui/i18n/ja.yaml

@@ -114,3 +114,6 @@ IN_FRONT: "正面"
 RIGHT_SIDE: "右側"
 LEFT_SIDE: "左側"
 APPROACEHD_TO_FACILITY: "{1}に{0}があります"
+
+## Describe Surround
+REQUESTING_DESCRIBE_SURROUND: "周囲の状況を確認します。"

cabot_ui/scripts/cabot_ui_manager.py

@@ -57,13 +57,14 @@
 from cabot_ui.interface import UserInterface
 from cabot_ui.navigation import Navigation, NavigationInterface
 from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil
+from cabot_ui.description import Description
 
 from diagnostic_updater import Updater, FunctionDiagnosticTask
 from diagnostic_msgs.msg import DiagnosticStatus
 
 
 class CabotUIManager(NavigationInterface, object):
-    def __init__(self, node, nav_node, tf_node, srv_node, act_node, soc_node):
+    def __init__(self, node, nav_node, tf_node, srv_node, act_node, soc_node, desc_node):
         self._node = node
         self._logger = self._node.get_logger()
         CaBotRclpyUtil.initialize(self._node)
@@ -81,6 +82,7 @@ def __init__(self, node, nav_node, tf_node, srv_node, act_node, soc_node):
         self._interface.delegate = self
         self._navigation = Navigation(nav_node, tf_node, srv_node, act_node, soc_node)
         self._navigation.delegate = self
+        self._description = Description(desc_node)
         # self._exploration = Exploration()
         # self._exploration.delegate = self
 
@@ -387,6 +389,17 @@ def _process_navigation_event(self, event):
             self._interface.set_pause_control(True)
             self._navigation.set_pause_control(True)
 
+        if event.subtype == "description" and self._description.enabled:
+            # TODO: needs to reset last_plan_distance when arrived/paused
+            self._logger.info(F"Request Description duration={event.param}")
+            if self._interface.last_pose:
+                self._interface.requesting_describe_surround()
+                gp = self._interface.last_pose['global_position']
+                length_index = min(2, int(event.param) - 1)   # 1 sec -> 0, 2 sec -> 1, < 3 sec -> 2
+                result = self._description.request_description_with_images(gp, length_index=length_index)
+                if result:
+                    self._interface.describe_surround(result['description'])
+
         # operations depents on the current navigation state
         if self._status_manager.state == State.in_preparation:
             self.activity_log("cabot_ui/navigation", "in preparation")
@@ -532,6 +545,7 @@ def _process_exploration_event(self, event):
 class EventMapper(object):
     def __init__(self):
         self._manager = StatusManager.get_instance()
+        self.description_duration = 0
 
     def push(self, event):
         # state = self._manager.state
@@ -573,20 +587,26 @@ def map_button_to_menu(self, event):
         return None
 
     def map_button_to_navigation(self, event):
-        if event.type == "button" and event.down:
-            if event.button == cabot_common.button.BUTTON_UP:
+        if event.type == "button" and not event.down and self.description_duration > 0:
+            navigation_event = NavigationEvent(subtype="description", param=self.description_duration)
+            self.description_duration = 0
+            return navigation_event
+        if event.type == "click" and event.count == 1:
+            if event.buttons == cabot_common.button.BUTTON_UP:
                 return NavigationEvent(subtype="speedup")
-            if event.button == cabot_common.button.BUTTON_DOWN:
+            if event.buttons == cabot_common.button.BUTTON_DOWN:
                 return NavigationEvent(subtype="speeddown")
-            if event.button == cabot_common.button.BUTTON_LEFT:
+            if event.buttons == cabot_common.button.BUTTON_LEFT:
                 return NavigationEvent(subtype="pause")
-            if event.button == cabot_common.button.BUTTON_RIGHT:
+            if event.buttons == cabot_common.button.BUTTON_RIGHT:
                 return NavigationEvent(subtype="resume")
-            if event.button == cabot_common.button.BUTTON_CENTER:
+            if event.buttons == cabot_common.button.BUTTON_CENTER:
                 return NavigationEvent(subtype="decision")
         if event.type == HoldDownEvent.TYPE:
-            if event.holddown == cabot_common.button.BUTTON_LEFT:
+            if event.holddown == cabot_common.button.BUTTON_LEFT and event.duration == 3:
                 return NavigationEvent(subtype="idle")
+            if event.holddown == cabot_common.button.BUTTON_RIGHT:
+                self.description_duration = event.duration
         '''
         if event.button == cabot_common.button.BUTTON_SELECT:
                 return NavigationEvent(subtype="pause")
@@ -618,15 +638,18 @@ def receiveSignal(signal_num, frame):
     srv_node = Node("cabot_ui_manager_navigation_service", start_parameter_services=False)
     act_node = Node("cabot_ui_manager_navigation_actions", start_parameter_services=False)
     soc_node = Node("cabot_ui_manager_navigation_social", start_parameter_services=False)
-    nodes = [node, nav_node, tf_node, srv_node, act_node, soc_node]
+    desc_node = Node("cabot_ui_manager_description", start_parameter_services=False)
+    nodes = [node, nav_node, tf_node, srv_node, act_node, soc_node, desc_node]
     executors = [SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
                  SingleThreadedExecutor(),
-                 SingleThreadedExecutor()]
-    names = ["node", "tf", "nav", "srv", "act", "soc"]
-    manager = CabotUIManager(node, nav_node, tf_node, srv_node, act_node, soc_node)
+                 SingleThreadedExecutor(),
+                 SingleThreadedExecutor(),
+                 ]
+    names = ["node", "tf", "nav", "srv", "act", "soc", "desc"]
+    manager = CabotUIManager(node, nav_node, tf_node, srv_node, act_node, soc_node, desc_node)
 
     threads = []
     for tnode, executor, name in zip(nodes, executors, names):

docker-compose-common.yaml

@@ -45,6 +45,11 @@ services:
       - CABOT_INITZ
       - CABOT_INITA
       - CABOT_SPEED_POI_PARAMS
+      - CABOT_IMAGE_DESCRIPTION_SERVER
+      - CABOT_IMAGE_DESCRIPTION_ENABLED
+      - CABOT_IMAGE_DESCRIPTION_ROTATE_LEFT
+      - CABOT_IMAGE_DESCRIPTION_ROTATE_FRONT
+      - CABOT_IMAGE_DESCRIPTION_ROTATE_RIGHT
 # ROS2/DDS
       - ROS_LOG_DIR
       - RMW_IMPLEMENTATION