diff --git a/README.md b/README.md index 35fd43c..ca68278 100644 --- a/README.md +++ b/README.md @@ -76,17 +76,58 @@ 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 @@ -94,7 +135,8 @@ ros2 run fbot_world pose_writer |---------|------|-------------| | `/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 | --- diff --git a/cyclonedds.log b/cyclonedds.log deleted file mode 100644 index 582c619..0000000 --- a/cyclonedds.log +++ /dev/null @@ -1,163 +0,0 @@ -1780531508.718687 [77] pose: config: Domain/General/MulticastRecvNetworkInterfaceAddresses/#text: preferred {} -1780531508.718714 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@autodetermine]: false {} -1780531508.718720 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@name]: enp86s0 {0} -1780531508.718724 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@address]: {} -1780531508.718728 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@priority]: default {} -1780531508.718732 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@prefer_multicast]: false {} -1780531508.718736 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@presence_required]: true {} -1780531508.718740 [77] pose: config: Domain/General/Interfaces/NetworkInterface[@multicast]: default {} -1780531508.718744 [77] pose: config: Domain/General/ExternalNetworkAddress/#text: auto {} -1780531508.718747 [77] pose: config: Domain/General/ExternalNetworkMask/#text: 0.0.0.0 {} -1780531508.718751 [77] pose: config: Domain/General/AllowMulticast/#text: false {0} -1780531508.718755 [77] pose: config: Domain/General/MulticastTimeToLive/#text: 32 {} -1780531508.718759 [77] pose: config: Domain/General/DontRoute/#text: false {} -1780531508.718762 [77] pose: config: Domain/General/Transport/#text: udp {} -1780531508.718766 [77] pose: config: Domain/General/EnableMulticastLoopback/#text: true {} -1780531508.718770 [77] pose: config: Domain/General/MaxMessageSize/#text: 14720 B {} -1780531508.718774 [77] pose: config: Domain/General/MaxRexmitMessageSize/#text: 1456 B {} -1780531508.718778 [77] pose: config: Domain/General/FragmentSize/#text: 1344 B {} -1780531508.718781 [77] pose: config: Domain/General/RedundantNetworking/#text: false {} -1780531508.718785 [77] pose: config: Domain/General/EntityAutoNaming/#text: empty {} -1780531508.718790 [77] pose: config: Domain/General/EntityAutoNaming[@seed]: 4903834 2499991318 2585752349 2329953503 3335176768 3456270440 3510467001 3473618066 {} -1780531508.718795 [77] pose: config: Domain/Sizing/ReceiveBufferSize/#text: 1 MiB {} -1780531508.718799 [77] pose: config: Domain/Sizing/ReceiveBufferChunkSize/#text: 128 KiB {} -1780531508.718803 [77] pose: config: Domain/Compatibility/StandardsConformance/#text: lax {} -1780531508.718806 [77] pose: config: Domain/Compatibility/ExplicitlyPublishQosSetToDefault/#text: false {} -1780531508.718810 [77] pose: config: Domain/Compatibility/ManySocketsMode/#text: single {} -1780531508.718814 [77] pose: config: Domain/Compatibility/AssumeRtiHasPmdEndpoints/#text: false {} -1780531508.718818 [77] pose: config: Domain/Discovery/Tag/#text: {} -1780531508.718821 [77] pose: config: Domain/Discovery/ExternalDomainId/#text: 77 {} -1780531508.718825 [77] pose: config: Domain/Discovery/DSGracePeriod/#text: 30 s {} -1780531508.718830 [77] pose: config: Domain/Discovery/Peers/Peer[@Address]: 192.168.4.100 {0} -1780531508.718834 [77] pose: config: Domain/Discovery/Peers/Peer[@Address]: 192.168.4.102 {0} -1780531508.718838 [77] pose: config: Domain/Discovery/ParticipantIndex/#text: auto {0} -1780531508.718842 [77] pose: config: Domain/Discovery/MaxAutoParticipantIndex/#text: 120 {0} -1780531508.718845 [77] pose: config: Domain/Discovery/SPDPMulticastAddress/#text: 239.255.0.1 {} -1780531508.718849 [77] pose: config: Domain/Discovery/SPDPInterval/#text: 30 s {} -1780531508.718853 [77] pose: config: Domain/Discovery/DefaultMulticastAddress/#text: auto {} -1780531508.718857 [77] pose: config: Domain/Discovery/Ports/Base/#text: 7400 {} -1780531508.718861 [77] pose: config: Domain/Discovery/Ports/DomainGain/#text: 250 {} -1780531508.718865 [77] pose: config: Domain/Discovery/Ports/ParticipantGain/#text: 2 {} -1780531508.718869 [77] pose: config: Domain/Discovery/Ports/MulticastMetaOffset/#text: 0 {} -1780531508.718873 [77] pose: config: Domain/Discovery/Ports/UnicastMetaOffset/#text: 10 {} -1780531508.718881 [77] pose: config: Domain/Discovery/Ports/MulticastDataOffset/#text: 1 {} -1780531508.718885 [77] pose: config: Domain/Discovery/Ports/UnicastDataOffset/#text: 11 {} -1780531508.718889 [77] pose: config: Domain/Discovery/EnableTopicDiscoveryEndpoints/#text: false {} -1780531508.718893 [77] pose: config: Domain/Discovery/LeaseDuration/#text: 10 s {} -1780531508.718902 [77] pose: config: Domain/Tracing/Category/#text: fatal,error,warning,info,config {0} -1780531508.718906 [77] pose: config: Domain/Tracing/OutputFile/#text: cyclonedds.log {} -1780531508.718910 [77] pose: config: Domain/Tracing/AppendToFile/#text: false {} -1780531508.718913 [77] pose: config: Domain/Tracing/PacketCaptureFile/#text: {} -1780531508.718917 [77] pose: config: Domain/Internal/DeliveryQueueMaxSamples/#text: 256 {} -1780531508.718921 [77] pose: config: Domain/Internal/PrimaryReorderMaxSamples/#text: 128 {} -1780531508.718924 [77] pose: config: Domain/Internal/SecondaryReorderMaxSamples/#text: 128 {} -1780531508.718928 [77] pose: config: Domain/Internal/DefragUnreliableMaxSamples/#text: 4 {} -1780531508.718932 [77] pose: config: Domain/Internal/DefragReliableMaxSamples/#text: 16 {} -1780531508.718935 [77] pose: config: Domain/Internal/BuiltinEndpointSet/#text: writers {} -1780531508.718939 [77] pose: config: Domain/Internal/MeasureHbToAckLatency/#text: false {} -1780531508.718943 [77] pose: config: Domain/Internal/UnicastResponseToSPDPMessages/#text: true {} -1780531508.718946 [77] pose: config: Domain/Internal/SynchronousDeliveryPriorityThreshold/#text: 0 {} -1780531508.718950 [77] pose: config: Domain/Internal/SynchronousDeliveryLatencyBound/#text: inf {} -1780531508.718954 [77] pose: config: Domain/Internal/MaxParticipants/#text: 0 {} -1780531508.718958 [77] pose: config: Domain/Internal/AccelerateRexmitBlockSize/#text: 0 {} -1780531508.718961 [77] pose: config: Domain/Internal/RetransmitMerging/#text: never {} -1780531508.718965 [77] pose: config: Domain/Internal/RetransmitMergingPeriod/#text: 5 ms {} -1780531508.718969 [77] pose: config: Domain/Internal/HeartbeatInterval/#text: 100 ms {} -1780531508.718973 [77] pose: config: Domain/Internal/HeartbeatInterval[@min]: 5 ms {} -1780531508.718977 [77] pose: config: Domain/Internal/HeartbeatInterval[@minsched]: 20 ms {} -1780531508.718980 [77] pose: config: Domain/Internal/HeartbeatInterval[@max]: 8 s {} -1780531508.718984 [77] pose: config: Domain/Internal/MaxQueuedRexmitBytes/#text: 512 KiB {} -1780531508.718988 [77] pose: config: Domain/Internal/MaxQueuedRexmitMessages/#text: 200 {} -1780531508.718991 [77] pose: config: Domain/Internal/WriterLingerDuration/#text: 1 s {} -1780531508.718996 [77] pose: config: Domain/Internal/SocketReceiveBufferSize[@min]: 10 MiB {0} -1780531508.718999 [77] pose: config: Domain/Internal/SocketReceiveBufferSize[@max]: default {} -1780531508.719003 [77] pose: config: Domain/Internal/SocketSendBufferSize[@min]: 64 KiB {} -1780531508.719007 [77] pose: config: Domain/Internal/SocketSendBufferSize[@max]: default {} -1780531508.719011 [77] pose: config: Domain/Internal/NackDelay/#text: 100 ms {} -1780531508.719014 [77] pose: config: Domain/Internal/AckDelay/#text: 10 ms {} -1780531508.719018 [77] pose: config: Domain/Internal/AutoReschedNackDelay/#text: 3 s {} -1780531508.719022 [77] pose: config: Domain/Internal/PreEmptiveAckDelay/#text: 10 ms {} -1780531508.719025 [77] pose: config: Domain/Internal/ScheduleTimeRounding/#text: 0 s {} -1780531508.719029 [77] pose: config: Domain/Internal/DDSI2DirectMaxThreads/#text: 1 {} -1780531508.719032 [77] pose: config: Domain/Internal/SquashParticipants/#text: false {} -1780531508.719036 [77] pose: config: Domain/Internal/SPDPResponseMaxDelay/#text: 0 s {} -1780531508.719039 [77] pose: config: Domain/Internal/LateAckMode/#text: false {} -1780531508.719043 [77] pose: config: Domain/Internal/RetryOnRejectBestEffort/#text: false {} -1780531508.719050 [77] pose: config: Domain/Internal/GenerateKeyhash/#text: false {} -1780531508.719053 [77] pose: config: Domain/Internal/MaxSampleSize/#text: 2147483647 B {} -1780531508.719057 [77] pose: config: Domain/Internal/LivelinessMonitoring/#text: false {} -1780531508.719061 [77] pose: config: Domain/Internal/LivelinessMonitoring[@StackTraces]: true {} -1780531508.719065 [77] pose: config: Domain/Internal/LivelinessMonitoring[@Interval]: 1 s {} -1780531508.719068 [77] pose: config: Domain/Internal/MonitorPort/#text: -1 {} -1780531508.719072 [77] pose: config: Domain/Internal/PrioritizeRetransmit/#text: true {} -1780531508.719076 [77] pose: config: Domain/Internal/UseMulticastIfMreqn/#text: 0 {} -1780531508.719079 [77] pose: config: Domain/Internal/RediscoveryBlacklistDuration/#text: 0 s {} -1780531508.719083 [77] pose: config: Domain/Internal/RediscoveryBlacklistDuration[@enforce]: false {} -1780531508.719087 [77] pose: config: Domain/Internal/MultipleReceiveThreads/#text: default {} -1780531508.719091 [77] pose: config: Domain/Internal/MultipleReceiveThreads[@maxretries]: 4294967295 {} -1780531508.719095 [77] pose: config: Domain/Internal/Test/XmitLossiness/#text: 0 {} -1780531508.719099 [77] pose: config: Domain/Internal/Watermarks/WhcLow/#text: 1 KiB {} -1780531508.719103 [77] pose: config: Domain/Internal/Watermarks/WhcHigh/#text: 500 KiB {0} -1780531508.719107 [77] pose: config: Domain/Internal/Watermarks/WhcHighInit/#text: 30 KiB {} -1780531508.719111 [77] pose: config: Domain/Internal/Watermarks/WhcAdaptive/#text: true {} -1780531508.719115 [77] pose: config: Domain/Internal/BurstSize/MaxRexmit/#text: 1 MiB {} -1780531508.719119 [77] pose: config: Domain/Internal/BurstSize/MaxInitTransmit/#text: 4294967295 {} -1780531508.719123 [77] pose: config: Domain/Internal/EnableExpensiveChecks/#text: {} -1780531508.719127 [77] pose: config: Domain/TCP/NoDelay/#text: true {} -1780531508.719130 [77] pose: config: Domain/TCP/Port/#text: -1 {} -1780531508.719134 [77] pose: config: Domain/TCP/ReadTimeout/#text: 2 s {} -1780531508.719138 [77] pose: config: Domain/TCP/WriteTimeout/#text: 2 s {} -1780531508.719142 [77] pose: config: Domain/TCP/AlwaysUsePeeraddrForUnicast/#text: false {} -1780531508.719145 [77] pose: config: Domain/SSL/Enable/#text: false {} -1780531508.719149 [77] pose: config: Domain/SSL/CertificateVerification/#text: true {} -1780531508.719153 [77] pose: config: Domain/SSL/VerifyClient/#text: true {} -1780531508.719156 [77] pose: config: Domain/SSL/SelfSignedCertificates/#text: false {} -1780531508.719160 [77] pose: config: Domain/SSL/KeystoreFile/#text: keystore {} -1780531508.719164 [77] pose: config: Domain/SSL/KeyPassphrase/#text: secret {} -1780531508.719167 [77] pose: config: Domain/SSL/Ciphers/#text: ALL:!ADH:!LOW:!EXP:!MD5:@STRENGTH {} -1780531508.719171 [77] pose: config: Domain/SSL/EntropyFile/#text: {} -1780531508.719175 [77] pose: config: Domain/SSL/MinimumTLSVersion/#text: 1.3 {} -1780531508.719179 [77] pose: config: Domain/SharedMemory/Enable/#text: false {} -1780531508.719182 [77] pose: config: Domain/SharedMemory/Locator/#text: {} -1780531508.719186 [77] pose: config: Domain/SharedMemory/Prefix/#text: DDS_CYCLONE {} -1780531508.719190 [77] pose: config: Domain/SharedMemory/LogLevel/#text: info {} -1780531508.719193 [77] pose: config: Domain[@Id]: 77 {} -1780531508.719218 [77] pose: started at 1780531508.06719200 -- 2026-06-03 21:05:08-03:00 -1780531508.719222 [77] pose: udp initialized -1780531508.720198 [77] pose: interfaces: lo udp/127.0.0.1(q1) enp86s0 udp/192.168.4.100(q9) wlo1 wireless udp/192.168.1.143(q9) -1780531508.720212 [77] pose: selected interfaces: enp86s0 (index 2 priority 0) -1780531508.720229 [77] pose: ownip: udp/192.168.4.100 -1780531508.720231 [77] pose: extmask: invalid/0 (not applicable) -1780531508.720258 [77] pose: SPDP MC: udp/239.255.0.1 -1780531508.720260 [77] pose: default MC: udp/239.255.0.1 -1780531508.720261 [77] pose: SSM support included -1780531508.720365 [77] pose: rtps_init: trying to find a free participant index -1780531508.720391 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720409 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720415 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720421 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720428 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720433 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720439 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720446 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720453 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720459 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720466 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720473 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720480 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720487 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720494 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720500 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720506 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720512 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720519 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720526 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720533 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720544 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720548 [77] pose: rtps_init: uc ports: disc 26700 data 26701 -1780531508.720551 [77] pose: rtps_init: domainid 77 participantid 21 -1780531508.720554 [77] pose: Unicast Ports: discovery 26700 data 26701 -1780531508.720560 [77] pose: socket receive buffer size set to 20971520 bytes -1780531508.720565 [77] pose: interface enp86s0: transmit port 48507 -1780531508.720639 [77] pose: add_peer_addresses: add udp/192.168.4.100:26660, :26662, :26664, :26666, :26668, :26670, :26672, :26674, :26676, :26678, :26680, :26682, :26684, :26686, :26688, :26690, :26692, :26694, :26696, :26698, :26700, :26702, :26704, :26706, :26708, :26710, :26712, :26714, :26716, :26718, :26720, :26722, :26724, :26726, :26728, :26730, :26732, :26734, :26736, :26738, :26740, :26742, :26744, :26746, :26748, :26750, :26752, :26754, :26756, :26758, :26760, :26762, :26764, :26766, :26768, :26770, :26772, :26774, :26776, :26778, :26780, :26782, :26784, :26786, :26788, :26790, :26792, :26794, :26796, :26798, :26800, :26802, :26804, :26806, :26808, :26810, :26812, :26814, :26816, :26818, :26820, :26822, :26824, :26826, :26828, :26830, :26832, :26834, :26836, :26838, :26840, :26842, :26844, :26846, :26848, :26850, :26852, :26854, :26856, :26858, :26860, :26862, :26864, :26866, :26868, :26870, :26872, :26874, :26876, :26878, :26880, :26882, :26884, :26886, :26888, :26890, :26892, :26894, :26896, :26898 -1780531508.720703 [77] pose: add_peer_addresses: add udp/192.168.4.102:26660, :26662, :26664, :26666, :26668, :26670, :26672, :26674, :26676, :26678, :26680, :26682, :26684, :26686, :26688, :26690, :26692, :26694, :26696, :26698, :26700, :26702, :26704, :26706, :26708, :26710, :26712, :26714, :26716, :26718, :26720, :26722, :26724, :26726, :26728, :26730, :26732, :26734, :26736, :26738, :26740, :26742, :26744, :26746, :26748, :26750, :26752, :26754, :26756, :26758, :26760, :26762, :26764, :26766, :26768, :26770, :26772, :26774, :26776, :26778, :26780, :26782, :26784, :26786, :26788, :26790, :26792, :26794, :26796, :26798, :26800, :26802, :26804, :26806, :26808, :26810, :26812, :26814, :26816, :26818, :26820, :26822, :26824, :26826, :26828, :26830, :26832, :26834, :26836, :26838, :26840, :26842, :26844, :26846, :26848, :26850, :26852, :26854, :26856, :26858, :26860, :26862, :26864, :26866, :26868, :26870, :26872, :26874, :26876, :26878, :26880, :26882, :26884, :26886, :26888, :26890, :26892, :26894, :26896, :26898 diff --git a/fbot_world/config/pick_place_poses.yaml b/fbot_world/config/pick_place_poses.yaml index 78ef015..ce13ac8 100644 --- a/fbot_world/config/pick_place_poses.yaml +++ b/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: {} diff --git a/fbot_world/config/pose.yaml b/fbot_world/config/pose.yaml index 2430a6e..88c4368 100644 --- a/fbot_world/config/pose.yaml +++ b/fbot_world/config/pose.yaml @@ -1,58 +1,164 @@ -targets: +poses: + targets: + living_room_look: + px: -11.205980123210482 + py: -5.971964121576364 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2535824278252746 + ow: 0.967313781716274 + living_room_trash: + px: -11.205980123210482 + py: -5.971964121576364 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2535824278252746 + ow: 0.967313781716274 + living_room_sofa: + px: -11.205980123210482 + py: -5.971964121576364 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2535824278252746 + ow: 0.967313781716274 + living_room: + px: -11.205980123210482 + py: -5.971964121576364 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2535824278252746 + ow: 0.967313781716274 + living_room_unknown: + px: 1.0 + py: 4.0 + pz: 2.0 + ox: 0.0 + oy: 0.0 + oz: 0.0 + ow: 1.0 + front_shelf: + px: 0.05594035566769722 + py: -0.2715101941059138 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.6915897116323705 + ow: 0.7222905722520921 + front_table: + px: 0.42792447473199186 + py: -1.347995928088804 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.698200065871624 + ow: 0.715902694517111 + sitable_positions: + living_room: + px: -11.205980123210482 + py: -5.971964121576364 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2535824278252746 + ow: 0.967313781716274 + living_room_unknown: + px: 1.0 + py: 4.0 + pz: 2.0 + ox: 0.0 + oy: 0.0 + oz: 0.0 + ow: 1.0 + front_shelf: + px: 0.05594035566769722 + py: -0.2715101941059138 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.6915897116323705 + ow: 0.7222905722520921 + front_table: + px: 0.42792447473199186 + py: -1.347995928088804 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.698200065871624 + ow: 0.715902694517111 +rooms: living_room: - px: -11.205980123210482 - py: -5.971964121576364 - pz: 0.0 - ox: 0.0 - oy: 0.0 - oz: -0.2535824278252746 - ow: 0.967313781716274 - living_room_unknown: - px: 1.0 - py: 4.0 - pz: 2.0 - ox: 0.0 - oy: 0.0 - oz: 0.0 - ow: 1.0 - pre_bookshelf: - px: -0.012418013375762491 - py: -0.07010421849654724 - pz: 0.0 - ox: 0.0 - oy: -0.0 - oz: -0.9992332087105509 - ow: -0.03915347506948325 - bookshelf: - px: -0.5380677902802506 - py: 0.03229247423360263 - pz: 0.0 - ox: 0.0 - oy: -0.0 - oz: -0.9885710261073397 - ow: -0.15075585010566397 - - pose_1: - px: -0.42173448932023855 - py: -2.094097409751827 - pz: 0.0 - ox: 0.0 - oy: 0.0 - oz: -0.9447629563891653 - ow: 0.32775441451611925 - pose_2: - px: 0.025064140482105774 - py: -0.34700893662234433 - pz: 0.0 - ox: 0.0 - oy: 0.0 - oz: -0.459055606752703 - ow: 0.8884075359365812 - go_to_out: - px: 1.3771494422451693 - py: -2.1908367507108895 - pz: 0.0 - ox: 0.0 - oy: 0.0 - oz: -0.3864405234913837 - ow: 0.9223143291761792 + poses: + - living_room_look + - living_room_trash + - living_room_sofa + vertices: + - - -1.0 + - -1.0 + - - -1.0 + - -0.5 + - - -0.5 + - -0.5 + - - -0.5 + - 0.5 + - - -1.0 + - 0.5 + - - -1.0 + - 1.0 + - - 1.0 + - 1.0 + - - 1.0 + - -1.0 + objects: + desk: + - - 10.0 + - 10.0 + - - 10.0 + - 10.0 + - - 10.0 + - 10.0 + - - 10.0 + - 10.0 + bedroom: + vertices: + - - 0.0 + - 0.0 + - - 1.0 + - 0.0 + - - 1.0 + - 1.0 + - - 0.0 + - 1.0 + objects: + bed: + - - 0.0 + - 0.0 + - - 1.0 + - 0.0 + - - 1.0 + - 1.0 + - - 0.0 + - 1.0 + kitchen: + vertices: + - - 0.0 + - 0.0 + - - 1.0 + - 0.0 + - - 1.0 + - 1.0 + - - 0.0 + - 1.0 + objects: + sink: + - - 0.0 + - 0.0 + - - 1.0 + - 0.0 + - - 1.0 + - 1.0 + - - 0.0 + - 1.0 diff --git a/fbot_world/config/pose_finals.yaml b/fbot_world/config/pose_finals.yaml new file mode 100644 index 0000000..9aad006 --- /dev/null +++ b/fbot_world/config/pose_finals.yaml @@ -0,0 +1,144 @@ +poses: + targets: + start_point: + px: 0.3310309730979384 + py: 0.03710571106057868 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.9473075215086629 + ow: 0.3203255526696461 + kitchen: + px: 0.4026570604285509 + py: -0.010518256442122008 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.5802666509704748 + ow: 0.8144265551733374 + kitchen_table: + px: 0.4404410314440263 + py: 0.425548740027738 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.7487688384380485 + ow: 0.6628312202847235 + living_room: + px: 0.686225165952682 + py: -0.10549187582943563 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.2805807135155283 + ow: 0.9598304346097372 + sofa: + px: 1.4109975257230924 + py: -0.5038269536915472 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.05113795577101034 + ow: 0.9986915987829087 + tv_stand: + px: 0.903956788309435 + py: -0.4862363852372649 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.992729565053208 + ow: 0.12036615250670947 + shelf: + px: 0.536181958828879 + py: -0.6314041929230471 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: -0.5830223021630958 + ow: 0.8124561496969814 + trashbin: + px: 0.8647341700912705 + py: 0.7391778887298036 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.4769264244384737 + ow: 0.8789432209604514 + refrigerator: + px: 0.9484175821890339 + py: 0.3019952617640565 + pz: 0.0 + ox: 0.0 + oy: 0.0 + oz: 0.23361556807011555 + ow: 0.9723290422255613 + sitable_positions: {} +rooms: + living_room: + poses: + - living_room + - sofa + - shelf + - tv_stand + vertices: + - - -0.68 + - -0.66 + - - -0.01 + - -1.93 + - - 3.17 + - -1.93 + - - 3.18 + - 1.26 + objects: + tv_stand: + - - -0.68 + - -0.66 + - - -0.01 + - -1.93 + - - 0.69 + - -1.92 + - - -0.27 + - -0.43 + shelf: + - - 0.69 + - -1.92 + - - 0.49 + - -1.44 + - - 1.52 + - -1.11 + - - 1.96 + - -1.94 + kitchen: + poses: + - kitchen + - kitchen_table + - refrigerator + - trashbin + vertices: + - - 2.65 + - 1.01 + - - 0.59 + - 2.8 + - - -0.66 + - 1.3 + - - 1.28 + - -0.41 + objects: + kitchen_table: + - - 0.59 + - 2.8 + - - -0.66 + - 1.3 + - - 0.06 + - 0.76 + - - 0.98 + - 2.11 + trashbin: + - - 1.3 + - 1.13 + - - 0.93 + - 1.48 + - - 1.31 + - 1.88 + - - 1.65 + - 1.49 diff --git a/fbot_world/fbot_world/marker_utils.py b/fbot_world/fbot_world/marker_utils.py new file mode 100644 index 0000000..df9934b --- /dev/null +++ b/fbot_world/fbot_world/marker_utils.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +""" +Shared RViz marker builders for fbot_world. + +Used by both the runtime pose plugin (pose.py) and the interactive room +annotator (room_writer.py), so the live annotation preview is drawn identically +to the markers published from the saved YAML. +""" + +import numpy as np + +from builtin_interfaces.msg import Duration +from geometry_msgs.msg import Point +from std_msgs.msg import ColorRGBA +from visualization_msgs.msg import Marker + + +def make_color(rgba: tuple) -> ColorRGBA: + '''@brief Build a ColorRGBA from an (r, g, b, a) tuple.''' + c = ColorRGBA() + c.r, c.g, c.b, c.a = (float(v) for v in rgba) + return c + + +def lifetime_forever() -> Duration: + '''@brief Duration(sec=0, nanosec=0) means the marker lives forever.''' + return Duration(sec=0, nanosec=0) + + +def centroid(polygon: list) -> tuple: + '''@brief Return the (x, y) centroid of a polygon given as [[x, y], ...].''' + arr = np.array(polygon, dtype=np.float32) + return float(arr[:, 0].mean()), float(arr[:, 1].mean()) + + +def polygon_line_strip( + polygon: list, + marker_id: int, + ns: str, + color: tuple, + z: float = 0.0, + line_width: float = 0.05, + frame: str = 'map', + closed: bool = True, +) -> Marker: + ''' + @brief Creates a LINE_STRIP marker that traces a polygon. + @param closed: if True, repeat the first vertex to close the loop. + ''' + m = Marker() + m.header.frame_id = frame + m.ns = ns + m.id = marker_id + m.type = Marker.LINE_STRIP + m.action = Marker.ADD + m.scale.x = line_width + m.color = make_color(color) + m.lifetime = lifetime_forever() + m.pose.orientation.w = 1.0 + + for v in polygon: + p = Point() + p.x = float(v[0]) + p.y = float(v[1]) + p.z = z + m.points.append(p) + + if closed and polygon: + p = Point() + p.x = float(polygon[0][0]) + p.y = float(polygon[0][1]) + p.z = z + m.points.append(p) + + return m + + +def polygon_fill( + polygon: list, + marker_id: int, + ns: str, + color: tuple, + z: float = 0.0, + frame: str = 'map', +) -> Marker: + ''' + @brief Creates a TRIANGLE_LIST marker that fills a polygon via a centroid fan. + Works well for convex/mildly concave shapes. + ''' + m = Marker() + m.header.frame_id = frame + m.ns = ns + '_fill' + m.id = marker_id + m.type = Marker.TRIANGLE_LIST + m.action = Marker.ADD + m.scale.x = 1.0 + m.scale.y = 1.0 + m.scale.z = 1.0 + # Make fill slightly more transparent than the outline. + fill_color = (color[0], color[1], color[2], color[3] * 0.6) + m.color = make_color(fill_color) + m.lifetime = lifetime_forever() + m.pose.orientation.w = 1.0 + + arr = np.array(polygon, dtype=np.float32) + cx, cy = arr[:, 0].mean(), arr[:, 1].mean() + n = len(arr) + for i in range(n): + j = (i + 1) % n + for vx, vy in [(cx, cy), (arr[i][0], arr[i][1]), (arr[j][0], arr[j][1])]: + p = Point() + p.x = float(vx) + p.y = float(vy) + p.z = z + m.points.append(p) + + return m + + +def text_marker( + text: str, + position: tuple, + marker_id: int, + ns: str, + z: float = 0.3, + text_size: float = 0.25, + frame: str = 'map', +) -> Marker: + ''' + @brief Creates a TEXT_VIEW_FACING marker at a given position. Always white for + readability. + ''' + m = Marker() + m.header.frame_id = frame + m.ns = ns + '_text' + m.id = marker_id + m.type = Marker.TEXT_VIEW_FACING + m.action = Marker.ADD + m.pose.position.x = float(position[0]) + m.pose.position.y = float(position[1]) + m.pose.position.z = z + m.pose.orientation.w = 1.0 + m.scale.z = text_size + m.color = make_color((1.0, 1.0, 1.0, 1.0)) + m.lifetime = lifetime_forever() + m.text = text + return m + + +def point_spheres( + points: list, + marker_id: int, + ns: str, + color: tuple, + scale: float = 0.12, + z: float = 0.05, + frame: str = 'map', +) -> Marker: + ''' + @brief Creates a SPHERE_LIST marker, one sphere per point. Used to show the + individual vertices clicked while a polygon is being drawn. + ''' + m = Marker() + m.header.frame_id = frame + m.ns = ns + '_points' + m.id = marker_id + m.type = Marker.SPHERE_LIST + m.action = Marker.ADD + m.scale.x = scale + m.scale.y = scale + m.scale.z = scale + m.color = make_color(color) + m.lifetime = lifetime_forever() + m.pose.orientation.w = 1.0 + for v in points: + p = Point() + p.x = float(v[0]) + p.y = float(v[1]) + p.z = z + m.points.append(p) + return m + + +def delete_all_marker(frame: str = 'map') -> Marker: + '''@brief A DELETEALL marker; prepend to a MarkerArray to clear stale markers.''' + m = Marker() + m.header.frame_id = frame + m.action = Marker.DELETEALL + return m diff --git a/fbot_world/fbot_world/place_pose_writer.py b/fbot_world/fbot_world/place_pose_writer.py index 5285c57..21a0e95 100644 --- a/fbot_world/fbot_world/place_pose_writer.py +++ b/fbot_world/fbot_world/place_pose_writer.py @@ -5,7 +5,6 @@ import threading import rclpy -import yaml from collections import OrderedDict from geometry_msgs.msg import Pose @@ -18,6 +17,8 @@ from interbotix_xs_msgs.srv import TorqueEnable +from fbot_world import yaml_io + ''' Save the current end-effector pose (read from TF, expressed in the map frame) to a yaml file, so fixed "place" locations such as shelves can be authored by moving the @@ -32,31 +33,6 @@ ''' -class OrderedDumper(yaml.SafeDumper): - ''' - @brief Custom YAML dumper to handle OrderedDict. - ''' - def represent_ordereddict(self, data): - return self.represent_dict(data.items()) - - -class OrderedLoader(yaml.SafeLoader): - pass - - -def construct_ordered_dict(loader, node): - ''' - @brief Custom YAML loader to handle OrderedDict. - ''' - return OrderedDict(loader.construct_pairs(node)) - - -OrderedLoader.add_constructor( - yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, - construct_ordered_dict -) - - class PlacePoseWriter(Node): ''' @class PlacePoseWriter @@ -254,27 +230,18 @@ def write_to_yaml(self): @brief Write the collected poses to a YAML file, appending to the group if it already exists. ''' - OrderedDumper.add_representer(OrderedDict, OrderedDumper.represent_ordereddict) - if os.path.exists(self.yaml_path): self.get_logger().info(f"{self.yaml_file} already exists. The new poses will be appended to the existing data.") - - with open(self.yaml_path, 'r') as yaml_file: - try: - existing_data = yaml.load(yaml_file, Loader=OrderedLoader) or OrderedDict() - except yaml.YAMLError as e: - self.get_logger().error(f"Error reading {self.yaml_file}: {e}") - existing_data = OrderedDict() else: self.get_logger().info(f"{self.yaml_file} does not exist. Creating a new file.") - existing_data = OrderedDict() + + existing_data = yaml_io.load_ordered(self.yaml_path) if self.group_set not in existing_data: existing_data[self.group_set] = OrderedDict() existing_data[self.group_set].update(self.poses[self.group_set]) - with open(self.yaml_path, 'w') as yaml_file: - yaml.dump(existing_data, yaml_file, default_flow_style=False, Dumper=OrderedDumper) + yaml_io.dump_ordered(existing_data, self.yaml_path) return diff --git a/fbot_world/fbot_world/pose.py b/fbot_world/fbot_world/pose.py index 758c570..401b8b1 100755 --- a/fbot_world/fbot_world/pose.py +++ b/fbot_world/fbot_world/pose.py @@ -3,12 +3,24 @@ import rclpy import yaml import os - -from world_scripts.world_plugin import WorldPlugin -from fbot_world_msgs.msg import FBOTPoses -from fbot_world_msgs.srv import GetPose, GetPoseFromSet, GetSets -from geometry_msgs.msg import Pose, Vector3 +import numpy as np +from scripts.world_plugin import WorldPlugin +from fbot_world import marker_utils +from fbot_world_msgs.msg import FBOTPoses, FBOTRooms, DBPose +from fbot_world_msgs.srv import GetPose, GetPoseFromSet, GetSets, GetRoom +from geometry_msgs.msg import Pose, Vector3, Point from ament_index_python.packages import get_package_share_directory +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import ColorRGBA +from builtin_interfaces.msg import Duration +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy + + +qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, +) def readYamlFile(file_path: str = None): """ @@ -29,6 +41,23 @@ class PosePlugin(WorldPlugin): based on configuration parameters or Redis database entries. """ + # Predefined colors for rooms (RGBA, 0.0–1.0) + ROOM_COLORS = [ + (0.2, 0.6, 1.0, 0.35), # blue + (0.2, 1.0, 0.4, 0.35), # green + (1.0, 0.6, 0.1, 0.35), # orange + (0.9, 0.2, 0.9, 0.35), # magenta + (0.1, 0.9, 0.9, 0.35), # cyan + (1.0, 1.0, 0.1, 0.35), # yellow + ] + + OBJECT_COLORS = [ + (1.0, 0.2, 0.2, 0.45), # red + (1.0, 0.8, 0.2, 0.45), # amber + (0.6, 0.2, 1.0, 0.45), # purple + (0.2, 1.0, 0.8, 0.45), # teal + ] + def __init__(self, node_name: str = 'pose'): """ @brief: Constructor for PosePlugin. @@ -39,15 +68,185 @@ def __init__(self, node_name: str = 'pose'): self.readParameters() ws_dir = os.path.abspath(os.path.join(get_package_share_directory('fbot_world'), '../../../..')) self.file_path = os.path.join(ws_dir, "src", "fbot_world","fbot_world", "config", self.config_file_name + '.yaml') - self.targets = readYamlFile(self.file_path) + self.loadTargets() self.get_logger().info(f"File name: {self.config_file_name}") self.setStaticPose() + + # --- Marker publisher --- + self.marker_pub = self.create_publisher(MarkerArray, '/fbot_world/debug_markers', qos) self.pose_server = self.create_service(GetPose, '/fbot_world/get_pose', self.getPose) self.set_server = self.create_service(GetPoseFromSet, '/fbot_world/get_set', self.getPoseFromSet) self.sets_names = self.create_service(GetSets, '/fbot_world/get_groups_names', self.getGroupNames) + self.room_server = self.create_service(GetRoom, '/fbot_world/get_room', self.getRoom) + + # Publish once after a short delay so RViz has time to subscribe + self.create_timer(1.0, self._publish_debug_markers_once) + self.get_logger().info(f"Pose node started!!!") + # ------------------------------------------------------------------ + # Debug marker helpers + # ------------------------------------------------------------------ + + def _make_color(self, rgba: tuple) -> ColorRGBA: + return marker_utils.make_color(rgba) + + def _make_lifetime_forever(self) -> Duration: + """Duration(sec=0, nanosec=0) means the marker lives forever.""" + return marker_utils.lifetime_forever() + + def _polygon_to_line_strip_marker( + self, + polygon: list, + marker_id: int, + ns: str, + color: tuple, + z: float = 0.0, + line_width: float = 0.05, + ) -> Marker: + """ + @brief: Creates a LINE_STRIP marker that traces a closed polygon. + @param polygon: List of [x, y] vertices. + @param marker_id: Unique integer ID within the namespace. + @param ns: Marker namespace string. + @param color: (r, g, b, a) tuple. + @param z: Height at which to draw the polygon. + @param line_width: Width of the lines in meters. + @return: Populated Marker message. + """ + return marker_utils.polygon_line_strip(polygon, marker_id, ns, color, z=z, line_width=line_width) + + def _polygon_to_fill_marker( + self, + polygon: list, + marker_id: int, + ns: str, + color: tuple, + z: float = 0.0, + ) -> Marker: + """ + @brief: Creates a TRIANGLE_LIST marker that fills a convex polygon. + Uses a fan triangulation from the centroid — works well for convex/mildly concave shapes. + @param polygon: List of [x, y] vertices. + @param marker_id: Unique integer ID within the namespace. + @param ns: Marker namespace string. + @param color: (r, g, b, a) tuple. + @param z: Height at which to draw the fill. + @return: Populated Marker message. + """ + return marker_utils.polygon_fill(polygon, marker_id, ns, color, z=z) + + def _text_marker( + self, + text: str, + position: tuple, + marker_id: int, + ns: str, + color: tuple, + z: float = 0.3, + text_size: float = 0.25, + ) -> Marker: + """ + @brief: Creates a TEXT_VIEW_FACING marker at a given position. + @param text: Label string to display. + @param position: (x, y) centroid for the text. + @param marker_id: Unique integer ID within the namespace. + @param ns: Marker namespace string. + @param color: (r, g, b, a) tuple. + @param z: Height offset for the text. + @param text_size: Size of the text in meters. + @return: Populated Marker message. + """ + return marker_utils.text_marker(text, position, marker_id, ns, z=z, text_size=text_size) + + def _centroid(self, polygon: list) -> tuple: + return marker_utils.centroid(polygon) + + def _publish_debug_markers_once(self): + """ + @brief: Builds and publishes a MarkerArray covering all rooms and their + sub-areas (objects) loaded from the YAML file. Called once via timer. + """ + marker_array = MarkerArray() + marker_id = 0 + + rooms = self.targets.get('rooms', {}) + for room_idx, (room_name, room_data) in enumerate(rooms.items()): + room_color = self.ROOM_COLORS[room_idx % len(self.ROOM_COLORS)] + room_vertices = room_data.get('vertices', []) + + if room_vertices: + # Filled polygon (z slightly below outline so it doesn't z-fight) + marker_array.markers.append( + self._polygon_to_fill_marker(room_vertices, marker_id, 'rooms', room_color, z=0.01) + ) + marker_id += 1 + + # Outline + marker_array.markers.append( + self._polygon_to_line_strip_marker(room_vertices, marker_id, 'rooms', room_color, z=0.02, line_width=0.06) + ) + marker_id += 1 + + # Room label at centroid + cx, cy = self._centroid(room_vertices) + marker_array.markers.append( + self._text_marker(room_name, (cx, cy), marker_id, 'rooms', room_color, z=0.5, text_size=0.3) + ) + marker_id += 1 + + # Sub-areas (objects) inside the room + objects = room_data.get('objects', {}) + for obj_idx, (obj_name, obj_vertices) in enumerate(objects.items()): + if not obj_vertices: + continue + obj_color = self.OBJECT_COLORS[obj_idx % len(self.OBJECT_COLORS)] + + marker_array.markers.append( + self._polygon_to_fill_marker(obj_vertices, marker_id, f'room_{room_name}_objects', obj_color, z=0.03) + ) + marker_id += 1 + + marker_array.markers.append( + self._polygon_to_line_strip_marker(obj_vertices, marker_id, f'room_{room_name}_objects', obj_color, z=0.04, line_width=0.04) + ) + marker_id += 1 + + # Object label + cx, cy = self._centroid(obj_vertices) + marker_array.markers.append( + self._text_marker(f"{room_name}\n{obj_name}", (cx, cy), marker_id, f'room_{room_name}_objects', obj_color, z=0.25, text_size=0.18) + ) + marker_id += 1 + + self.marker_pub.publish(marker_array) + self.get_logger().info(f"Published {len(marker_array.markers)} debug markers ({len(rooms)} rooms).") + + # Cancel timer so we only publish once (markers persist forever) + self._debug_timer.cancel() + + # Store timer ref so we can cancel it inside the callback + def create_timer(self, period, callback): + timer = super().create_timer(period, callback) + if callback == self._publish_debug_markers_once: + self._debug_timer = timer + return timer + + # ------------------------------------------------------------------ + # Existing methods (unchanged) + # ------------------------------------------------------------------ + + def loadTargets(self): + content = readYamlFile(self.file_path) + if 'poses' not in content: + self.targets = { + 'poses': content, + 'rooms': {} + } + else: + self.targets = content + def readPose(self, group_set: str, key: str): """ @brief: Reads pose data for a given key from the Redis database. @@ -66,6 +265,7 @@ def readPose(self, group_set: str, key: str): pose.orientation.z = float(db_pose[b'oz']) pose.orientation.w = float(db_pose[b'ow']) + return pose def declareParameters(self): @@ -80,7 +280,6 @@ def readParameters(self): """ self.config_file_name = self.get_parameter('config_file_name').get_parameter_value().string_value - def readSize(self, group_set: str, key: str): ''' @brief: Reads size (scale) data for a given key from the Redis database. @@ -103,13 +302,13 @@ def setStaticPose(self): @brief: Stores all target poses as static entries in the Redis database. ''' with self.r.pipeline() as pipe: - for target in self.targets.keys(): - for p_id, pose in self.targets[target].items(): + self.get_logger().info(f"Setting static poses in Redis for targets: {self.targets['poses'].keys()}") + for target in self.targets['poses'].keys(): + for p_id, pose in self.targets['poses'][target].items(): key = str(target)+'/' + p_id + '/' + 'pose' pipe.hmset(key, pose) pipe.execute() - def setResponseError(self): ''' @brief: A function that set a error pose @@ -136,36 +335,43 @@ def getPoseFromSet(self, req: GetPoseFromSet.Request, res: GetPoseFromSet.Respon If the group set is not found or empty, it returns a 'NaN. Error codes: - 0: Success - - 1: Group set is empty + - 1: Group set is empty, return all poses in 'targets' - 2: Group set not found in targets + - 3: Exception occurred @param req: The service request containing the group set. @param res: The service response with a array populate with poses and sizes. @return: A filled GetPoseFromSet.Response object. ''' - - if req.group_set in self.targets.keys(): - for key in self.targets[req.group_set]: + try: + if req.group_set in self.targets['poses'].keys(): + for key in self.targets['poses'][req.group_set]: + poses = FBOTPoses() + poses.key = key + poses.pose = self.readPose(req.group_set, key) + poses.size = self.readSize(req.group_set, key) + res.error = 0 + res.pose_array.append(poses) + elif req.group_set == '' or req.group_set == 'None': + res.error = 1 + for key in self.targets['poses']['targets'].keys(): + poses = FBOTPoses() + poses.key = key + poses.pose = self.readPose('targets', key) + poses.size = self.readSize('targets', key) + res.pose_array.append(poses) + else: poses = FBOTPoses() - poses.key = key - poses.pose = self.readPose(req.group_set, key) - poses.size = self.readSize(req.group_set, key) - res.error = 0 + poses.key = 'NaN' + poses.pose, poses.size = self.setResponseError() res.pose_array.append(poses) - - elif req.group_set == '' or req.group_set == 'None': - poses = FBOTPoses() - poses.key = 'NaN' - poses.pose, poses.size = self.setResponseError() - res.pose_array.append(poses) - res.error = 1 - - else: + res.error = 2 + except Exception as e: + self.get_logger().error(f"Error in getPoseFromSet: {e}") poses = FBOTPoses() poses.key = 'NaN' poses.pose, poses.size = self.setResponseError() res.pose_array.append(poses) - res.error = 2 - + res.error = 3 return res def getPose(self, req: GetPose.Request, res: GetPose.Response): @@ -182,25 +388,21 @@ def getPose(self, req: GetPose.Request, res: GetPose.Response): @param res: The service response to populate with pose and size. @return: A filled GetPose.Response object. ''' - if req.key == '' or req.key == 'None': self.get_logger().error("The key is empty.") res.error = 1 res.pose, res.size = self.setResponseError() return res - else: if req.group_set == '' or req.group_set == 'None': req.group_set = 'targets' self.get_logger().warning("Class is not specified, using 'targets' as default") - - if req.group_set not in self.targets.keys(): - self.get_logger().error("Group Set not found in targets: " + str(self.targets.keys())) + if req.group_set not in self.targets['poses'].keys(): + self.get_logger().error("Group Set not found in targets: " + str(self.targets['poses'].keys())) res.error = 2 res.pose, res.size = self.setResponseError() return res - - if req.key not in self.targets[req.group_set].keys(): + if req.key not in self.targets['poses'][req.group_set].keys(): self.get_logger().error("Key not found in "+req.group_set+": " + str(req.key)) res.error = 3 res.pose, res.size = self.setResponseError() @@ -214,23 +416,85 @@ def getPose(self, req: GetPose.Request, res: GetPose.Response): def getGroupNames(self, req: GetSets.Request, res: GetSets.Response): ''' - @brief: A service that returns all groups names in yaml file + @brief: A service that returns all poses and rooms names with postions and objects in yaml file @param req: The service request @param res: The service response - @return: A array with all group names + @return: A array with all poses names and rooms names with postions and objects in yaml file ''' - res.response = self.targets.keys() + for target in self.targets['poses'].keys(): + pose = DBPose() + pose.type = target + for key in self.targets['poses'][target].keys(): + pose.poses.append(key) + res.poses.append(pose) + for room in self.targets['rooms'].keys(): + room_ = FBOTRooms() + room_.room = room + if 'objects' in self.targets['rooms'][room].keys(): + for object in self.targets['rooms'][room]['objects'].keys(): + room_.objects.append(object) + if 'poses' in self.targets['rooms'][room].keys(): + for pose in self.targets['rooms'][room]['poses']: + room_.poses.append(pose) + res.rooms.append(room_) return res + def getRoom(self, req: GetRoom.Request, res: GetRoom.Response): + """ + @brief: A service that returns the room and place name where the robot is, based on its current position. + The function checks if the pose is within any of the defined rooms and places in the YAML configuration. + @param req: The service request containing the robot's current pose. + @param res: The service response to populate with the room and place names. + @return Execution outcome (SUCCEED, ABORT). + """ + pose = Pose() + pose = req.pose + for room in self.targets['rooms'].items(): + polygon = np.array(room[1]['vertices'],dtype= np.float32) + self.itens_points = pose.position + if self.is_point_in_area(polygon, [self.itens_points.x, self.itens_points.y]): + for place in room[1]['objects'].items(): + subpolygon = np.array(place[1],dtype= np.float32) + if self.is_point_in_area(subpolygon, [self.itens_points.x, self.itens_points.y]): + res.response = [room[0], place[0]] + return res + res.response = [room[0], 'None'] + return res + res.response = ['None', 'None'] + return res + + def is_point_in_area(self, polygon, point) -> bool: + """ + @brief A function thats verify if a point are inside a area. + @param point: Point to verify. + @return a bool param, if the intersections are a pair value return true, else, return false. + """ + x, y = point[0], point[1] + inside = False + n = len(polygon) + j = n - 1 + for i in range(n): + xi, yi = polygon[i][0], polygon[i][1] + xj, yj = polygon[j][0], polygon[j][1] + intersect = ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi) + if intersect: + inside = not inside + j = i + return inside + + def intersect(self, A,B,C,D): + return self.ccw(A,C,D) != self.ccw(B,C,D) and self.ccw(A,B,C) != self.ccw(A,B,D) + + def ccw(self,A,B,C): + return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0]) def main(args=None) -> None: rclpy.init(args=args) node = PosePlugin('pose') - rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/fbot_world/fbot_world/pose_writer.py b/fbot_world/fbot_world/pose_writer.py index c00a0cc..cc9359e 100755 --- a/fbot_world/fbot_world/pose_writer.py +++ b/fbot_world/fbot_world/pose_writer.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import rclpy -import yaml import os from rclpy.exceptions import ROSInterruptException @@ -10,6 +9,8 @@ from collections import OrderedDict from rclpy.node import Node +from fbot_world import yaml_io + ''' Save the current pose in the specified topic to a yaml file. Created by Gabriel Dorneles on 2024-10-06. @@ -17,27 +18,6 @@ ''' -class OrderedDumper(yaml.SafeDumper): - ''' - @brief Custom YAML dumper to handle OrderedDict. - ''' - def represent_ordereddict(self, data): - return self.represent_dict(data.items()) - -class OrderedLoader(yaml.SafeLoader): - pass - -def construct_ordered_dict(loader, node): - ''' - @brief Custom YAML loader to handle OrderedDict. - ''' - return OrderedDict(loader.construct_pairs(node)) - -OrderedLoader.add_constructor( - yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, - construct_ordered_dict -) - class PoseWriter (Node): ''' @class PoseWriter @@ -117,28 +97,19 @@ def write_to_yaml(self): ''' @brief Write the collected poses to a YAML file. ''' - OrderedDumper.add_representer(OrderedDict, OrderedDumper.represent_ordereddict) - if os.path.exists(self.yaml_path): self.get_logger().info(f"{self.yaml_file} already exists. The new poses will be appended to the existing data.") - - with open(self.yaml_path, 'r') as yaml_file: - try: - existing_data = yaml.load(yaml_file, Loader=OrderedLoader) or OrderedDict() - except yaml.YAMLError as e: - self.get_logger().error(f"Error reading {self.yaml_file}: {e}") - existing_data = OrderedDict() else: self.get_logger().info(f"{self.yaml_file} does not exist. Creating a new file.") - existing_data = OrderedDict() + + existing_data = yaml_io.load_ordered(self.yaml_path) if 'targets' not in existing_data: existing_data['targets']= OrderedDict() existing_data['targets'].update(self.poses['targets']) - with open(self.yaml_path, 'w') as yaml_file: - yaml.dump(existing_data, yaml_file, default_flow_style=False, Dumper=OrderedDumper) - + yaml_io.dump_ordered(existing_data, self.yaml_path) + return diff --git a/fbot_world/fbot_world/room_writer.py b/fbot_world/fbot_world/room_writer.py new file mode 100644 index 0000000..bf512ab --- /dev/null +++ b/fbot_world/fbot_world/room_writer.py @@ -0,0 +1,358 @@ +#!/usr/bin/env python3 + +import os +import re +import threading + +import rclpy + +from collections import OrderedDict +from rclpy.exceptions import ROSInterruptException +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from geometry_msgs.msg import PointStamped +from visualization_msgs.msg import MarkerArray + +from fbot_world import marker_utils, yaml_io + +''' +Interactively annotate room (and object sub-area) boundaries by clicking points +in RViz, then write them to a YAML file under the 'rooms' section in the same +format consumed by pose.py (PosePlugin). + +Workflow: launch navigation with the map and RViz, run this node, then use the +RViz "Publish Point" tool to click the corners of each room. The in-progress and +completed polygons are republished live as markers so you can see the boundaries +as you draw them. Only the 'rooms' section is written; the 'poses' section (filled +by pose_writer.py / place_pose_writer.py) is preserved untouched. + +Created by Gabriel Dorneles on 2026-06-08. +''' + + +# Latched so RViz still receives the latest markers if it subscribes late. +qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, +) + +NAME_RE = re.compile(r"[A-Za-z0-9_.-]+") + +# Same palettes as PosePlugin so the preview matches the runtime markers. +ROOM_COLORS = [ + (0.2, 0.6, 1.0, 0.35), # blue + (0.2, 1.0, 0.4, 0.35), # green + (1.0, 0.6, 0.1, 0.35), # orange + (0.9, 0.2, 0.9, 0.35), # magenta + (0.1, 0.9, 0.9, 0.35), # cyan + (1.0, 1.0, 0.1, 0.35), # yellow +] + +OBJECT_COLORS = [ + (1.0, 0.2, 0.2, 0.45), # red + (1.0, 0.8, 0.2, 0.45), # amber + (0.6, 0.2, 1.0, 0.45), # purple + (0.2, 1.0, 0.8, 0.45), # teal +] + +ACTIVE_COLOR = (1.0, 1.0, 1.0, 0.9) + + +class RoomWriter(Node): + ''' + @class RoomWriter + @brief A ROS 2 node that captures room/object boundary polygons from RViz + "Publish Point" clicks and saves them to a YAML file's 'rooms' section. + ''' + + def __init__(self): + ''' + @brief Constructor for the RoomWriter node. + ''' + super().__init__(node_name='room_writer') + + self.declare_parameter('clicked_point_topic', '/clicked_point') + self.declare_parameter('marker_topic', '/room_writer/preview') + self.declare_parameter('frame', 'map') + + self.clicked_topic = self.get_parameter('clicked_point_topic').get_parameter_value().string_value + self.marker_topic = self.get_parameter('marker_topic').get_parameter_value().string_value + self.frame = self.get_parameter('frame').get_parameter_value().string_value + + ws_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../../../..")) + self.config_path = os.path.join(ws_dir, "src", "fbot_world", "fbot_world", "config") + + while True: + self.yaml_file = input("Enter the name of the file to save the rooms (e.g., 'pose_finals.yaml'): ").strip() + if self.yaml_file.endswith('.yaml'): + break + self.get_logger().warning("Invalid input. The file name must end with '.yaml'. Please try again.") + self.yaml_path = os.path.join(self.config_path, self.yaml_file) + + # State shared with the clicked-point callback (which runs on the spin + # thread while the main thread blocks on input()). + self.lock = threading.Lock() + self.current_points = [] + self.active_label = None + self.completed_rooms = OrderedDict() + + self.marker_pub = self.create_publisher(MarkerArray, self.marker_topic, qos) + self.click_sub = self.create_subscription( + PointStamped, self.clicked_topic, self.clicked_cb, 10 + ) + + # Spin in the background so clicked points are received while the main + # thread blocks on input(). + self._stop_event = threading.Event() + self.executor_thread = threading.Thread(target=self._spin, daemon=True) + self.executor_thread.start() + + def _spin(self) -> None: + # spin_once in a loop (rather than rclpy.spin) so stop() can end the + # thread before the node is destroyed, avoiding a teardown race. + try: + while rclpy.ok() and not self._stop_event.is_set(): + rclpy.spin_once(self, timeout_sec=0.1) + except (ROSInterruptException, rclpy.executors.ExternalShutdownException): + pass + + def stop(self) -> None: + '''@brief Stop the background spin thread and wait for it to finish.''' + self._stop_event.set() + if self.executor_thread.is_alive(): + self.executor_thread.join(timeout=2.0) + + # ------------------------------------------------------------------ + # Clicked-point handling and live preview + # ------------------------------------------------------------------ + + def clicked_cb(self, msg: PointStamped) -> None: + '''@brief Append a clicked point to the active polygon and refresh preview.''' + point = [msg.point.x, msg.point.y] + with self.lock: + self.current_points.append(point) + count = len(self.current_points) + self.get_logger().info(f"Point {count}: ({point[0]:.3f}, {point[1]:.3f})") + self.publish_preview() + + def publish_preview(self) -> None: + ''' + @brief Rebuild and publish the full marker array: every completed room and + object, plus the in-progress polygon. A leading DELETEALL clears stale + markers so undo/reset are reflected. + ''' + with self.lock: + rooms = [(name, data) for name, data in self.completed_rooms.items()] + active = [list(p) for p in self.current_points] + + marker_array = MarkerArray() + marker_array.markers.append(marker_utils.delete_all_marker(self.frame)) + marker_id = 0 + + for room_idx, (room_name, room_data) in enumerate(rooms): + room_color = ROOM_COLORS[room_idx % len(ROOM_COLORS)] + vertices = room_data.get('vertices', []) + if vertices: + marker_array.markers.append( + marker_utils.polygon_fill(vertices, marker_id, 'rooms', room_color, z=0.01, frame=self.frame)) + marker_id += 1 + marker_array.markers.append( + marker_utils.polygon_line_strip(vertices, marker_id, 'rooms', room_color, z=0.02, line_width=0.06, frame=self.frame)) + marker_id += 1 + cx, cy = marker_utils.centroid(vertices) + marker_array.markers.append( + marker_utils.text_marker(room_name, (cx, cy), marker_id, 'rooms', z=0.5, text_size=0.3, frame=self.frame)) + marker_id += 1 + + objects = room_data.get('objects', {}) + for obj_idx, (obj_name, obj_vertices) in enumerate(objects.items()): + if not obj_vertices: + continue + obj_color = OBJECT_COLORS[obj_idx % len(OBJECT_COLORS)] + ns = f'room_{room_name}_objects' + marker_array.markers.append( + marker_utils.polygon_fill(obj_vertices, marker_id, ns, obj_color, z=0.03, frame=self.frame)) + marker_id += 1 + marker_array.markers.append( + marker_utils.polygon_line_strip(obj_vertices, marker_id, ns, obj_color, z=0.04, line_width=0.04, frame=self.frame)) + marker_id += 1 + cx, cy = marker_utils.centroid(obj_vertices) + marker_array.markers.append( + marker_utils.text_marker(f"{room_name}\n{obj_name}", (cx, cy), marker_id, ns, z=0.25, text_size=0.18, frame=self.frame)) + marker_id += 1 + + # In-progress polygon: spheres at each click plus an open outline. + if active: + marker_array.markers.append( + marker_utils.point_spheres(active, marker_id, 'active', ACTIVE_COLOR, scale=0.12, z=0.06, frame=self.frame)) + marker_id += 1 + if len(active) >= 2: + marker_array.markers.append( + marker_utils.polygon_line_strip(active, marker_id, 'active', ACTIVE_COLOR, z=0.06, line_width=0.04, frame=self.frame, closed=False)) + marker_id += 1 + + self.marker_pub.publish(marker_array) + + # ------------------------------------------------------------------ + # Interactive capture + # ------------------------------------------------------------------ + + def capture_polygon(self, label: str) -> list: + ''' + @brief Capture one polygon: the user clicks corners in RViz, then commands + the terminal to close it. + @return The list of [x, y] vertices, or None if aborted. + ''' + with self.lock: + self.current_points = [] + self.active_label = label + self.publish_preview() + + print(f"\n Drawing '{label}'. Use the RViz 'Publish Point' tool to click each corner.") + while True: + cmd = input(" [Enter]=close polygon u=undo last r=reset a=abort : ").strip().lower() + if cmd == '': + with self.lock: + count = len(self.current_points) + if count < 3: + self.get_logger().warning(f"A polygon needs at least 3 points (have {count}).") + continue + with self.lock: + points = [list(p) for p in self.current_points] + self.current_points = [] + self.active_label = None + # Don't publish here: the caller registers the closed polygon + # (as a room or object) and then refreshes the preview, so the + # finished shape is drawn instead of a momentary blank. + return points + elif cmd == 'u': + with self.lock: + if self.current_points: + removed = self.current_points.pop() + self.get_logger().info(f"Removed point ({removed[0]:.3f}, {removed[1]:.3f}).") + self.publish_preview() + elif cmd == 'r': + with self.lock: + self.current_points = [] + self.get_logger().info("Reset current polygon.") + self.publish_preview() + elif cmd == 'a': + with self.lock: + self.current_points = [] + self.active_label = None + self.publish_preview() + return None + else: + self.get_logger().warning("Unknown command. Use Enter, u, r or a.") + + def _ask_name(self, prompt: str) -> str: + '''@brief Read a name and validate it against the allowed character set.''' + name = input(prompt).strip() + if name == '': + return '' + if not NAME_RE.fullmatch(name): + self.get_logger().warning("Invalid name. Use only letters, numbers, '-', '_' or '.'.") + return None + return name + + def run(self) -> None: + '''@brief Main interactive session: capture rooms until the user is done.''' + print("\n=== Room annotator ===") + print("Make sure RViz has a MarkerArray display on topic " + f"'{self.marker_topic}' (frame '{self.frame}').") + self.publish_preview() + + while rclpy.ok(): + room_name = self._ask_name("\nRoom name (blank to finish and save): ") + if room_name is None: + continue + if room_name == '': + break + if room_name in self.completed_rooms: + overwrite = input(f"Room '{room_name}' already exists. Overwrite? (y/n): ").strip().lower() + if overwrite != 'y': + continue + + vertices = self.capture_polygon(f"room:{room_name}") + if vertices is None: + self.get_logger().info(f"Aborted room '{room_name}'.") + continue + + # Register the room as soon as it's closed, so the finished polygon + # (filled, labelled with its name, coloured) is shown immediately and + # stays visible while we prompt for pose names and draw its objects. + room_entry = OrderedDict() + room_entry['poses'] = [] + room_entry['vertices'] = vertices + room_entry['objects'] = OrderedDict() + with self.lock: + self.completed_rooms[room_name] = room_entry + self.publish_preview() + + poses_raw = input(" Pose names in this room (comma-separated, blank for none): ") + room_entry['poses'] = [p.strip() for p in poses_raw.split(',') if p.strip()] + + while True: + obj_name = self._ask_name(" Object sub-area name (blank to finish this room): ") + if obj_name is None: + continue + if obj_name == '': + break + obj_vertices = self.capture_polygon(f"{room_name}/{obj_name}") + if obj_vertices is None: + self.get_logger().info(f"Aborted object '{obj_name}'.") + continue + with self.lock: + room_entry['objects'][obj_name] = obj_vertices + self.publish_preview() + + self.get_logger().info( + f"Room '{room_name}' captured: {len(vertices)} vertices, " + f"{len(room_entry['objects'])} object(s), {len(room_entry['poses'])} pose name(s)." + ) + + if self.completed_rooms: + self.write_to_yaml() + self.get_logger().info(f"Rooms saved to {self.yaml_file}. Shutting down node.") + else: + self.get_logger().info("No rooms captured. Nothing written.") + + def write_to_yaml(self) -> None: + ''' + @brief Merge the captured rooms into the YAML file's 'rooms' section, + preserving the existing 'poses' section and any other top-level keys. + ''' + if os.path.exists(self.yaml_path): + self.get_logger().info(f"{self.yaml_file} already exists. Rooms will be merged into the existing data.") + else: + self.get_logger().info(f"{self.yaml_file} does not exist. Creating a new file.") + + existing_data = yaml_io.load_ordered(self.yaml_path) + + rooms = existing_data.get('rooms') + if not isinstance(rooms, (dict, OrderedDict)): + rooms = OrderedDict() + rooms.update(self.completed_rooms) + existing_data['rooms'] = rooms + + yaml_io.dump_ordered(existing_data, self.yaml_path) + + +def main(args=None) -> None: + writer = None + try: + rclpy.init(args=args) + writer = RoomWriter() + writer.run() + except (ROSInterruptException, KeyboardInterrupt): + pass + finally: + if writer is not None: + writer.stop() + writer.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/fbot_world/fbot_world/yaml_io.py b/fbot_world/fbot_world/yaml_io.py new file mode 100644 index 0000000..ea42ab0 --- /dev/null +++ b/fbot_world/fbot_world/yaml_io.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +""" +Shared ordered-YAML helpers for fbot_world. + +The pose writers and the room annotator all need to round-trip YAML while +preserving key order, so the saved config files stay diff-friendly and match +the hand-authored layout. This centralises the OrderedDict dumper/loader that +was previously duplicated across the writer nodes. +""" + +import os + +from collections import OrderedDict + +import yaml + + +class OrderedDumper(yaml.SafeDumper): + '''@brief YAML dumper that serialises OrderedDict as a plain mapping.''' + + +def _represent_ordereddict(dumper, data): + return dumper.represent_dict(data.items()) + + +OrderedDumper.add_representer(OrderedDict, _represent_ordereddict) + + +class OrderedLoader(yaml.SafeLoader): + '''@brief YAML loader that builds OrderedDict to preserve key order.''' + + +def _construct_ordered_dict(loader, node): + return OrderedDict(loader.construct_pairs(node)) + + +OrderedLoader.add_constructor( + yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, + _construct_ordered_dict, +) + + +def load_ordered(path: str) -> OrderedDict: + ''' + @brief Load a YAML file as an OrderedDict. + @return The parsed mapping, or an empty OrderedDict if the file is missing, + empty or unparseable. + ''' + if not os.path.exists(path): + return OrderedDict() + with open(path, 'r') as f: + try: + return yaml.load(f, Loader=OrderedLoader) or OrderedDict() + except yaml.YAMLError: + return OrderedDict() + + +def dump_ordered(data, path: str) -> None: + '''@brief Write a mapping to a YAML file in block style, preserving order.''' + with open(path, 'w') as f: + yaml.dump(data, f, default_flow_style=False, Dumper=OrderedDumper) diff --git a/fbot_world/world_scripts/__init__.py b/fbot_world/scripts/__init__.py similarity index 100% rename from fbot_world/world_scripts/__init__.py rename to fbot_world/scripts/__init__.py diff --git a/fbot_world/world_scripts/world_plugin.py b/fbot_world/scripts/world_plugin.py similarity index 100% rename from fbot_world/world_scripts/world_plugin.py rename to fbot_world/scripts/world_plugin.py diff --git a/fbot_world/setup.py b/fbot_world/setup.py index 54fd50b..bd03fae 100644 --- a/fbot_world/setup.py +++ b/fbot_world/setup.py @@ -13,7 +13,7 @@ ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'fbot_world'), glob.glob('fbot_world/*.py')), (os.path.join('share', package_name, 'launch'), glob.glob('launch/*.launch.py')), - (os.path.join('share', package_name, 'world_scripts'), glob.glob('world_scripts/*.py')), + (os.path.join('share', package_name, 'scripts'), glob.glob('scripts/*.py')), (os.path.join('share', package_name, 'config'), glob.glob('config/*.yaml')) ], @@ -29,6 +29,7 @@ 'pose = fbot_world.pose:main', 'pose_writer = fbot_world.pose_writer:main', 'place_pose_writer = fbot_world.place_pose_writer:main', + 'room_writer = fbot_world.room_writer:main', ], }, ) diff --git a/fbot_world/world_scripts/merge_yaml_files.py b/fbot_world/world_scripts/merge_yaml_files.py deleted file mode 100644 index ac69e94..0000000 --- a/fbot_world/world_scripts/merge_yaml_files.py +++ /dev/null @@ -1,46 +0,0 @@ -import yaml - -def mergeYamlFiles(file: str = None, - plugin_file: str = None) -> dict: - ''' - @brief Utility function to merge two ROS 2 YAML parameter files. - @param file: The base YAML file whit targets. - @param plugin_file: The plugin YAML file. - - This script defines a function that reads two YAML files—one for the base configuration - and one for a plugin—and merges their `ros__parameters`. It also extracts a list of target keys - if available and injects them as a separate 'target' list. - ''' - with open(file, 'r', encoding='utf-8') as f1, open(plugin_file, 'r', encoding='utf-8') as f2: - file1 = yaml.safe_load(f1) or {} - file2 = yaml.safe_load(f2) or {} - file1 = file1[list(file1.keys())[0]]['ros__parameters'] - file2 = file2['plugin']['ros__parameters'] - - def recursiveMerge(dict1: dict, dict2: dict) -> dict: - ''' - @brief Recursive function to merge two dictionaries. - @param dict1: The first dictionary. - @param dict2: The second dictionary. - @return: The merged dictionary. - - ''' - for key, value in dict2.items(): - if key in dict1 and isinstance(dict1[key], dict) and isinstance(value, dict): - recursiveMerge(dict1[key], value) - else: - dict1[key] = value - return dict1 - - parameters = recursiveMerge(file1, file2) - - if ('targets' in file1): - targets = list(file1.get('targets').keys()) - target = [] - for _ in range (len(targets)): - target.append(str(targets[_])) - target_dict = {'target': target} - - parameters.update(target_dict) - - return parameters diff --git a/fbot_world_msgs/CMakeLists.txt b/fbot_world_msgs/CMakeLists.txt index 06e7e79..5516dcc 100644 --- a/fbot_world_msgs/CMakeLists.txt +++ b/fbot_world_msgs/CMakeLists.txt @@ -13,10 +13,13 @@ find_package(nav_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/DBPose.msg" "msg/FBOTPoses.msg" + "msg/FBOTRooms.msg" + "msg/FBOTVertices.msg" "srv/GetKey.srv" "srv/GetPose.srv" "srv/GetPoseFromSet.srv" "srv/GetSets.srv" + "srv/GetRoom.srv" DEPENDENCIES std_msgs geometry_msgs sensor_msgs vision_msgs ) diff --git a/fbot_world_msgs/msg/DBPose.msg b/fbot_world_msgs/msg/DBPose.msg index 7aa1b71..34c5c29 100644 --- a/fbot_world_msgs/msg/DBPose.msg +++ b/fbot_world_msgs/msg/DBPose.msg @@ -1,2 +1,2 @@ -string label -geometry_msgs/Pose pose +string type +string[] poses diff --git a/fbot_world_msgs/msg/FBOTRooms.msg b/fbot_world_msgs/msg/FBOTRooms.msg new file mode 100644 index 0000000..b823489 --- /dev/null +++ b/fbot_world_msgs/msg/FBOTRooms.msg @@ -0,0 +1,3 @@ +string room +string[] poses +string[] objects \ No newline at end of file diff --git a/fbot_world_msgs/msg/FBOTVertices.msg b/fbot_world_msgs/msg/FBOTVertices.msg new file mode 100644 index 0000000..936cc68 --- /dev/null +++ b/fbot_world_msgs/msg/FBOTVertices.msg @@ -0,0 +1,2 @@ +string key +geometry_msgs/Point[] points diff --git a/fbot_world_msgs/srv/GetRoom.srv b/fbot_world_msgs/srv/GetRoom.srv new file mode 100644 index 0000000..322ade3 --- /dev/null +++ b/fbot_world_msgs/srv/GetRoom.srv @@ -0,0 +1,4 @@ +geometry_msgs/Pose pose +--- +string[] response + diff --git a/fbot_world_msgs/srv/GetSets.srv b/fbot_world_msgs/srv/GetSets.srv index 5c96fc9..1b95403 100644 --- a/fbot_world_msgs/srv/GetSets.srv +++ b/fbot_world_msgs/srv/GetSets.srv @@ -1,3 +1,4 @@ std_msgs/Empty empty --- -string[] response \ No newline at end of file +fbot_world_msgs/DBPose[] poses +fbot_world_msgs/FBOTRooms[] rooms \ No newline at end of file