From a7a5e92a2e0babd1fa76ddc406fe3f92af62b045 Mon Sep 17 00:00:00 2001 From: philippewarren Date: Fri, 28 Jan 2022 14:39:21 -0500 Subject: [PATCH] wip --- .../opentera-webrtc-teleop-frontend | 2 +- opentera_webrtc_ros/scripts/labels_manager.py | 165 +++++++---- .../scripts/liblabelsdatabase.py | 268 ++++++++++++++++++ opentera_webrtc_ros/scripts/libposediff.py | 55 ++++ 4 files changed, 435 insertions(+), 55 deletions(-) create mode 100644 opentera_webrtc_ros/scripts/liblabelsdatabase.py create mode 100644 opentera_webrtc_ros/scripts/libposediff.py diff --git a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend index 21c55b4..5cc062f 160000 --- a/opentera_webrtc_demos/opentera-webrtc-teleop-frontend +++ b/opentera_webrtc_demos/opentera-webrtc-teleop-frontend @@ -1 +1 @@ -Subproject commit 21c55b41ed91a82876dfd4369b36595425a79ed5 +Subproject commit 5cc062f93cec44a0219d37057d6454f424a61cf2 diff --git a/opentera_webrtc_ros/scripts/labels_manager.py b/opentera_webrtc_ros/scripts/labels_manager.py index 4a36758..98e8e77 100755 --- a/opentera_webrtc_ros/scripts/labels_manager.py +++ b/opentera_webrtc_ros/scripts/labels_manager.py @@ -9,64 +9,116 @@ from opentera_webrtc_ros_msgs.msg import Label, LabelArray, LabelEdit from opentera_webrtc_ros_msgs.msg import Waypoint from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseStamped, Pose from libmapimageconverter import convert_waypoint_to_pose as wp2pose from libmapimageconverter import convert_pose_to_waypoint as pose2wp from libyamldatabase import YamlDatabase +from liblabelsdatabase import LabelsDb, LabelData from libnavigation import WaypointNavigationClient +from rtabmap_ros.srv import GetNodesInRadius, GetNodesInRadiusRequest, GetNodesInRadiusResponse +from rtabmap_ros.srv import GetMap, GetMapRequest, GetMapResponse +from libposediff import transform_from_pose, pose_from_transform, invert_transform +from functools import cached_property +from rtabmap_ros.msg import NodeData +from typing import Tuple, cast, List class ConversionError(Exception): pass -class LabelData: - yaml_tag = "!label" - - def __init__(self, label: Label) -> None: - self.data = { - "name": label.name, - "description": label.description, - "pose": { - "header": { - "frame_id": label.pose.header.frame_id, - }, - "pose": { - "position": { - "x": label.pose.pose.position.x, - "y": label.pose.pose.position.y, - "z": label.pose.pose.position.z, - }, - "orientation": { - "x": label.pose.pose.orientation.x, - "y": label.pose.pose.orientation.y, - "z": label.pose.pose.orientation.z, - "w": label.pose.pose.orientation.w, - }, - }, - }, - } - - @property - def label(self) -> Label: - pose = PoseStamped() - pose.header.frame_id = str(self.data["pose"]["header"]["frame_id"]) - pose.pose.position.x = float( - self.data["pose"]["pose"]["position"]["x"]) - pose.pose.position.y = float( - self.data["pose"]["pose"]["position"]["y"]) - pose.pose.position.z = float( - self.data["pose"]["pose"]["position"]["z"]) - pose.pose.orientation.x = float( - self.data["pose"]["pose"]["orientation"]["x"]) - pose.pose.orientation.y = float( - self.data["pose"]["pose"]["orientation"]["y"]) - pose.pose.orientation.z = float( - self.data["pose"]["pose"]["orientation"]["z"]) - pose.pose.orientation.w = float( - self.data["pose"]["pose"]["orientation"]["w"]) - - return Label(name=self.data["name"], description=self.data["description"], pose=pose) +class LabelDataUtil: + def __init__(self) -> None: + self.get_node_from_pose_service = rospy.ServiceProxy( + "/rtabmap/get_nodes_in_radius", GetNodesInRadius) + + def make_label_data(self, label: Label) -> LabelData: + self.get_node_from_pose_service.wait_for_service() + req = GetNodesInRadiusRequest() + req.node_id = 0 + req.x = label.pose.pose.position.x + req.y = label.pose.pose.position.y + req.z = label.pose.pose.position.z + req.radius = 0 + # req.k = 1 + + try: + rep: GetNodesInRadiusResponse = self.get_node_from_pose_service.call( + req) + if rep.poses is None or len(rep.poses) == 0 or rep.ids is None or len(rep.ids) == 0: + raise ConversionError(f"No node found near label {label.name}") + except rospy.ServiceException as e: + raise ConversionError( + f"Getting nearest node of label {label.name} failed: {e}") + + pose: Pose = rep.poses[0] + posem = transform_from_pose(pose) + lposem = transform_from_pose(label.pose.pose) + diff_posem = invert_transform(posem) * lposem + + return LabelData(label, rep.ids[0], node_tf=pose_from_transform(diff_posem)) + + def update_label_data(self, label_data: LabelData) -> LabelData: + del self._node_data # Regenerate cache + for node in self._node_data: + pass + + @cached_property + def _node_data(self) -> Tuple[NodeData, ...]: + service = rospy.ServiceProxy("/rtabmap/get_map_data", GetMap) + service.wait_for_service() + data: GetMapResponse = service.call(GetMapRequest(True, True, True)) + assert ( + data is not None), "No data received from rtabmap, this should not happen" + return tuple(cast(List[NodeData], data.data.nodes)) + + # class LabelData: + # yaml_tag = "!label" + + # def __init__(self, label: Label) -> None: + # self.data = { + # "name": label.name, + # "description": label.description, + # "pose": { + # "header": { + # "frame_id": label.pose.header.frame_id, + # }, + # "pose": { + # "position": { + # "x": label.pose.pose.position.x, + # "y": label.pose.pose.position.y, + # "z": label.pose.pose.position.z, + # }, + # "orientation": { + # "x": label.pose.pose.orientation.x, + # "y": label.pose.pose.orientation.y, + # "z": label.pose.pose.orientation.z, + # "w": label.pose.pose.orientation.w, + # }, + # }, + # }, + # } + + # @property + # def label(self) -> Label: + # pose = PoseStamped() + # pose.header.frame_id = str(self.data["pose"]["header"]["frame_id"]) + # pose.pose.position.x = float( + # self.data["pose"]["pose"]["position"]["x"]) + # pose.pose.position.y = float( + # self.data["pose"]["pose"]["position"]["y"]) + # pose.pose.position.z = float( + # self.data["pose"]["pose"]["position"]["z"]) + # pose.pose.orientation.x = float( + # self.data["pose"]["pose"]["orientation"]["x"]) + # pose.pose.orientation.y = float( + # self.data["pose"]["pose"]["orientation"]["y"]) + # pose.pose.orientation.z = float( + # self.data["pose"]["pose"]["orientation"]["z"]) + # pose.pose.orientation.w = float( + # self.data["pose"]["pose"]["orientation"]["w"]) + + # return Label(name=self.data["name"], description=self.data["description"], pose=pose) class LabelsManager: @@ -88,8 +140,9 @@ def __init__(self) -> None: self.database_path: str = rospy.get_param( "~database_path", "~/.ros/labels.yaml") - self.db: YamlDatabase[LabelData] = YamlDatabase( - Path(self.database_path), LabelData) + # self.db: YamlDatabase[LabelData] = YamlDatabase( + # Path(self.database_path), LabelData) + self.db = LabelsDb(Path(self.database_path)) self.pub_timer_stored_labels = rospy.Timer(rospy.Duration( 1), self.publish_stored_labels) @@ -98,6 +151,8 @@ def __init__(self) -> None: self.nav_client = WaypointNavigationClient() + self.label_data_utils = LabelDataUtil() + rospy.loginfo("Labels manager initialized") def publish_stored_labels_text(self, _: rospy.timer.TimerEvent) -> None: @@ -114,7 +169,8 @@ def publish_stored_labels(self, _: rospy.timer.TimerEvent) -> None: def add_label_simple_callback(self, msg: LabelSimple) -> None: try: - label = LabelData(self.simple2label(msg)) + label = self.label_data_utils.make_label_data( + self.simple2label(msg)) self.db.add(msg.name, label) self.db.commit() except (IndexError, ConversionError) as e: @@ -135,7 +191,8 @@ def edit_label_simple_callback(self, msg: LabelSimpleEdit) -> None: updated = self.simple2label(msg.updated) if msg.ignore_waypoint is True: updated.pose = self.db[msg.updated.name].label.pose - self.db.replace(msg.updated.name, LabelData(updated)) + self.db.replace(msg.updated.name, + self.label_data_utils.make_label_data(updated)) self.db.commit() except (IndexError, ConversionError) as e: @@ -151,7 +208,7 @@ def navigate_to_label_callback(self, msg: String) -> None: self.nav_client.add_to_image(label.pose) self.nav_client.navigate_to_goal(label.pose, 1) - @staticmethod + @ staticmethod def label2simple(label: Label) -> LabelSimple: waypoint = pose2wp(label.pose) if waypoint is None: @@ -159,7 +216,7 @@ def label2simple(label: Label) -> LabelSimple: f"Conversion of pose to waypoint for label {label.name} failed") return LabelSimple(name=label.name, description=label.description, waypoint=waypoint) - @staticmethod + @ staticmethod def simple2label(label_simple: LabelSimple) -> Label: pose = wp2pose(label_simple.waypoint) if pose is None: diff --git a/opentera_webrtc_ros/scripts/liblabelsdatabase.py b/opentera_webrtc_ros/scripts/liblabelsdatabase.py new file mode 100644 index 0000000..d5a51cf --- /dev/null +++ b/opentera_webrtc_ros/scripts/liblabelsdatabase.py @@ -0,0 +1,268 @@ +from __future__ import annotations + +import rospy +import sqlite3 +from typing import Union +from pathlib import Path +from opentera_webrtc_ros_msgs.msg import Label +from geometry_msgs.msg import PoseStamped, Pose, Vector3, Quaternion +from std_msgs.msg import Header +from dataclasses import dataclass +from typing import Generator + + +@dataclass +class LabelData: + label: Label + node_id: int + node_tf: Pose + + +class LabelsDb: + def __init__(self, db_path: Union[str, Path]) -> None: + self.path = Path(db_path).expanduser() + self.db = LabelsDb._make_db(self.path) + self._make_table() + self.commit() + + def __getitem__(self, __name: str) -> LabelData: + return self._get(__name) + + def __setitem__(self, __name: str, label: LabelData) -> None: + self._set(__name, label) + + def __delitem__(self, __name: str) -> None: + self.db.execute( + "DELETE FROM OpenTeraLabels WHERE name=:name", {"name": __name}) + + def __contains__(self, __name: str) -> bool: + val = self.db.execute( + "SELECT EXISTS (SELECT name FROM OpenTeraLabels WHERE name=:name)", {"name": __name}).fetchone() + return bool(val[0]) + + def get(self, name: str) -> LabelData: + return self._get(name) + + def remove(self, name) -> None: + if name not in self: + raise IndexError(f"{name} not found in database") + else: + del self[name] + + def add(self, name: str, label: LabelData) -> None: + if name in self: + raise IndexError(f"{name} already exists in database") + else: + self._add(name, label) + + def rename(self, name: str, new_name: str) -> None: + if name not in self: + raise IndexError(f"{name} not found in database") + elif new_name in self: + raise IndexError(f"{new_name} already exists in database") + else: + self.db.execute("UPDATE OpenTeraLabels SET name=:new_name WHERE name=:name", + {"name": name, "new_name": new_name}) + + def replace(self, name: str, label: LabelData) -> None: + if name not in self: + raise IndexError(f"{name} not found in database") + else: + self._edit(name, label) + + def commit(self) -> None: + self.db.commit() + + def rollback(self) -> LabelsDb: + self.db.rollback() + return self + + def clear(self) -> LabelsDb: + self._clear_table() + self._make_table() + return self + + def values(self) -> Generator[LabelData, None, None]: + return (v for v in self.db.execute("SELECT * FROM OpenTeraLabels")) + + def _fetch(self, __name: str) -> sqlite3.Cursor: + return self.db.execute("SELECT * FROM OpenTeraLabels WHERE name=:name", {"name": __name}) + + def _set(self, __name: str, label: LabelData) -> None: + self._add(__name, label) + self._edit(__name, label) + + def _edit(self, __name: str, label: LabelData) -> None: + self.db.execute(""" + UPDATE OpenTeraLabels SET ( + name=:name, + node_id=:node_id, + description=:description, + frame_id=:frame_id, + position_x=:position_x, + position_y=:position_y, + position_z=:position_z, + orientation_x=:orientation_x, + orientation_y=:orientation_y, + orientation_z=:orientation_z, + orientation_w=:orientation_w, + tf_position_x=:tf_position_x, + tf_position_y=:tf_position_y, + tf_position_z=:tf_position_z, + tf_orientation_x=:tf_orientation_x, + tf_orientation_y=:tf_orientation_y, + tf_orientation_z=:tf_orientation_z, + tf_orientation_w=:tf_orientation_w + ) WHERE name=:old_name + """, { + "name": label.label.name, + "node_id": label.node_id, + "description": label.label.description, + "frame_id": label.label.pose.header.frame_id, + "position_x": label.label.pose.pose.position.x, + "position_y": label.label.pose.pose.position.y, + "position_z": label.label.pose.pose.position.z, + "orientation_x": label.label.pose.pose.orientation.x, + "orientation_y": label.label.pose.pose.orientation.y, + "orientation_z": label.label.pose.pose.orientation.z, + "orientation_w": label.label.pose.pose.orientation.w, + "tf_position_x": label.node_tf.position.x, + "tf_position_y": label.node_tf.position.y, + "tf_position_z": label.node_tf.position.z, + "tf_orientation_x": label.node_tf.orientation.x, + "tf_orientation_y": label.node_tf.orientation.y, + "tf_orientation_z": label.node_tf.orientation.z, + "tf_orientation_w": label.node_tf.orientation.w, + "old_name": __name, + }) + + def _add(self, __name: str, label: LabelData) -> None: + self.db.execute(""" + INSERT OR IGNORE INTO OpenTeraLabels ( + name, + node_id, + description, + frame_id, + position_x, + position_y, + position_z, + orientation_x, + orientation_y, + orientation_z, + orientation_w, + tf_position_x, + tf_position_y, + tf_position_z, + tf_orientation_x, + tf_orientation_y, + tf_orientation_z, + tf_orientation_w + ) VALUES ( + :name, + :node_id, + :description, + :frame_id, + :position_x, + :position_y, + :position_z, + :orientation_x, + :orientation_y, + :orientation_z, + :orientation_w, + :tf_position_x, + :tf_position_y, + :tf_position_z, + :tf_orientation_x, + :tf_orientation_y, + :tf_orientation_z, + :tf_orientation_w + ) + """, { + "name": __name, + "node_id": label.node_id, + "description": label.label.description, + "frame_id": label.label.pose.header.frame_id, + "position_x": label.label.pose.pose.position.x, + "position_y": label.label.pose.pose.position.y, + "position_z": label.label.pose.pose.position.z, + "orientation_x": label.label.pose.pose.orientation.x, + "orientation_y": label.label.pose.pose.orientation.y, + "orientation_z": label.label.pose.pose.orientation.z, + "orientation_w": label.label.pose.pose.orientation.w, + "tf_position_x": label.node_tf.position.x, + "tf_position_y": label.node_tf.position.y, + "tf_position_z": label.node_tf.position.z, + "tf_orientation_x": label.node_tf.orientation.x, + "tf_orientation_y": label.node_tf.orientation.y, + "tf_orientation_z": label.node_tf.orientation.z, + "tf_orientation_w": label.node_tf.orientation.w, + }) + + def _get(self, __name: str) -> LabelData: + val = self._fetch(__name).fetchone() + if val is None: + raise IndexError(f"{__name} not found in database") + + header = Header(frame_id=val["frame_id"]) + position = Vector3(x=val["position_x"], + y=val["position_y"], z=val["position_z"]) + orientation = Quaternion( + x=val["orientation_x"], y=val["orientation_y"], z=val["orientation_z"], w=val["orientation_w"]) + pose = Pose(position=position, orientation=orientation) + pose_stamped = PoseStamped(header=header, pose=pose) + label = Label(name=val["name"], description=val["description"], + pose=pose_stamped) + + tf_position = Vector3( + x=val["tf_position_x"], y=val["tf_position_y"], z=val["tf_position_z"]) + tf_orientation = Quaternion( + x=val["tf_orientation_x"], y=val["tf_orientation_y"], z=val["tf_orientation_z"], w=val["tf_orientation_w"]) + node_tf = Pose(position=tf_position, orientation=tf_orientation) + + return LabelData(label=label, node_id=val["node_id"], node_tf=node_tf) + + @staticmethod + def _make_db(sb_path: Path) -> sqlite3.Connection: + + for i in range(10): + if not sb_path.exists(): + rospy.logwarn( + "Labels database not found. Trying again in 2 seconds.") + rospy.sleep(2) + else: + break + else: + rospy.logerr( + "Could not find labels database at {}".format(sb_path)) + raise FileNotFoundError + + db = sqlite3.connect(sb_path) + db.row_factory = sqlite3.Row + + return db + + def _make_table(self) -> None: + self.db.execute("""CREATE TABLE IF NOT EXISTS "OpenTeraLabels" ( + "name" TEXT NOT NULL UNIQUE, + "node_id" INTEGER NOT NULL, + "description" TEXT DEFAULT "", + "frame_id" TEXT DEFAULT "map", + "position_x" REAL DEFAULT 0, + "position_y" REAL DEFAULT 0, + "position_z" REAL DEFAULT 0, + "orientation_x" REAL DEFAULT 0, + "orientation_y" REAL DEFAULT 0, + "orientation_z" REAL DEFAULT 0, + "orientation_w" REAL DEFAULT 0, + "tf_position_x" REAL DEFAULT 0, + "tf_position_y" REAL DEFAULT 0, + "tf_position_z" REAL DEFAULT 0, + "tf_orientation_x" REAL DEFAULT 0, + "tf_orientation_y" REAL DEFAULT 0, + "tf_orientation_z" REAL DEFAULT 0, + "tf_orientation_w" REAL DEFAULT 0, + PRIMARY KEY("name") + )""") + + def _clear_table(self) -> None: + self.db.execute("DROP TABLE IF EXISTS OpenTeraLabels") diff --git a/opentera_webrtc_ros/scripts/libposediff.py b/opentera_webrtc_ros/scripts/libposediff.py new file mode 100644 index 0000000..1eb170b --- /dev/null +++ b/opentera_webrtc_ros/scripts/libposediff.py @@ -0,0 +1,55 @@ +# From https://linuxtut.com/en/80c8e0fa539ddff617e5/ + +# from tf.transformations import euler_from_quaternion, quaternion_from_euler +# from tf.transformations import compose_matrix, decompose_matrix, inverse_matrix +from geometry_msgs.msg import Pose, Point, Quaternion +import numpy as np +# from typing import List +# from numpy import ndarray + +from pytransform3d.transformations import transform_from_pq, pq_from_transform, invert_transform + + +def transform_from_pose(pose: Pose) -> np.ndarray: + """ + Convert a Pose to a 4x4 homogeneous transformation matrix. + """ + return transform_from_pq((pose.position.x, pose.position.y, pose.position.z, + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) + + +def pose_from_transform(transform: np.ndarray) -> Pose: + """ + Convert a 4x4 homogeneous transformation matrix to a Pose. + """ + p, q = pq_from_transform(transform) + return Pose(position=Point(*p), orientation=Quaternion(*q)) + +# def pose2homogeneousM(poseobj: Pose) -> ndarray: +# # It says Quat to euler sxyz, but the order of XYZW is fine. Isn't it a little confusing? +# tfeul = euler_from_quaternion( +# [poseobj.orientation.x, poseobj.orientation.y, poseobj.orientation.z, poseobj.orientation.w], axes='sxyz') +# # Description of translation amount +# tftrans = [poseobj.position.x, poseobj.position.y, poseobj.position.z] +# poseobjM = compose_matrix(angles=tfeul, translate=tftrans) +# return poseobjM + + +# def homogeneousM2pose(Mat: ndarray): +# _, _, angles, trans, _ = decompose_matrix(Mat) +# quat = quaternion_from_euler(angles[0], angles[1], angles[2]) +# poseobj = Pose() +# poseobj.orientation.x = quat[0] +# poseobj.orientation.y = quat[1] +# poseobj.orientation.z = quat[2] +# poseobj.orientation.w = quat[3] +# poseobj.position.x = trans[0] +# poseobj.position.y = trans[1] +# poseobj.position.z = trans[2] +# return poseobj + + +# def pose_diff(p1, p2): +# p1M = pose2homogeneousM(p1) +# p2M = pose2homogeneousM(p2) +# return homogeneousM2pose(p2M.dot(inverse_matrix(p1M)))