Feat/get room

README.md

@@ -76,25 +76,67 @@ fbot_world/
 ## Usage
 
 ### Pose Node
+Loads a poses/rooms YAML file and serves it through the `/fbot_world/*` services
+(see the table below). It also publishes RViz markers of every room and object
+sub-area on `/fbot_world/debug_markers`, and answers "which room am I in?" queries
+via `/fbot_world/get_room`.
 ```bash
 # Launch pose node
 ros2 launch fbot_world pose.launch.py config_file_name:=file_name_without_dot_yaml
 ```
 
 ### Pose Writer Node
+Interactively records the robot's **base poses** and saves them under the
+`targets` group of a YAML file. Drive/teleop the robot to a spot, give the pose a
+name, and the current `/amcl_pose` is captured (position + orientation). New poses
+are appended to the chosen file, so existing entries are preserved.
 ```bash
-# Launch YOLO tracker with pose estimation
 ros2 run fbot_world pose_writer
 ```
 
+### Place Pose Writer Node
+Records fixed **end-effector "place" poses** (e.g. a shelf or bin drop point) under
+the `place_poses` group. It reads the gripper pose from TF (in the `map` frame) and
+**disables arm torque** so you can hand-guide the arm to the target, name it, and
+capture it; torque is re-enabled on exit.
+
+> ⚠️ The arm goes limp when torque is cut — hold it before confirming the prompt.
+
+Configurable via parameters: `reference_frame` (default `map`), `ee_frame`
+(default `wx200/ee_gripper_link`), `group_set` (default `place_poses`),
+`robot_name` (default `wx200`) and `torque_group` (default `arm`).
+```bash
+ros2 run fbot_world place_pose_writer
+```
+
+### Room Writer Node
+Annotates **room boundaries and object sub-areas** by clicking points in RViz, and
+writes them to the `rooms` section of a YAML file — the same structure the Pose
+Node loads (used by `/fbot_world/get_room`). Only the `rooms` section is written;
+any existing `poses`/`targets` are left untouched.
+
+Workflow: launch navigation with your map and RViz, run the node, then use the
+RViz **Publish Point** tool to click each corner of a room. The in-progress polygon
+is previewed live; when you close it, the finished area is drawn filled, labelled
+and coloured. You can then type the pose names that belong to the room and draw any
+object sub-areas the same way.
+
+In RViz, add a **MarkerArray** display on `/room_writer/preview` (fixed frame
+`map`). Terminal commands while drawing: `Enter` closes the polygon, `u` undoes the
+last point, `r` resets, `a` aborts.
+```bash
+ros2 run fbot_world room_writer
+```
+
 ## fbot_world message and services
 ### Services
 
 | Service | Type | Description |
 |---------|------|-------------|
 | `/fbot_world/get_pose` | [`GetPose`](fbot_world_msgs/srv/GetPose.srv) | Service callback to return the pose and size for a requested target key |
 | `/fbot_world/get_set` | [`GetPoseFromSet`](fbot_world_msgs/srv/GetPoseFromSet.srv) | Service callback to return all poses for a requested group name key |
-| `/fbot_world/get_groups_names` | [`GetSets`](fbot_world_msgs/srv/GetSets.srv) | Service callback to return all gorup names in redis |
+| `/fbot_world/get_groups_names` | [`GetSets`](fbot_world_msgs/srv/GetSets.srv) | Service callback to return all gorup names in poses and rooms names with postions and objetcs in yaml file |
+| `/fbot_world/get_room ` | [`GetRoom`](fbot_world_msgs/srv/GetRoom.srv) | Service that returns the room and place name where the robot is, based on its current position |
 
 ---
 

fbot_world/config/pick_place_poses.yaml

@@ -1,67 +1,43 @@
-targets:
-  pose_to_table:
-    px: 1.8908765980702646
-    py: 0.6513262188135082
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: 0.5395731784028819
-    ow: 0.8419387062893663
-  pose_to_bin:
-    px: 0.8479466644181386
-    py: 1.2495991793775092
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: 0.9336292037713236
-    ow: 0.35824085454526855
-  trash_can:
-    px: 0.8479466644181386
-    py: 1.2495991793775092
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: 0.9336292037713236
-    ow: 0.35824085454526855
-  pose_to_dishwasher:
-    px: 0.8479466644181386
-    py: 1.2495991793775092
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: 0.9336292037713236
-    ow: 0.35824085454526855
-  pose_to_extra_surface:
-    px: 1.9357591129488454
-    py: -0.4670448277388026
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: -0.07527829568508634
-    ow: 0.9971625635766461
-  pose_to_bookshelf:
-    px: 0.9926801349370632
-    py: 1.205041351560584
-    pz: 0.0
-    ox: 0.0
-    oy: 0.0
-    oz: 0.9371329679623501
-    ow: 0.34897249226561805
-sitable_positions: {}
-place_poses:
-  release_shelf:
-    px: 0.5458867046821676
-    py: 1.7653715414771918
-    pz: 0.9422977020036839
-    ox: 0.020981909016772323
-    oy: -0.01638402118804777
-    oz: 0.895690099877355
-    ow: 0.4438812547578634
-  release_shelf2:
-    px: 0.3881321562760789
-    py: 1.6348179065442459
-    pz: 0.9388022924512043
-    ox: 0.006535387588964672
-    oy: -0.008724145782341656
-    oz: 0.9707564537803911
-    ow: 0.23981885962774033
+poses:
+  targets:
+    pose_to_bookshelf:
+      px: 0.4091328818576528
+      py: -1.0287915545575543
+      pz: 0.0
+      ox: 0.0
+      oy: 0.0
+      oz: -0.5385353158860683
+      ow: 0.8426029394344009
+    pose_to_extra_surface:
+      px: 0.3358618622103488
+      py: 0.7024107082398413
+      pz: 0.0
+      ox: 0.0
+      oy: 0.0
+      oz: 0.9384906041620402
+      ow: 0.34530477248304686
+    table:
+      px: 0.7808942924102151
+      py: 1.1375189681681164
+      pz: 0.0
+      ox: 0.0
+      oy: 0.0
+      oz: 0.9157941762074888
+      ow: 0.4016478891074208
+    trashbin:
+      px: 0.8161346185167272
+      py: 0.8802096806568238
+      pz: 0.0
+      ox: 0.0
+      oy: 0.0
+      oz: 0.4166555141212606
+      ow: 0.9090644545643328
+    dishwasher:
+      px: 1.4116466924180076
+      py: 0.31998485172349883
+      pz: 0.0
+      ox: 0.0
+      oy: 0.0
+      oz: 0.44009550909906053
+      ow: 0.8979509690795142
+  sitable_positions: {}