Skip to content

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@philippewarren
philippewarren committed Jan 28, 2022
1 parent 9ff640a commit a7a5e92
Show file tree
Hide file tree
Showing 4 changed files with 435 additions and 55 deletions.
2 changes: 1 addition & 1 deletion opentera_webrtc_demos/opentera-webrtc-teleop-frontend
Submodule opentera-webrtc-teleop-frontend updated 6 files
+7 −3 teleop-vue/src/components/Dropdown/Dropdown.scss
+40 −0 teleop-vue/src/components/DropdownMenu/DropdownMenu.scss
+106 −0 teleop-vue/src/components/DropdownMenu/DropdownMenu.vue
+1 −0 teleop-vue/src/components/DropdownMenu/index.ts
+33 −57 teleop-vue/src/components/ExpandableMap/ExpandableMap.scss
+89 −73 teleop-vue/src/components/ExpandableMap/ExpandableMap.vue
165 changes: 111 additions & 54 deletions opentera_webrtc_ros/scripts/labels_manager.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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)
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -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:
Expand All @@ -151,15 +208,15 @@ 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:
raise ConversionError(
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:
Expand Down
Loading

0 comments on commit a7a5e92

Please sign in to comment.