diff --git a/README.md b/README.md
new file mode 100644
index 0000000..99f2da4
--- /dev/null
+++ b/README.md
@@ -0,0 +1,74 @@
+# Scene Manipulation Service
+
+This is the new and integrated scene manipulation service, that also offers a lookup service.
+
+
+
+### Documentation:
+
+Pending
+
+
+
+### Acknowledgements:
+Christian Larsen - FCC
\ No newline at end of file
diff --git a/description/sms_chart.graphml b/description/sms_chart.graphml
new file mode 100644
index 0000000..ffc3ccc
--- /dev/null
+++ b/description/sms_chart.graphml
@@ -0,0 +1,2713 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ manipulate
+
+
+
+
+
+
+
+
+
+ with?
+
+
+
+
+
+
+
+
+
+ folder
+
+
+
+
+
+
+
+
+
+ service
+
+
+
+
+
+
+
+
+
+ insert frame
+into folder
+
+
+
+
+
+
+
+
+
+ how?
+
+
+
+
+
+
+
+
+
+ remove frame
+from folder
+
+
+
+
+
+
+
+
+
+ how?
+
+
+
+
+
+
+
+
+
+ update
+
+
+
+
+
+
+
+
+
+ remove
+
+
+
+
+
+
+
+
+
+ static?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+can't manipulate
+static frames
+
+
+
+
+
+
+
+
+
+ keep position
+in world?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => false
+active => true
+tf => msg.tf
+
+
+
+
+
+
+
+
+
+ lookup tree:
+found?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ FAULT:
+frame doesn't
+exist
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => false
+active => true
+tf => lookup.tf
+
+
+
+
+
+
+
+
+
+ static?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ FAULT:
+can't manipulate
+static frames
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => true
+active => true
+tf => json.tf
+
+
+
+
+
+
+
+
+
+ change will
+produce cycle?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+the tf is a tree
+structure
+
+
+
+
+
+
+
+
+
+ parent exists
+in world?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ exists?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ static?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+can't manipulate
+static frames
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ folder tag is
+true?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ remove frame
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ change will
+produce cycle?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+the tf is a tree
+structure
+
+
+
+
+
+
+
+
+
+ parent exists
+in world?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => false
+active => true
+tf => msg.tf
+
+
+
+
+
+
+
+
+
+ FAULT
+
+
+
+
+
+
+
+
+
+ SUCCESS
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ change will
+produce cycle?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+the tf is a tree
+structure
+
+
+
+
+
+
+
+
+
+ parent exists
+in world?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ WARNING:
+frame will be
+parentless
+
+
+
+
+
+
+
+
+
+ exists in the
+tf buffer?
+
+
+
+
+
+
+
+
+
+ exists in the
+broadcaster
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ exists in the
+broadcaster
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ static?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+can't manipulate
+static frames
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ folder tag is
+true?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ remove frame
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ WARNING:
+following children
+are orphaned
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ exists in the
+tf buffer?
+
+
+
+
+
+
+
+
+
+ exists in the
+broadcaster
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ exists in the
+broadcaster
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ wait for
+2 seconds
+
+
+
+
+
+
+
+
+
+ exists in the
+tf buffer?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ wait for
+2 seconds
+
+
+
+
+
+
+
+
+
+ exists in the
+tf buffer?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT:
+do nothing.
+
+
+
+
+
+
+
+
+
+ WARNING:
+frame will be
+parentless
+
+
+
+
+
+
+
+
+
+ WARNING:
+following children
+are orphaned
+
+
+
+
+
+
+
+
+
+ WARNING:
+frame will be
+parentless
+
+
+
+
+
+
+
+
+
+ WARNING
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => false
+active => true
+tf => msg.tf
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => false
+active => true
+tf => msg.tf
+
+
+
+
+
+
+
+
+
+ add frame as:
+folder => true
+active => true
+tf => json.tf
+
+
+
+
+
+
+
+
+
+ WARNING:
+will be published
+until session
+ is over
+
+
+
+
+
+
+
+
+
+ name ==
+"world"?
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ FAULT: world
+is reserved.
+
+
+
+
+
+
+
+
+
+ name ==
+"world"?
+
+
+
+
+
+
+
+
+
+ YES
+
+
+
+
+
+
+
+
+
+ NO
+
+
+
+
+
+
+
+
+
+ FAULT: world
+is reserved.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/description/sms_chart.pdf b/description/sms_chart.pdf
new file mode 100644
index 0000000..c8b3dd6
Binary files /dev/null and b/description/sms_chart.pdf differ
diff --git a/description/sms_chart.png b/description/sms_chart.png
new file mode 100644
index 0000000..3a2e613
Binary files /dev/null and b/description/sms_chart.png differ
diff --git a/description/sms_steps.pdf b/description/sms_steps.pdf
new file mode 100644
index 0000000..9229da9
Binary files /dev/null and b/description/sms_steps.pdf differ
diff --git a/scene_manipulation_bringup/CMakeLists.txt b/scene_manipulation_bringup/CMakeLists.txt
new file mode 100644
index 0000000..4671421
--- /dev/null
+++ b/scene_manipulation_bringup/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(scene_manipulation_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch config
+ DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/scene_manipulation_bringup/config/scenario_1.rviz b/scene_manipulation_bringup/config/scenario_1.rviz
new file mode 100644
index 0000000..042f087
--- /dev/null
+++ b/scene_manipulation_bringup/config/scenario_1.rviz
@@ -0,0 +1,187 @@
+Panels:
+ - Class: rviz_common/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5
+ Tree Height: 728
+ - Class: rviz_common/Selection
+ Name: Selection
+ - Class: rviz_common/Tool Properties
+ Expanded:
+ - /2D Goal Pose1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz_common/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Class: rviz_default_plugins/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ above_toolchange_one:
+ Value: true
+ above_toolchange_two:
+ Value: true
+ base_link:
+ Value: true
+ dummy_frame:
+ Value: true
+ floor:
+ Value: true
+ gantry:
+ Value: true
+ gantry_plate:
+ Value: true
+ robot_facade_full:
+ Value: true
+ toolchange_one:
+ Value: true
+ toolchange_two:
+ Value: true
+ world:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ world:
+ dummy_frame:
+ {}
+ floor:
+ {}
+ gantry:
+ gantry_plate:
+ above_toolchange_one:
+ {}
+ above_toolchange_two:
+ {}
+ base_link:
+ {}
+ toolchange_one:
+ {}
+ toolchange_two:
+ {}
+ robot_facade_full:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
+ Single click: true
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
+ Value: true
+ Views:
+ Current:
+ Class: rviz_default_plugins/Orbit
+ Distance: 5.277319431304932
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: -1.5911049842834473
+ Y: -1.0035814046859741
+ Z: 1.2574316263198853
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5203982591629028
+ Target Frame:
+ Value: Orbit (rviz)
+ Yaw: 1.7403969764709473
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1025
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065010000000000000752000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1874
+ X: 46
+ Y: 25
diff --git a/scene_manipulation_bringup/launch/sms_example.launch.py b/scene_manipulation_bringup/launch/sms_example.launch.py
new file mode 100644
index 0000000..c9d970c
--- /dev/null
+++ b/scene_manipulation_bringup/launch/sms_example.launch.py
@@ -0,0 +1,26 @@
+import os
+from launch_ros.substitutions import FindPackageShare
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ scenario_path = "/home/endre/ws2/src/scene_manipulation_service/scene_manipulation_bringup/scenario_1"
+ parameters = {
+ "scenario_path": scenario_path
+ }
+
+ sms_node = Node(
+ package="scene_manipulation_service",
+ executable="sms",
+ namespace="",
+ output="screen",
+ parameters=[parameters],
+ remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
+ emulate_tty=True
+ )
+
+ nodes_to_start = [
+ sms_node
+ ]
+
+ return LaunchDescription(nodes_to_start)
diff --git a/scene_manipulation_bringup/package.xml b/scene_manipulation_bringup/package.xml
new file mode 100644
index 0000000..b5eae8a
--- /dev/null
+++ b/scene_manipulation_bringup/package.xml
@@ -0,0 +1,32 @@
+
+
+
+ scene_manipulation_bringup
+ 0.0.1
+
+ Bringup package for the Volvo Rita project.
+
+
+ Endre Erős
+ Endre Erős
+
+ Endre Erős
+
+ no_license
+
+ ament_cmake
+
+ ament_index_python
+ launch
+ launch_ros
+ scene_manipulation_msgs
+ ros2run
+
+ ament_cmake_gtest
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/scene_manipulation_bringup/scenario_1/frame_1.json b/scene_manipulation_bringup/scenario_1/frame_1.json
new file mode 100644
index 0000000..214ce04
--- /dev/null
+++ b/scene_manipulation_bringup/scenario_1/frame_1.json
@@ -0,0 +1,18 @@
+{
+ "active": true,
+ "child_frame_id": "frame_1",
+ "parent_frame_id": "world",
+ "transform": {
+ "translation": {
+ "x": 1.0,
+ "y": 0.0,
+ "z": 0.0
+ },
+ "rotation": {
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0,
+ "w": 1.0
+ }
+ }
+}
\ No newline at end of file
diff --git a/scene_manipulation_bringup/scenario_1/frame_2.json b/scene_manipulation_bringup/scenario_1/frame_2.json
new file mode 100644
index 0000000..e554c4f
--- /dev/null
+++ b/scene_manipulation_bringup/scenario_1/frame_2.json
@@ -0,0 +1,18 @@
+{
+ "active": false,
+ "child_frame_id": "frame_2",
+ "parent_frame_id": "frame_1",
+ "transform": {
+ "translation": {
+ "x": 0.0,
+ "y": 1.0,
+ "z": 0.0
+ },
+ "rotation": {
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0,
+ "w": 1.0
+ }
+ }
+}
\ No newline at end of file
diff --git a/scene_manipulation_msgs/CMakeLists.txt b/scene_manipulation_msgs/CMakeLists.txt
new file mode 100644
index 0000000..d0ffcc1
--- /dev/null
+++ b/scene_manipulation_msgs/CMakeLists.txt
@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(scene_manipulation_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "srv/ManipulateScene.srv"
+ "srv/LookupTransform.srv"
+ DEPENDENCIES
+ builtin_interfaces
+ geometry_msgs
+)
+
+ament_package()
diff --git a/scene_manipulation_msgs/package.xml b/scene_manipulation_msgs/package.xml
new file mode 100644
index 0000000..acf3743
--- /dev/null
+++ b/scene_manipulation_msgs/package.xml
@@ -0,0 +1,32 @@
+
+
+
+ scene_manipulation_msgs
+ 0.0.1
+ Messages for the Scene Manipulation Service.
+
+ Endre Erős
+ Endre Erős
+ Endre Erős
+
+ no_license
+ ament_cmake
+
+ rclcpp
+ std_msgs
+
+ builtin_interfaces
+ rosidl_default_generators
+
+ builtin_interfaces
+ rosidl_default_runtime
+
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/scene_manipulation_msgs/srv/LookupTransform.srv b/scene_manipulation_msgs/srv/LookupTransform.srv
new file mode 100644
index 0000000..333f389
--- /dev/null
+++ b/scene_manipulation_msgs/srv/LookupTransform.srv
@@ -0,0 +1,10 @@
+# Request
+string parent_frame_id
+string child_frame_id
+int32 deadline
+
+---
+
+# Reply
+bool success
+geometry_msgs/TransformStamped transform
\ No newline at end of file
diff --git a/scene_manipulation_msgs/srv/ManipulateScene.srv b/scene_manipulation_msgs/srv/ManipulateScene.srv
new file mode 100644
index 0000000..c89616f
--- /dev/null
+++ b/scene_manipulation_msgs/srv/ManipulateScene.srv
@@ -0,0 +1,23 @@
+# Request
+
+# What to do with the frame [update(this will add the frame if it doesn't exist), remove]
+string command
+
+# Name of frame to add to the scene
+string child_frame_id
+
+# Desired frame to put the child frame in
+string parent_frame_id
+
+# child_frame_id expressed in parent_frame_id
+geometry_msgs/Transform transform
+
+# If same_position is true, the position in world will be the same after change of parent
+# If it is true, the transform will be ignored.
+bool same_position_in_world
+
+---
+
+# Reply
+bool success
+string info
\ No newline at end of file
diff --git a/scene_manipulation_service/.gitignore b/scene_manipulation_service/.gitignore
new file mode 100644
index 0000000..6748d11
--- /dev/null
+++ b/scene_manipulation_service/.gitignore
@@ -0,0 +1,24 @@
+**/target/
+big_files/
+
+# These are backup files generated by rustfmt
+**/*.rs.bk
+
+**/build/
+**/install/
+**/log/
+
+
+# Python stuff
+**/__pycache__/
+**/*.py[cod]
+
+# MAC
+.DS_Store
+
+# local store
+*.sz
+*.bmc
+
+# vscode
+*.vscode
\ No newline at end of file
diff --git a/scene_manipulation_service/CMakeLists.txt b/scene_manipulation_service/CMakeLists.txt
new file mode 100644
index 0000000..edd3bcc
--- /dev/null
+++ b/scene_manipulation_service/CMakeLists.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.5)
+project(scene_manipulation_service)
+
+find_package(ament_cmake REQUIRED)
+
+if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
+ set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
+endif()
+
+include(r2r_cargo.cmake)
+
+# put ros package dependencies here.
+r2r_cargo(rcl # mandatory
+ rcl_action # mandatory
+ rmw_fastrtps_cpp # (needed to build with RMW_IMPLEMENTATION=rmw_fastrtps_cpp)
+ FastRTPS # (needed to build with RMW_IMPLEMENTATION=rmw_fastrtps_cpp)
+ tf2_msgs # to publish TF messages to tf and tf_static
+ std_msgs # the Header message is here
+ geometry_msgs # the Transform and TransformStamped messages are here
+ scene_manipulation_msgs # the TransformLookup and ManipulateScene service msgs
+ )
+
+# install binaries
+install(PROGRAMS
+ ${CMAKE_SOURCE_DIR}/target/release/sms
+ DESTINATION lib/${PROJECT_NAME}
+)
+
+# we need this for ros/colcon
+ament_package()
\ No newline at end of file
diff --git a/scene_manipulation_service/Cargo.lock b/scene_manipulation_service/Cargo.lock
new file mode 100644
index 0000000..a2023dd
--- /dev/null
+++ b/scene_manipulation_service/Cargo.lock
@@ -0,0 +1,909 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 3
+
+[[package]]
+name = "aho-corasick"
+version = "0.7.18"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1e37cfd5e7657ada45f742d6e99ca5788580b5c529dc78faf11ece6dc702656f"
+dependencies = [
+ "memchr",
+]
+
+[[package]]
+name = "ansi_term"
+version = "0.12.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d52a9bb7ec0cf484c551830a7ce27bd20d67eac647e1befb56b0be4ee39a55d2"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "atty"
+version = "0.2.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8"
+dependencies = [
+ "hermit-abi",
+ "libc",
+ "winapi",
+]
+
+[[package]]
+name = "bindgen"
+version = "0.59.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2bd2a9a458e8f4304c52c43ebb0cfbd520289f8379a52e329a38afda99bf8eb8"
+dependencies = [
+ "bitflags",
+ "cexpr",
+ "clang-sys",
+ "clap",
+ "env_logger",
+ "lazy_static",
+ "lazycell",
+ "log",
+ "peeking_take_while",
+ "proc-macro2",
+ "quote",
+ "regex",
+ "rustc-hash",
+ "shlex",
+ "which",
+]
+
+[[package]]
+name = "bitflags"
+version = "1.3.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
+
+[[package]]
+name = "bytes"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c4872d67bab6358e59559027aa3b9157c53d9358c51423c17554809a8858e0f8"
+
+[[package]]
+name = "cexpr"
+version = "0.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766"
+dependencies = [
+ "nom",
+]
+
+[[package]]
+name = "cfg-if"
+version = "1.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
+
+[[package]]
+name = "clang-sys"
+version = "1.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4cc00842eed744b858222c4c9faf7243aafc6d33f92f96935263ef4d8a41ce21"
+dependencies = [
+ "glob",
+ "libc",
+ "libloading",
+]
+
+[[package]]
+name = "clap"
+version = "2.34.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a0610544180c38b88101fecf2dd634b174a62eef6946f84dfc6a7127512b381c"
+dependencies = [
+ "ansi_term",
+ "atty",
+ "bitflags",
+ "strsim",
+ "textwrap",
+ "unicode-width",
+ "vec_map",
+]
+
+[[package]]
+name = "either"
+version = "1.6.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e78d4f1cc4ae33bbfc157ed5d5a5ef3bc29227303d595861deb238fcec4e9457"
+
+[[package]]
+name = "env_logger"
+version = "0.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0b2cf0344971ee6c64c31be0d530793fba457d322dfec2810c453d0ef228f9c3"
+dependencies = [
+ "atty",
+ "humantime",
+ "log",
+ "regex",
+ "termcolor",
+]
+
+[[package]]
+name = "fastrand"
+version = "1.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c3fcf0cee53519c866c09b5de1f6c56ff9d647101f81c1964fa632e148896cdf"
+dependencies = [
+ "instant",
+]
+
+[[package]]
+name = "futures"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f73fe65f54d1e12b726f517d3e2135ca3125a437b6d998caf1962961f7172d9e"
+dependencies = [
+ "futures-channel",
+ "futures-core",
+ "futures-executor",
+ "futures-io",
+ "futures-sink",
+ "futures-task",
+ "futures-util",
+]
+
+[[package]]
+name = "futures-channel"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c3083ce4b914124575708913bca19bfe887522d6e2e6d0952943f5eac4a74010"
+dependencies = [
+ "futures-core",
+ "futures-sink",
+]
+
+[[package]]
+name = "futures-core"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0c09fd04b7e4073ac7156a9539b57a484a8ea920f79c7c675d05d289ab6110d3"
+
+[[package]]
+name = "futures-executor"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9420b90cfa29e327d0429f19be13e7ddb68fa1cccb09d65e5706b8c7a749b8a6"
+dependencies = [
+ "futures-core",
+ "futures-task",
+ "futures-util",
+]
+
+[[package]]
+name = "futures-io"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fc4045962a5a5e935ee2fdedaa4e08284547402885ab326734432bed5d12966b"
+
+[[package]]
+name = "futures-macro"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "33c1e13800337f4d4d7a316bf45a567dbcb6ffe087f16424852d97e97a91f512"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "futures-sink"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "21163e139fa306126e6eedaf49ecdb4588f939600f0b1e770f4205ee4b7fa868"
+
+[[package]]
+name = "futures-task"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "57c66a976bf5909d801bbef33416c41372779507e7a6b3a5e25e4749c58f776a"
+
+[[package]]
+name = "futures-util"
+version = "0.3.21"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d8b7abd5d659d9b90c8cba917f6ec750a74e2dc23902ef9cd4cc8c8b22e6036a"
+dependencies = [
+ "futures-channel",
+ "futures-core",
+ "futures-io",
+ "futures-macro",
+ "futures-sink",
+ "futures-task",
+ "memchr",
+ "pin-project-lite",
+ "pin-utils",
+ "slab",
+]
+
+[[package]]
+name = "getrandom"
+version = "0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "418d37c8b1d42553c93648be529cb70f920d3baf8ef469b74b9638df426e0b4c"
+dependencies = [
+ "cfg-if",
+ "libc",
+ "wasi",
+]
+
+[[package]]
+name = "glam"
+version = "0.20.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3412e74893a912839a67e975aca0c88561e20e5461d2d358a5fa6d3b229fae59"
+
+[[package]]
+name = "glob"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9b919933a397b79c37e33b77bb2aa3dc8eb6e165ad809e58ff75bc7db2e34574"
+
+[[package]]
+name = "heck"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2540771e65fc8cb83cd6e8a237f70c319bd5c29f78ed1084ba5d50eeac86f7f9"
+
+[[package]]
+name = "hermit-abi"
+version = "0.1.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33"
+dependencies = [
+ "libc",
+]
+
+[[package]]
+name = "humantime"
+version = "2.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4"
+
+[[package]]
+name = "instant"
+version = "0.1.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7a5bbe824c507c5da5956355e86a746d82e0e1464f65d862cc5e71da70e94b2c"
+dependencies = [
+ "cfg-if",
+]
+
+[[package]]
+name = "itertools"
+version = "0.10.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a9a9d19fa1e79b6215ff29b9d6880b706147f16e9b1dbb1e4e5947b5b02bc5e3"
+dependencies = [
+ "either",
+]
+
+[[package]]
+name = "itoa"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1aab8fc367588b89dcee83ab0fd66b72b50b72fa1904d7095045ace2b0c81c35"
+
+[[package]]
+name = "lazy_static"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
+
+[[package]]
+name = "lazycell"
+version = "1.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55"
+
+[[package]]
+name = "libc"
+version = "0.2.119"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bf2e165bb3457c8e098ea76f3e3bc9db55f87aa90d52d0e6be741470916aaa4"
+
+[[package]]
+name = "libloading"
+version = "0.7.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "efbc0f03f9a775e9f6aed295c6a1ba2253c5757a9e03d55c6caa46a681abcddd"
+dependencies = [
+ "cfg-if",
+ "winapi",
+]
+
+[[package]]
+name = "lock_api"
+version = "0.4.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "88943dd7ef4a2e5a4bfa2753aaab3013e34ce2533d1996fb18ef591e315e2b3b"
+dependencies = [
+ "scopeguard",
+]
+
+[[package]]
+name = "log"
+version = "0.4.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710"
+dependencies = [
+ "cfg-if",
+]
+
+[[package]]
+name = "memchr"
+version = "2.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "308cc39be01b73d0d18f82a0e7b2a3df85245f84af96fdddc5d202d27e47b86a"
+
+[[package]]
+name = "minimal-lexical"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a"
+
+[[package]]
+name = "mio"
+version = "0.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ba272f85fa0b41fc91872be579b3bbe0f56b792aa361a380eb669469f68dafb2"
+dependencies = [
+ "libc",
+ "log",
+ "miow",
+ "ntapi",
+ "winapi",
+]
+
+[[package]]
+name = "miow"
+version = "0.3.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b9f1c5b025cda876f66ef43a113f91ebc9f4ccef34843000e0adf6ebbab84e21"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "nom"
+version = "7.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1b1d11e1ef389c76fe5b81bcaf2ea32cf88b62bc494e19f493d0b30e7a930109"
+dependencies = [
+ "memchr",
+ "minimal-lexical",
+ "version_check",
+]
+
+[[package]]
+name = "ntapi"
+version = "0.3.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c28774a7fd2fbb4f0babd8237ce554b73af68021b5f695a3cebd6c59bac0980f"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "num_cpus"
+version = "1.13.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "19e64526ebdee182341572e50e9ad03965aa510cd94427a4549448f285e957a1"
+dependencies = [
+ "hermit-abi",
+ "libc",
+]
+
+[[package]]
+name = "once_cell"
+version = "1.9.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "da32515d9f6e6e489d7bc9d84c71b060db7247dc035bbe44eac88cf87486d8d5"
+
+[[package]]
+name = "parking_lot"
+version = "0.12.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "87f5ec2493a61ac0506c0f4199f99070cbe83857b0337006a30f3e6719b8ef58"
+dependencies = [
+ "lock_api",
+ "parking_lot_core",
+]
+
+[[package]]
+name = "parking_lot_core"
+version = "0.9.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "28141e0cc4143da2443301914478dc976a61ffdb3f043058310c70df2fed8954"
+dependencies = [
+ "cfg-if",
+ "libc",
+ "redox_syscall",
+ "smallvec",
+ "windows-sys",
+]
+
+[[package]]
+name = "paste"
+version = "1.0.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0744126afe1a6dd7f394cb50a716dbe086cb06e255e53d8d0185d82828358fb5"
+
+[[package]]
+name = "peeking_take_while"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099"
+
+[[package]]
+name = "pin-project-lite"
+version = "0.2.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e280fbe77cc62c91527259e9442153f4688736748d24660126286329742b4c6c"
+
+[[package]]
+name = "pin-utils"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.36"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c7342d5883fbccae1cc37a2353b09c87c9b0f3afd73f5fb9bba687a1f733b029"
+dependencies = [
+ "unicode-xid",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "864d3e96a899863136fc6e99f3d7cae289dafe43bf2c5ac19b70df7210c0a145"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "r2r"
+version = "0.6.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b7576c8418ebd9e537715acbd6ba391d8b281eeb84ae0d10e9c633b84570e9f5"
+dependencies = [
+ "futures",
+ "lazy_static",
+ "r2r_actions",
+ "r2r_common",
+ "r2r_msg_gen",
+ "r2r_rcl",
+ "retain_mut",
+ "serde",
+ "serde_json",
+ "thiserror",
+ "uuid",
+]
+
+[[package]]
+name = "r2r_actions"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dbd9220633b7292e34d1fa1c959a2b3c412d1e1dbe7f20918c61203f4b7681b5"
+dependencies = [
+ "bindgen",
+ "itertools",
+ "r2r_common",
+ "r2r_msg_gen",
+ "r2r_rcl",
+]
+
+[[package]]
+name = "r2r_common"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "89bc8c0e0fb6c3304ec02d4bc51bb5875215118691ead1bb30fe7424cb82dae5"
+
+[[package]]
+name = "r2r_msg_gen"
+version = "0.3.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7f07dab0be5ad8ae9627654a6803f9ad0219cae2ca2f7184d1f9212147b5539f"
+dependencies = [
+ "bindgen",
+ "heck",
+ "lazy_static",
+ "r2r_common",
+ "r2r_rcl",
+]
+
+[[package]]
+name = "r2r_rcl"
+version = "0.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c635a186ba1e30eb062598f0fe6ebab1d7c59283669b25d2596446287b777d46"
+dependencies = [
+ "bindgen",
+ "itertools",
+ "paste",
+ "r2r_common",
+ "widestring",
+]
+
+[[package]]
+name = "redox_syscall"
+version = "0.2.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8383f39639269cde97d255a32bdb68c047337295414940c68bdd30c2e13203ff"
+dependencies = [
+ "bitflags",
+]
+
+[[package]]
+name = "regex"
+version = "1.5.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d07a8629359eb56f1e2fb1652bb04212c072a87ba68546a04065d525673ac461"
+dependencies = [
+ "aho-corasick",
+ "memchr",
+ "regex-syntax",
+]
+
+[[package]]
+name = "regex-syntax"
+version = "0.6.25"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f497285884f3fcff424ffc933e56d7cbca511def0c9831a7f9b5f6153e3cc89b"
+
+[[package]]
+name = "remove_dir_all"
+version = "0.5.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3acd125665422973a33ac9d3dd2df85edad0f4ae9b00dafb1a05e43a9f5ef8e7"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "retain_mut"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "51dd4445360338dab5116712bee1388dc727991d51969558a8882ab552e6db30"
+
+[[package]]
+name = "rustc-hash"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2"
+
+[[package]]
+name = "ryu"
+version = "1.0.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73b4b750c782965c211b42f022f59af1fbceabdd026623714f104152f1ec149f"
+
+[[package]]
+name = "scene_manipulation_service"
+version = "0.1.0"
+dependencies = [
+ "futures",
+ "glam",
+ "r2r",
+ "serde",
+ "serde_json",
+ "tempfile",
+ "tokio",
+]
+
+[[package]]
+name = "scopeguard"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd"
+
+[[package]]
+name = "serde"
+version = "1.0.136"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ce31e24b01e1e524df96f1c2fdd054405f8d7376249a5110886fb4b658484789"
+dependencies = [
+ "serde_derive",
+]
+
+[[package]]
+name = "serde_derive"
+version = "1.0.136"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "08597e7152fcd306f41838ed3e37be9eaeed2b61c42e2117266a554fab4662f9"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "serde_json"
+version = "1.0.79"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8e8d9fa5c3b304765ce1fd9c4c8a3de2c8db365a5b91be52f186efc675681d95"
+dependencies = [
+ "itoa",
+ "ryu",
+ "serde",
+]
+
+[[package]]
+name = "shlex"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3"
+
+[[package]]
+name = "signal-hook-registry"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e51e73328dc4ac0c7ccbda3a494dfa03df1de2f46018127f60c693f2648455b0"
+dependencies = [
+ "libc",
+]
+
+[[package]]
+name = "slab"
+version = "0.4.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9def91fd1e018fe007022791f865d0ccc9b3a0d5001e01aabb8b40e46000afb5"
+
+[[package]]
+name = "smallvec"
+version = "1.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f2dd574626839106c320a323308629dcb1acfc96e32a8cba364ddc61ac23ee83"
+
+[[package]]
+name = "socket2"
+version = "0.4.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "66d72b759436ae32898a2af0a14218dbf55efde3feeb170eb623637db85ee1e0"
+dependencies = [
+ "libc",
+ "winapi",
+]
+
+[[package]]
+name = "strsim"
+version = "0.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ea5119cdb4c55b55d432abb513a0429384878c15dde60cc77b1c99de1a95a6a"
+
+[[package]]
+name = "syn"
+version = "1.0.86"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8a65b3f4ffa0092e9887669db0eae07941f023991ab58ea44da8fe8e2d511c6b"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-xid",
+]
+
+[[package]]
+name = "tempfile"
+version = "3.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5cdb1ef4eaeeaddc8fbd371e5017057064af0911902ef36b39801f67cc6d79e4"
+dependencies = [
+ "cfg-if",
+ "fastrand",
+ "libc",
+ "redox_syscall",
+ "remove_dir_all",
+ "winapi",
+]
+
+[[package]]
+name = "termcolor"
+version = "1.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2dfed899f0eb03f32ee8c6a0aabdb8a7949659e3466561fc0adf54e26d88c5f4"
+dependencies = [
+ "winapi-util",
+]
+
+[[package]]
+name = "textwrap"
+version = "0.11.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d326610f408c7a4eb6f51c37c330e496b08506c9457c9d34287ecc38809fb060"
+dependencies = [
+ "unicode-width",
+]
+
+[[package]]
+name = "thiserror"
+version = "1.0.30"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "854babe52e4df1653706b98fcfc05843010039b406875930a70e4d9644e5c417"
+dependencies = [
+ "thiserror-impl",
+]
+
+[[package]]
+name = "thiserror-impl"
+version = "1.0.30"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "aa32fd3f627f367fe16f893e2597ae3c05020f8bba2666a4e6ea73d377e5714b"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "tokio"
+version = "1.17.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2af73ac49756f3f7c01172e34a23e5d0216f6c32333757c2c61feb2bbff5a5ee"
+dependencies = [
+ "bytes",
+ "libc",
+ "memchr",
+ "mio",
+ "num_cpus",
+ "once_cell",
+ "parking_lot",
+ "pin-project-lite",
+ "signal-hook-registry",
+ "socket2",
+ "tokio-macros",
+ "winapi",
+]
+
+[[package]]
+name = "tokio-macros"
+version = "1.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b557f72f448c511a979e2564e55d74e6c4432fc96ff4f6241bc6bded342643b7"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "unicode-width"
+version = "0.1.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3ed742d4ea2bd1176e236172c8429aaf54486e7ac098db29ffe6529e0ce50973"
+
+[[package]]
+name = "unicode-xid"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8ccb82d61f80a663efe1f787a51b16b5a51e3314d6ac365b08639f52387b33f3"
+
+[[package]]
+name = "uuid"
+version = "0.8.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bc5cf98d8186244414c848017f0e2676b3fcb46807f6668a97dfe67359a3c4b7"
+dependencies = [
+ "getrandom",
+ "serde",
+]
+
+[[package]]
+name = "vec_map"
+version = "0.8.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191"
+
+[[package]]
+name = "version_check"
+version = "0.9.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f"
+
+[[package]]
+name = "wasi"
+version = "0.10.2+wasi-snapshot-preview1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fd6fbd9a79829dd1ad0cc20627bf1ed606756a7f77edff7b66b7064f9cb327c6"
+
+[[package]]
+name = "which"
+version = "4.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2a5a7e487e921cf220206864a94a89b6c6905bfc19f1057fa26a4cb360e5c1d2"
+dependencies = [
+ "either",
+ "lazy_static",
+ "libc",
+]
+
+[[package]]
+name = "widestring"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "17882f045410753661207383517a6f62ec3dbeb6a4ed2acce01f0728238d1983"
+
+[[package]]
+name = "winapi"
+version = "0.3.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419"
+dependencies = [
+ "winapi-i686-pc-windows-gnu",
+ "winapi-x86_64-pc-windows-gnu",
+]
+
+[[package]]
+name = "winapi-i686-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6"
+
+[[package]]
+name = "winapi-util"
+version = "0.1.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "winapi-x86_64-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
+
+[[package]]
+name = "windows-sys"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3df6e476185f92a12c072be4a189a0210dcdcf512a1891d6dff9edb874deadc6"
+dependencies = [
+ "windows_aarch64_msvc",
+ "windows_i686_gnu",
+ "windows_i686_msvc",
+ "windows_x86_64_gnu",
+ "windows_x86_64_msvc",
+]
+
+[[package]]
+name = "windows_aarch64_msvc"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d8e92753b1c443191654ec532f14c199742964a061be25d77d7a96f09db20bf5"
+
+[[package]]
+name = "windows_i686_gnu"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6a711c68811799e017b6038e0922cb27a5e2f43a2ddb609fe0b6f3eeda9de615"
+
+[[package]]
+name = "windows_i686_msvc"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "146c11bb1a02615db74680b32a68e2d61f553cc24c4eb5b4ca10311740e44172"
+
+[[package]]
+name = "windows_x86_64_gnu"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c912b12f7454c6620635bbff3450962753834be2a594819bd5e945af18ec64bc"
+
+[[package]]
+name = "windows_x86_64_msvc"
+version = "0.32.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "504a2476202769977a040c6364301a3f65d0cc9e3fb08600b2bda150a0488316"
diff --git a/scene_manipulation_service/Cargo.toml b/scene_manipulation_service/Cargo.toml
new file mode 100644
index 0000000..38aa825
--- /dev/null
+++ b/scene_manipulation_service/Cargo.toml
@@ -0,0 +1,19 @@
+[package]
+name = "scene_manipulation_service"
+version = "0.1.0"
+authors = ["Endre Erős "]
+edition = "2021"
+autotests = false
+
+[dependencies]
+r2r = "0.6.2"
+futures = "0.3.15"
+tokio = { version = "1", features = ["full"] }
+serde = "1.0.136"
+serde_json = "1.0.79"
+tempfile = "3.3.0"
+glam = "0.20.3"
+
+[[bin]]
+name = "sms"
+path = "src/sms.rs"
\ No newline at end of file
diff --git a/scene_manipulation_service/package.xml b/scene_manipulation_service/package.xml
new file mode 100644
index 0000000..5efc439
--- /dev/null
+++ b/scene_manipulation_service/package.xml
@@ -0,0 +1,34 @@
+
+
+
+ scene_manipulation_service
+ 0.0.1
+ Scene Manipulation Service
+ Endre Erős
+ no_license
+ Endre Erős
+
+ ament_cmake
+
+ rcl
+ rcl_action
+ rmw_fastrtps_cpp
+ FastRTPS
+ tf2_msgs
+ std_msgs
+ geometry_msgs
+ scene_manipulation_msgs
+
+ rcl
+ rcl_action
+ rmw_fastrtps_cpp
+ FastRTPS
+ tf2_msgs
+ std_msgs
+ geometry_msgs
+ scene_manipulation_msgs
+
+
+ ament_cmake
+
+
diff --git a/scene_manipulation_service/r2r_cargo.cmake b/scene_manipulation_service/r2r_cargo.cmake
new file mode 100644
index 0000000..6fcbeaa
--- /dev/null
+++ b/scene_manipulation_service/r2r_cargo.cmake
@@ -0,0 +1,87 @@
+# the "recursive" dependencies already defined don't seem to include all
+# packages. sigh. so here we traverse the dependencies manually
+# instead. we also keep track of which packages that contain idl files.
+function(get_idl_deps OUT_INC OUT_LIB OUT_PKGS PKG)
+ find_package(${PKG} REQUIRED)
+ list(APPEND VISITED_TARGETS ${PKG})
+
+ set(INCS "${${PKG}_INCLUDE_DIRS}")
+ set(LIBS "${${PKG}_LIBRARIES}")
+
+ set(PKGS "")
+ list(APPEND PKGS ${PKG})
+ if(DEFINED ${PKG}_IDL_FILES)
+ list(APPEND PKGS ${PKG})
+ endif()
+ foreach(LIB ${${PKG}_DEPENDENCIES})
+ list(FIND VISITED_TARGETS ${LIB} VISITED)
+ if (${VISITED} EQUAL -1)
+ get_idl_deps(NEW_INCS NEW_LIBS NEW_PKGS ${LIB})
+ list(APPEND INCS ${NEW_INCS})
+ list(APPEND LIBS ${NEW_LIBS})
+ list(APPEND PKGS ${NEW_PKGS})
+ list(REMOVE_DUPLICATES INCS)
+ list(REMOVE_DUPLICATES LIBS)
+ list(REMOVE_DUPLICATES PKGS)
+ endif()
+ endforeach()
+ set(VISITED_TARGETS ${VISITED_TARGETS} PARENT_SCOPE)
+ set(${OUT_INC} ${INCS} PARENT_SCOPE)
+ set(${OUT_LIB} ${LIBS} PARENT_SCOPE)
+ set(${OUT_PKGS} ${PKGS} PARENT_SCOPE)
+endfunction()
+
+function(r2r_cargo)
+ foreach(f ${ARGN})
+ find_package(${f} REQUIRED)
+ set(REC_INC "")
+ set(REC_LIB "")
+ set(REC_IDL_PKGS "")
+ get_idl_deps(REC_INC REC_LIB REC_IDL_PKGS ${f})
+ list(APPEND CMAKE_INCLUDE_DIRS ${REC_INC})
+ list(APPEND CMAKE_LIBRARIES ${REC_LIB})
+ list(APPEND CMAKE_IDL_PACKAGES "${REC_IDL_PKGS}")
+ endforeach()
+ list(REMOVE_DUPLICATES CMAKE_INCLUDE_DIRS)
+ string (REPLACE ";" ":" CMAKE_INCLUDE_DIRS_STR "${CMAKE_INCLUDE_DIRS}")
+ set(ENV{CMAKE_INCLUDE_DIRS} ${CMAKE_INCLUDE_DIRS_STR})
+ list(REMOVE_DUPLICATES CMAKE_LIBRARIES)
+
+ # On OSX colcon eats the DYLD_LIBRARY_PATH... so we need to add the rpaths
+ # manually...
+ set(RUSTFLAGS "")
+ foreach(p ${CMAKE_LIBRARIES})
+ get_filename_component(_parent "${p}" DIRECTORY)
+ if(IS_DIRECTORY ${_parent})
+ list(APPEND RUSTFLAGS "-C link-arg=-Wl,-rpath,${_parent}")
+ endif()
+ endforeach()
+ list(REMOVE_DUPLICATES RUSTFLAGS)
+
+ string (REPLACE ";" " " RUSTFLAGS_STR "${RUSTFLAGS}")
+ set(ENV{RUSTFLAGS} ${RUSTFLAGS_STR})
+
+ string (REPLACE ":" "+" CMAKE_LIBRARIES_STR "${CMAKE_LIBRARIES}")
+ string (REPLACE ";" ":" CMAKE_LIBRARIES_STR "${CMAKE_LIBRARIES_STR}")
+ set(ENV{CMAKE_LIBRARIES} "${CMAKE_LIBRARIES_STR}")
+ list(REMOVE_DUPLICATES CMAKE_IDL_PACKAGES)
+ string (REPLACE ";" ":" CMAKE_IDL_PACKAGES_STR "${CMAKE_IDL_PACKAGES}")
+ set(ENV{CMAKE_IDL_PACKAGES} ${CMAKE_IDL_PACKAGES_STR})
+
+ # custom target for building using cargo
+ option(CARGO_CLEAN "Invoke cargo clean before building" OFF)
+ if(CARGO_CLEAN)
+ add_custom_target(cargo_target ALL
+ COMMAND ${CMAKE_COMMAND} "-E" "env" "cargo" "clean"
+ COMMAND ${CMAKE_COMMAND} "-E" "env" "RUSTFLAGS=$ENV{RUSTFLAGS}" "CMAKE_INCLUDE_DIRS=$ENV{CMAKE_INCLUDE_DIRS}" "CMAKE_LIBRARIES=$ENV{CMAKE_LIBRARIES}" "CMAKE_IDL_PACKAGES=$ENV{CMAKE_IDL_PACKAGES}" "cargo" "build" "--release"
+ WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+ )
+ else()
+ add_custom_target(cargo_target ALL
+ COMMAND ${CMAKE_COMMAND} "-E" "env" "RUSTFLAGS=$ENV{RUSTFLAGS}" "CMAKE_INCLUDE_DIRS=$ENV{CMAKE_INCLUDE_DIRS}" "CMAKE_LIBRARIES=$ENV{CMAKE_LIBRARIES}" "CMAKE_IDL_PACKAGES=$ENV{CMAKE_IDL_PACKAGES}" "cargo" "build" "--release"
+ WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+ )
+ endif(CARGO_CLEAN)
+ unset(CARGO_CLEAN CACHE)
+
+endfunction()
diff --git a/scene_manipulation_service/src/sms.rs b/scene_manipulation_service/src/sms.rs
new file mode 100644
index 0000000..6cfee79
--- /dev/null
+++ b/scene_manipulation_service/src/sms.rs
@@ -0,0 +1,1208 @@
+use futures::{stream::Stream, StreamExt};
+use glam::{DAffine3, DQuat, DVec3};
+use r2r::builtin_interfaces::msg::Time;
+use r2r::geometry_msgs::msg::{Quaternion, Transform, TransformStamped, Vector3};
+use r2r::scene_manipulation_msgs::srv::{LookupTransform, ManipulateScene};
+use r2r::std_msgs::msg::Header;
+use r2r::tf2_msgs::msg::TFMessage;
+use r2r::{ParameterValue, QosProfile, ServiceRequest, log_warn};
+use serde::{Deserialize, Serialize};
+use std::collections::{HashMap, HashSet};
+use std::fs::{self, File};
+use std::io::BufReader;
+use std::sync::{Arc, Mutex};
+
+pub static NODE_ID: &'static str = "scene_manipulation_service";
+pub static FOLDER_RELOAD_SCENE_RATE: u64 = 3000;
+pub static STATIC_BROADCAST_RATE: u64 = 1000;
+pub static ACTIVE_BROADCAST_RATE: u64 = 100;
+pub static BUFFER_MAINTAIN_RATE: u64 = 100;
+pub static ACTIVE_FRAME_LIFETIME: i32 = 3; //seconds
+pub static MAX_TRANSFORM_CHAIN: u64 = 100;
+
+#[derive(Debug, Serialize, Deserialize, Clone, PartialEq)]
+pub struct FrameData {
+ pub parent_frame_id: String,
+ pub child_frame_id: String,
+ pub transform: Transform,
+ pub active: bool,
+}
+
+// differentiate frames loaded through service call or loaded from folder
+#[derive(Debug, Serialize, Deserialize, Clone, PartialEq)]
+pub struct ExtendedFrameData {
+ pub frame_data: FrameData,
+ pub folder_loaded: bool,
+ pub time_stamp: Time,
+}
+
+// the testing module
+mod tests;
+
+#[tokio::main]
+async fn main() -> Result<(), Box> {
+ // setup the node
+ let ctx = r2r::Context::create()?;
+ let mut node = r2r::Node::create(ctx, NODE_ID, "")?;
+
+ // handle parameters passed on from the launch files
+ let params = node.params.clone();
+ let params_things = params.lock().unwrap(); // OK to panic
+ let scenario_path = params_things.get("scenario_path");
+
+ let path_param = match scenario_path {
+ Some(p) => match p {
+ ParameterValue::String(path_param) => path_param.clone(),
+ _ => {
+ r2r::log_warn!(
+ NODE_ID,
+ "Parameter 'scenario_path' has to be of type String. Empty scenario will be launched."
+ );
+ "".to_string()
+ }
+ },
+ None => {
+ r2r::log_warn!(
+ NODE_ID,
+ "Parameter 'scenario_path' not specified. Empty scenario will be launched."
+ );
+ "".to_string()
+ }
+ };
+
+ let path = Arc::new(Mutex::new(path_param.clone()));
+ let scenario = list_frames_in_dir(&path_param).await;
+
+ // TODO: offer a service to load the scenario, i.e. if sms was launched without the specified folder
+ // TODO: offer a service to get all frames from tf
+
+ let init_loaded = load_scenario(&scenario).await;
+ r2r::log_info!(
+ NODE_ID,
+ "Initial frames added to the scene: '{:?}'.",
+ init_loaded.keys()
+ );
+
+ // frames that are published by this broadcaster
+ let broadcaster_frames = Arc::new(Mutex::new(init_loaded));
+
+ // frames that exist on the tf and tf_static topic, i.e. this is a local tf buffer
+ let buffered_frames = Arc::new(Mutex::new(HashMap::::new()));
+
+ // listen to the active frames on the tf topic to see if a frame exists before broadcasting it
+ let active_tf_listener = node.subscribe::("tf", QosProfile::default())?;
+
+ // listen to the active frames on the tf topic to see if a frame exists before broadcasting it
+ let static_tf_listener = node.subscribe::(
+ "tf_static",
+ QosProfile::transient_local(QosProfile::default()),
+ )?;
+
+ // publish the static frames to tf_static
+ let static_pub_timer =
+ node.create_wall_timer(std::time::Duration::from_millis(STATIC_BROADCAST_RATE))?;
+ let static_frame_broadcaster = node.create_publisher::(
+ "tf_static",
+ QosProfile::transient_local(QosProfile::default()),
+ )?;
+
+ // publish the active frames to tf
+ let active_pub_timer =
+ node.create_wall_timer(std::time::Duration::from_millis(ACTIVE_BROADCAST_RATE))?;
+ let active_frame_broadcaster = node
+ .create_publisher::("tf", QosProfile::transient_local(QosProfile::default()))?;
+
+ let scene_manipulation_service =
+ node.create_service::("manipulate_scene")?;
+
+ let transform_lookup_service =
+ node.create_service::("lookup_transform")?;
+
+ // occasionally look into the folder specified by the path to see if there are changes
+ let reload_timer =
+ node.create_wall_timer(std::time::Duration::from_millis(FOLDER_RELOAD_SCENE_RATE))?;
+
+ // spawn a tokio task to handle publishing static frames
+ let broadcaster_frames_clone_1 = broadcaster_frames.clone();
+ tokio::task::spawn(async move {
+ match static_frame_broadcaster_callback(
+ static_frame_broadcaster,
+ static_pub_timer,
+ &broadcaster_frames_clone_1,
+ )
+ .await
+ {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Static frame broadcaster failed with: '{}'.", e),
+ };
+ });
+
+ // spawn a tokio task to handle publishing active frames
+ let broadcaster_frames_clone_2 = broadcaster_frames.clone();
+ tokio::task::spawn(async move {
+ match active_frame_broadcaster_callback(
+ active_frame_broadcaster,
+ active_pub_timer,
+ &broadcaster_frames_clone_2,
+ )
+ .await
+ {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Active frame broadcaster failed with: '{}'.", e),
+ };
+ });
+
+ // spawn a tokio task to handle reloading the scene when a new frame is to be added manually
+ let broadcaster_frames_clone_3 = broadcaster_frames.clone();
+ let path_clone_1 = path.clone();
+ tokio::task::spawn(async move {
+ match folder_manupulation_callback(reload_timer, &broadcaster_frames_clone_3, &path_clone_1)
+ .await
+ {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Active frame broadcaster failed with: '{}'.", e),
+ };
+ });
+
+ // offer a service to manipulate the scene. This means adding (loading), updating, and removing frames through a service call.
+ let broadcaster_frames_clone_4 = broadcaster_frames.clone();
+ let buffered_frames_clone_1 = buffered_frames.clone();
+ tokio::task::spawn(async move {
+ let result = scene_manipulation_server(
+ scene_manipulation_service,
+ &broadcaster_frames_clone_4,
+ &buffered_frames_clone_1,
+ )
+ .await;
+ match result {
+ Ok(()) => r2r::log_info!(NODE_ID, "Scene Manipulation Service call succeeded."),
+ Err(e) => r2r::log_error!(
+ NODE_ID,
+ "Scene Manipulation Service call failed with: {}.",
+ e
+ ),
+ };
+ });
+
+ // offer a service for clients that want to lookup the transform between two frames from the local buffer
+ let buffered_frames_clone_2 = buffered_frames.clone();
+ tokio::task::spawn(async move {
+ // std::thread::sleep(std::time::Duration::from_millis(2000));
+ let result =
+ transform_lookup_server(transform_lookup_service, &buffered_frames_clone_2).await;
+ match result {
+ Ok(()) => r2r::log_info!(NODE_ID, "Transform Lookup Service call succeeded."),
+ Err(e) => r2r::log_error!(NODE_ID, "Transform Lookup Service call failed with: {}.", e),
+ };
+ });
+
+ // spawn a tokio task to listen to the active frames and add them to the buffer
+ let buffered_frames_clone_2 = buffered_frames.clone();
+ tokio::task::spawn(async move {
+ match active_tf_listener_callback(active_tf_listener, &buffered_frames_clone_2).await {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Active tf listener failed with: '{}'.", e),
+ };
+ });
+
+ // spawn a tokio task to listen to the active frames and add them to the buffer
+ let buffered_frames_clone_2 = buffered_frames.clone();
+ tokio::task::spawn(async move {
+ match static_tf_listener_callback(static_tf_listener, &buffered_frames_clone_2).await {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Static tf listener failed with: '{}'.", e),
+ };
+ });
+
+ let buffer_maintain_timer =
+ node.create_wall_timer(std::time::Duration::from_millis(BUFFER_MAINTAIN_RATE))?;
+
+ // spawn a tokio task to maintain the buffer by removing stale active frames
+ let buffered_frames_clone_3 = buffered_frames.clone();
+ tokio::task::spawn(async move {
+ match maintain_buffer(buffer_maintain_timer, &buffered_frames_clone_3).await {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(NODE_ID, "Buffer maintainer failed with: '{}'.", e),
+ };
+ });
+
+ // keep the node alive
+ let handle = std::thread::spawn(move || loop {
+ node.spin_once(std::time::Duration::from_millis(1000));
+ });
+
+ r2r::log_warn!(NODE_ID, "Node started.");
+
+ handle.join().unwrap();
+
+ Ok(())
+}
+
+async fn list_frames_in_dir(path: &str) -> Vec {
+ let mut scenario = vec![];
+ match fs::read_dir(path) {
+ Ok(dir) => dir.for_each(|file| match file {
+ Ok(entry) => match entry.path().to_str() {
+ Some(valid) => scenario.push(valid.to_string()),
+ None => r2r::log_warn!(NODE_ID, "Path is not valid unicode."),
+ },
+ Err(e) => r2r::log_warn!(NODE_ID, "Reading entry failed with '{}'.", e),
+ }),
+ Err(e) => {
+ println!("{:?}", path);
+ r2r::log_error!(
+ NODE_ID,
+ "Reading the scenario directory failed with: '{}'. Empty scenario will be launched.",
+ e
+ );
+ }
+ }
+ scenario
+}
+
+async fn load_scenario(scenario: &Vec) -> HashMap {
+ let mut frame_datas: HashMap = HashMap::new();
+ scenario.iter().for_each(|x| match File::open(x) {
+ Ok(file) => {
+ let reader = BufReader::new(file);
+ match serde_json::from_reader(reader) {
+ Ok(jsonl) => {
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+ let json = ExtendedFrameData {
+ frame_data: jsonl,
+ folder_loaded: true,
+ time_stamp,
+ };
+ frame_datas.insert(json.frame_data.child_frame_id.clone(), json.clone());
+ }
+ Err(e) => r2r::log_warn!(NODE_ID, "Serde failed with: '{}'.", e),
+ }
+ }
+ Err(e) => r2r::log_warn!(NODE_ID, "Opening json file failed with: '{}'.", e),
+ });
+ frame_datas
+}
+
+async fn scene_manipulation_server(
+ mut service: impl Stream- > + Unpin,
+ broadcaster_frames: &Arc>>,
+ buffered_frames: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ match service.next().await {
+ Some(request) => match request.message.command.as_str() {
+ "update" => {
+ r2r::log_info!(NODE_ID, "Got 'update' request: {:?}.", request.message);
+ let response =
+ update_frame(&request.message, &broadcaster_frames, &buffered_frames).await;
+ request
+ .respond(response)
+ .expect("Could not send service response.");
+ continue;
+ }
+ "remove" => {
+ r2r::log_info!(NODE_ID, "Got 'remove' request: {:?}.", request.message);
+ let response =
+ remove_frame(&request.message, &broadcaster_frames, &buffered_frames).await;
+ request
+ .respond(response)
+ .expect("Could not send service response.");
+ continue;
+ }
+ _ => {
+ r2r::log_error!(NODE_ID, "No such command.");
+ continue;
+ }
+ },
+ None => {}
+ }
+ }
+}
+
+async fn transform_lookup_server(
+ mut service: impl Stream
- > + Unpin,
+ buffered_frames: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ match service.next().await {
+ Some(request) => match lookup_transform(
+ &request.message.parent_frame_id,
+ &request.message.child_frame_id,
+ &buffered_frames,
+ )
+ .await
+ {
+ Some(transform) => {
+ r2r::log_info!(
+ NODE_ID,
+ "Found transform from '{}' to '{}': {:?}",
+ &request.message.parent_frame_id,
+ &request.message.child_frame_id,
+ transform
+ );
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+ let response = LookupTransform::Response {
+ success: true,
+ transform: TransformStamped {
+ header: Header {
+ stamp: time_stamp,
+ frame_id: request.message.parent_frame_id.clone(),
+ },
+ child_frame_id: request.message.child_frame_id.clone(),
+ transform,
+ },
+ };
+ request
+ .respond(response)
+ .expect("Could not send service response.");
+ continue;
+ }
+ None => {
+ r2r::log_warn!(
+ NODE_ID,
+ "Failed to lookup transform from '{}' to '{}'.",
+ &request.message.parent_frame_id,
+ &request.message.child_frame_id,
+ );
+ let response = LookupTransform::Response {
+ success: false,
+ ..Default::default()
+ };
+ request
+ .respond(response)
+ .expect("Could not send service response.");
+ continue;
+ }
+ },
+ None => {}
+ }
+ }
+}
+
+fn error_response(msg: &str) -> ManipulateScene::Response {
+ let info = msg.to_string();
+ r2r::log_error!(NODE_ID, "{}", info);
+ ManipulateScene::Response {
+ success: false,
+ info,
+ }
+}
+
+fn success_response(msg: &str) -> ManipulateScene::Response {
+ let info = msg.to_string();
+ r2r::log_info!(NODE_ID, "{}", info);
+ ManipulateScene::Response {
+ success: true,
+ info,
+ }
+}
+
+async fn update_frame(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ buffered_frames: &Arc>>,
+) -> ManipulateScene::Response {
+ let local_buffered_frames = buffered_frames.lock().unwrap().clone();
+ let local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+
+ fn make_extended_frame_data(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ ) -> ExtendedFrameData {
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+ ExtendedFrameData {
+ frame_data: FrameData {
+ parent_frame_id: message.parent_frame_id.clone(),
+ child_frame_id: message.child_frame_id.clone(),
+ transform: message.transform.clone(),
+ active: true,
+ },
+ folder_loaded: false,
+ time_stamp,
+ }
+ }
+
+ fn add_with_msg_tf(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ ) -> ManipulateScene::Response {
+ let mut local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+ local_broadcaster_frames.insert(
+ message.child_frame_id.clone(),
+ make_extended_frame_data(message),
+ );
+ *broadcaster_frames.lock().unwrap() = local_broadcaster_frames;
+ success_response(&format!(
+ "Frame '{}' added to the scene.",
+ message.child_frame_id
+ ))
+ }
+
+ fn add_with_lookup_tf(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ transform: Transform,
+ ) -> ManipulateScene::Response {
+ let mut local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+ local_broadcaster_frames.insert(
+ message.child_frame_id.clone(),
+ ExtendedFrameData {
+ frame_data: FrameData {
+ parent_frame_id: message.parent_frame_id.clone(),
+ child_frame_id: message.child_frame_id.clone(),
+ transform: transform,
+ active: true,
+ },
+ folder_loaded: false,
+ time_stamp,
+ },
+ );
+ *broadcaster_frames.lock().unwrap() = local_broadcaster_frames;
+ success_response(&format!(
+ "Frame '{}' added to the scene.",
+ message.child_frame_id
+ ))
+ }
+
+ async fn inner(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ buffered_frames: &Arc>>,
+ ) -> ManipulateScene::Response {
+ let local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+ let local_buffered_frames = buffered_frames.lock().unwrap().clone();
+
+ match local_broadcaster_frames.get(&message.child_frame_id) {
+ Some(frame) => match frame.frame_data.active {
+ false => match local_buffered_frames.contains_key(&message.parent_frame_id) {
+ false => {
+ // log_warn!()
+ add_with_msg_tf(message, broadcaster_frames)
+ }, //add warning
+ true => match check_would_produce_cycle(
+ &make_extended_frame_data(&message),
+ &local_buffered_frames,
+ ) {
+ (false, _) => match message.same_position_in_world {
+ false => add_with_msg_tf(message, broadcaster_frames),
+ true => match lookup_transform(
+ &message.parent_frame_id,
+ &message.child_frame_id,
+ &buffered_frames,
+ )
+ .await
+ {
+ None => error_response("Frailed to lookup transform."),
+ Some(transform) => {
+ add_with_lookup_tf(message, broadcaster_frames, transform)
+ }
+ },
+ },
+ (true, cause) => error_response(&format!(
+ "Adding frame '{}' would produce a cycle. Not added",
+ cause
+ )),
+ },
+ },
+ true => error_response("Can't manipulate_static frames."),
+ },
+ None => error_response("Failed to get frame data from broadcaster hashmap."),
+ }
+ }
+
+ match &message.child_frame_id == "world" {
+ false => match local_buffered_frames.contains_key(&message.child_frame_id) {
+ false => match local_broadcaster_frames.contains_key(&message.child_frame_id) {
+ false => match local_buffered_frames.contains_key(&message.parent_frame_id) {
+ false => add_with_msg_tf(message, broadcaster_frames), //add warning
+ true => match check_would_produce_cycle(&make_extended_frame_data(&message), &local_buffered_frames) {
+ (false, _) => add_with_msg_tf(message, broadcaster_frames),
+ (true, cause) => error_response(&format!("Adding frame '{}' would produce a cycle. Not added", cause))
+ }
+ }
+ true => {
+ std::thread::sleep(std::time::Duration::from_millis(2000));
+ match local_buffered_frames.contains_key(&message.child_frame_id) {
+ false => error_response("Frame doesn't exist in tf, but it is published by this broadcaster? Investigate."),
+ true => inner(message, broadcaster_frames, buffered_frames).await
+ }
+ }
+ },
+ true => match local_broadcaster_frames.contains_key(&message.child_frame_id) {
+ false => error_response("Frame exists in the tf, but it can't be updated since it is not published by this broadcaster."),
+ true => inner(message, broadcaster_frames, buffered_frames).await
+ }
+ },
+ true => error_response("Frame 'world' is reserved as the universal tree root."),
+ }
+}
+
+async fn remove_frame(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ buffered_frames: &Arc>>,
+) -> ManipulateScene::Response {
+ let local_buffered_frames = buffered_frames.lock().unwrap().clone();
+ let local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+
+ fn inner(
+ message: &r2r::scene_manipulation_msgs::srv::ManipulateScene::Request,
+ broadcaster_frames: &Arc>>,
+ ) -> ManipulateScene::Response {
+ let mut local_broadcaster_frames = broadcaster_frames.lock().unwrap().clone();
+ match local_broadcaster_frames.get(&message.child_frame_id) {
+ Some(frame) => match frame.frame_data.active {
+ false => match frame.folder_loaded {
+ false => match local_broadcaster_frames.remove(&message.child_frame_id) {
+ Some(_) => {
+ *broadcaster_frames.lock().unwrap() = local_broadcaster_frames;
+ success_response(&format!("Frame '{}' removed from the scene.", message.child_frame_id))
+ }
+ None => error_response(&format!(
+ "Failed to remove frame '{}' from the scene.",
+ message.child_frame_id
+ ))
+ },
+ true => error_response("Frame is loaded via the folder and not manipulated afterwards. Frame won't be removed.")
+ },
+ true => error_response("Can't manipulate_static frames.")
+ },
+ None => error_response("Failed to get frame data from broadcaster hashmap.")
+ }
+ }
+
+ match &message.child_frame_id == "world" {
+ false => match local_buffered_frames.contains_key(&message.child_frame_id) {
+ false => match local_broadcaster_frames.contains_key(&message.child_frame_id) {
+ false => error_response("Frame doesn't exist in tf, nor is it published by this broadcaster."),
+ true => {
+ std::thread::sleep(std::time::Duration::from_millis(2000));
+ match local_buffered_frames.contains_key(&message.child_frame_id) {
+ false => error_response("Frame doesn't exist in tf, but it is published by this broadcaster? Investigate."),
+ true => inner(message, broadcaster_frames)
+ }
+ }
+ },
+ true => match local_broadcaster_frames.contains_key(&message.child_frame_id) {
+ false => error_response("Frame exists in the tf, but it can't be removed since it is not published by this broadcaster."),
+ true => inner(message, broadcaster_frames)
+ }
+ },
+ true => error_response("Frame 'world' is reserved as the universal tree root."),
+ }
+}
+
+// broadcast static frames
+async fn static_frame_broadcaster_callback(
+ publisher: r2r::Publisher,
+ mut timer: r2r::Timer,
+ frames: &Arc>>,
+ // service_loaded: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+
+ let transforms_local = frames.lock().unwrap().clone();
+ let mut updated_transforms = vec![];
+
+ transforms_local
+ .iter()
+ .for_each(|(_, v)| match v.frame_data.active {
+ false => {
+ updated_transforms.push(TransformStamped {
+ header: Header {
+ stamp: time_stamp.clone(),
+ frame_id: v.frame_data.parent_frame_id.clone(),
+ },
+ child_frame_id: v.frame_data.child_frame_id.clone(),
+ transform: v.frame_data.transform.clone(),
+ });
+ }
+ true => (),
+ });
+
+ let msg = TFMessage {
+ transforms: updated_transforms,
+ };
+
+ match publisher.publish(&msg) {
+ Ok(()) => (),
+ Err(e) => {
+ r2r::log_error!(
+ NODE_ID,
+ "Active broadcaster failed to send a message with: '{}'",
+ e
+ );
+ }
+ };
+ timer.tick().await?;
+ }
+}
+
+// broadcast active frames
+async fn active_frame_broadcaster_callback(
+ publisher: r2r::Publisher,
+ mut timer: r2r::Timer,
+ frames: &Arc>>,
+ // service_loaded: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let time_stamp = r2r::Clock::to_builtin_time(&now);
+
+ let transforms_local = frames.lock().unwrap().clone();
+ let mut updated_transforms = vec![];
+
+ transforms_local
+ .iter()
+ .for_each(|(_, v)| match v.frame_data.active {
+ true => {
+ updated_transforms.push(TransformStamped {
+ header: Header {
+ stamp: time_stamp.clone(),
+ frame_id: v.frame_data.parent_frame_id.clone(),
+ },
+ child_frame_id: v.frame_data.child_frame_id.clone(),
+ transform: v.frame_data.transform.clone(),
+ });
+ }
+ false => (),
+ });
+
+ let msg = TFMessage {
+ transforms: updated_transforms,
+ };
+
+ match publisher.publish(&msg) {
+ Ok(()) => (),
+ Err(e) => {
+ r2r::log_error!(
+ NODE_ID,
+ "Active broadcaster failed to send a message with: '{}'",
+ e
+ );
+ }
+ };
+ timer.tick().await?;
+ }
+}
+
+// occasionally look at the scenario folder specified by the path and if there
+// have been changes, add or remove the the frames that have been manually added
+// to the folder or removed from the folder
+async fn folder_manupulation_callback(
+ mut timer: r2r::Timer,
+ frames: &Arc>>,
+ path: &Arc>,
+) -> Result<(), Box> {
+ loop {
+ // let mut to_be_added = vec![];
+ let path = path.lock().unwrap().clone();
+ let dir_frames = list_frames_in_dir(&path).await.clone();
+
+ // differentiate between frames loaded from folder and through services
+ // for example, if we add a frame through the service call, it will be
+ // removed every RELOAD_SCENE_RATE since it is not in the folder
+ let mut folder_scenario = load_scenario(&dir_frames).await;
+ folder_scenario.retain(|_, v| v.folder_loaded);
+ let mut frames_local = frames.lock().unwrap().clone();
+ frames_local.retain(|_, v| v.folder_loaded);
+
+ let folder_vec = folder_scenario
+ .iter()
+ .map(|(k, _)| k.clone())
+ .collect::>();
+ let loaded_vec = frames_local
+ .iter()
+ .map(|(k, _)| k.clone())
+ .collect::>();
+
+ let folder_set: HashSet =
+ HashSet::from_iter(folder_vec.clone().iter().map(|x| x.clone()));
+ let local_set: HashSet =
+ HashSet::from_iter(loaded_vec.clone().iter().map(|x| x.clone()));
+
+ let to_be_added_names: Vec = folder_set
+ .difference(&local_set)
+ .map(|x| x.clone())
+ .collect();
+ let to_be_removed_names: Vec = local_set
+ .difference(&folder_set)
+ .map(|x| x.clone())
+ .collect();
+
+ let mut to_be_added = HashMap::::new();
+ let mut to_be_removed = HashMap::::new();
+ let mut transforms_local = frames.lock().unwrap().clone();
+
+ to_be_added_names.iter().for_each(|name| {
+ match folder_scenario.iter().find(|(json_k, _)| *json_k == name) {
+ Some((data_k, data_v)) => {
+ to_be_added.insert(data_k.to_string(), data_v.clone());
+ }
+ None => (),
+ }
+ });
+
+ to_be_removed_names.iter().for_each(|name| {
+ match frames_local.iter().find(|(json_k, _)| *json_k == name) {
+ Some((data_k, data_v)) => {
+ to_be_removed.insert(data_k.to_string(), data_v.clone());
+ }
+ None => (),
+ }
+ });
+
+ // TODO: check if parent exists in the world
+ // TODO: check if this addition will produce a cycle
+ // TODO: check if it is folder loaded or if that has been changed in the meantime
+
+ to_be_added
+ .iter()
+ .for_each(|(k, v)| match frames_local.get(k) {
+ Some(frame_data) => match frame_data.frame_data.active {
+ false => {
+ let info = format!("Can't manipulate static frame '{}'.", k);
+ r2r::log_error!(NODE_ID, "{}", info);
+ }
+ true => {
+ transforms_local.insert(k.clone(), v.clone());
+ r2r::log_info!(NODE_ID, "Updated active frame: '{}'.", k);
+ }
+ },
+ None => {
+ transforms_local.insert(k.clone(), v.clone());
+ r2r::log_info!(NODE_ID, "Added new frame: '{}'.", k);
+ }
+ });
+
+ to_be_removed
+ .iter()
+ .for_each(|(k, _)| match frames_local.get(k) {
+ Some(frame_data) => match frame_data.frame_data.active {
+ false => {
+ let info = format!("Can't manipulate static frame '{}'.", k);
+ r2r::log_error!(NODE_ID, "{}", info);
+ }
+ true => {
+ transforms_local.remove(k);
+ r2r::log_info!(NODE_ID, "Removed active frame: '{}'.", k);
+ }
+ },
+ None => {
+ let info = format!(
+ "Couldn't find the frame '{}' in this broadcaster. None removed.",
+ k
+ );
+ r2r::log_warn!(NODE_ID, "{}", info);
+ }
+ });
+
+ *frames.lock().unwrap() = transforms_local;
+
+ timer.tick().await?;
+ }
+}
+
+// update the buffer with active frames from the tf topic
+async fn active_tf_listener_callback(
+ mut subscriber: impl Stream
- + Unpin,
+ frames: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ match subscriber.next().await {
+ Some(message) => {
+ let mut frames_local = frames.lock().unwrap().clone();
+ message.transforms.iter().for_each(|t| {
+ // before adding an active frame to the buffer, check if the frame is
+ // already stale as defined with ACTIVE_FRAME_LIFETIME
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let current_time = r2r::Clock::to_builtin_time(&now);
+
+ // match current_time.sec > t.header.stamp.sec + ACTIVE_FRAME_LIFETIME {
+ // false => {
+ frames_local.insert(
+ t.child_frame_id.clone(),
+ ExtendedFrameData {
+ frame_data: FrameData {
+ parent_frame_id: t.header.frame_id.clone(),
+ child_frame_id: t.child_frame_id.clone(),
+ transform: t.transform.clone(),
+ active: true,
+ },
+ folder_loaded: false, // should be option None
+ time_stamp: t.header.stamp.clone(),
+ },
+ );
+ // }
+ // true => (),
+ // }
+ });
+ *frames.lock().unwrap() = frames_local;
+ }
+ None => {
+ r2r::log_error!(NODE_ID, "Subscriber did not get the message?");
+ }
+ }
+ }
+}
+
+// update the buffer with static frames from the tf_static topic
+async fn static_tf_listener_callback(
+ mut subscriber: impl Stream
- + Unpin,
+ frames: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ match subscriber.next().await {
+ Some(message) => {
+ let mut frames_local = frames.lock().unwrap().clone();
+ message.transforms.iter().for_each(|t| {
+ // static frames are always true, so we don't need to check their timestamp
+ frames_local.insert(
+ t.child_frame_id.clone(),
+ ExtendedFrameData {
+ frame_data: FrameData {
+ parent_frame_id: t.header.frame_id.clone(),
+ child_frame_id: t.child_frame_id.clone(),
+ transform: t.transform.clone(),
+ active: false,
+ },
+ folder_loaded: false, // should be option None, but lazy...
+ time_stamp: t.header.stamp.clone(),
+ },
+ );
+ });
+ *frames.lock().unwrap() = frames_local;
+ }
+ None => {
+ r2r::log_error!(NODE_ID, "Subscriber did not get the message?");
+ }
+ }
+ }
+}
+
+// task to remove all stale active frames older than ACTIVE_FRAME_LIFETIME
+// also, if a stale active frame is on the tf for some reason, don't include it
+async fn maintain_buffer(
+ mut timer: r2r::Timer,
+ frames: &Arc>>,
+) -> Result<(), Box> {
+ loop {
+ let frames_local = frames.lock().unwrap().clone();
+ let mut frames_local_reduced = frames_local.clone();
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let current_time = r2r::Clock::to_builtin_time(&now);
+ frames_local.iter().for_each(|(k, v)| {
+ match v.folder_loaded {
+ false => {
+ match v.frame_data.active {
+ true => match current_time.sec > v.time_stamp.sec + ACTIVE_FRAME_LIFETIME {
+ true => {
+ frames_local_reduced.remove(k);
+ }
+ false => (),
+ },
+ false => (),
+ };
+ }
+ true => {
+ match v.frame_data.active {
+ true => match current_time.sec > v.time_stamp.sec + ACTIVE_FRAME_LIFETIME {
+ true => {
+ frames_local_reduced.remove(k);
+ }
+ false => (),
+ },
+ false => ()
+ }
+ }
+ }
+ });
+ *frames.lock().unwrap() = frames_local_reduced;
+ timer.tick().await?;
+ }
+}
+
+// fn quaternion_conjugate(q: &Quaternion) -> Quaternion {
+// Quaternion {
+// x: -q.x,
+// y: -q.y,
+// z: -q.z,
+// w: q.w,
+// }
+// }
+
+// // to apply the rotation of one quaternion to a pose, multiply the previous quaternion of the pose
+// // by the quaternion representing the desired rotation
+// // the order of this multiplication matters
+// fn multiply_quaternion(q1: &Quaternion, q2: &Quaternion) -> Quaternion {
+// Quaternion {
+// x: q2.x * q1.w + q2.y * q1.z - q2.z * q1.y + q2.w * q1.x,
+// y: -q2.x * q1.z + q2.y * q1.w + q2.z * q1.x + q2.w * q1.y,
+// z: q2.x * q1.y - q2.y * q1.x + q2.z * q1.w + q2.w * q1.z,
+// w: -q2.x * q1.x - q2.y * q1.y - q2.z * q1.z + q2.w * q1.w,
+// }
+// }
+
+// // the magnitude of a quaternion should always be one
+// // if numerical errors cause a quaternion magnitude other than one, ROS 2 will print warnings
+// // to avoid these warnings, normalize the quaternion
+// fn normalize_quaternion(q: &Quaternion) -> Quaternion {
+// let norm = (q.x.powi(2) + q.y.powi(2) + q.z.powi(2) + q.w.powi(2)).sqrt();
+// Quaternion {
+// x: q.x / norm,
+// y: q.y / norm,
+// z: q.z / norm,
+// w: q.w / norm,
+// }
+// }
+
+// fn multiply_and_normalize(q1: &Quaternion, q2: &Quaternion) -> Quaternion {
+// normalize_quaternion(&multiply_quaternion(q1, q2))
+// }
+
+// easier to manipulate transforms in glam's affine format
+fn tf_to_affine(t: &Transform) -> DAffine3 {
+ DAffine3::from_rotation_translation(
+ DQuat::from_xyzw(t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w),
+ DVec3::new(t.translation.x, t.translation.y, t.translation.z),
+ )
+}
+
+// back to ros's Transform type when needed
+fn affine_to_tf(a: &DAffine3) -> Transform {
+ match a.to_scale_rotation_translation() {
+ (_, r, t) => Transform {
+ translation: Vector3 {
+ x: t.x,
+ y: t.y,
+ z: t.z,
+ },
+ rotation: Quaternion {
+ x: r.x,
+ y: r.y,
+ z: r.z,
+ w: r.w,
+ },
+ },
+ }
+}
+
+async fn lookup_transform(
+ parent_frame_id: &str,
+ child_frame_id: &str,
+ frames: &Arc>>,
+) -> Option {
+ let frames_local = frames.lock().unwrap().clone();
+ let mut chain = vec![];
+ match is_cyclic_all(&frames_local) {
+ (false, _) => match parent_to_world(parent_frame_id, &frames_local) {
+ Some(up_chain) => match world_to_child(child_frame_id, &frames_local) {
+ Some(down_chain) => {
+ chain.extend(up_chain);
+ chain.extend(down_chain);
+ Some(affine_to_tf(&chain.iter().product::()))
+ }
+ None => None,
+ },
+ None => None,
+ },
+ (true, cause) => None,
+ }
+}
+
+// when looking up a frame in a parent, first we have to find a common parent.
+// we know that the world is always there so we go up until the world frame.
+fn parent_to_world(
+ parent_frame_id: &str,
+ frames: &HashMap,
+) -> Option> {
+ let mut current_parent = parent_frame_id.to_string();
+ let mut path = vec![];
+ let mut lenght = 0;
+ loop {
+ match lenght > MAX_TRANSFORM_CHAIN {
+ false => {
+ lenght = lenght + 1;
+ // let current_parent_2 = current_parent.clone();
+ match frames.get(¤t_parent.clone()) {
+ Some(parent) => {
+ println!("PARENT TO WORLD: {:?}", parent.frame_data.child_frame_id);
+ let a = tf_to_affine(&parent.frame_data.transform);
+ path.push(a.inverse());
+ let p = parent.frame_data.parent_frame_id.clone();
+ match p == "world".to_string() {
+ true => break Some(path),
+ false => {
+ let p = parent.frame_data.parent_frame_id.clone();
+ current_parent = p;
+ continue;
+ }
+ }
+ }
+ None => break None, // log err
+ }
+ }
+ true => {
+ r2r::log_error!(
+ NODE_ID,
+ "TF lookup failed with: 'MAX_TRANSFORM_CHAIN reached'."
+ );
+ break None;
+ }
+ }
+ }
+}
+
+// basically BFS to get the path to the child...
+// even simpler since we check for cycles beforehand and its a tree, so not tracking visited...
+// add max depth though just in case.
+fn world_to_child(
+ child_frame_id: &str,
+ frames: &HashMap,
+) -> Option> {
+ let mut stack = vec![];
+ get_frame_children_3("world", frames)
+ .iter()
+ .for_each(|(k, v)| {
+ stack.push((
+ k.to_string(),
+ vec![k.to_string()],
+ vec![tf_to_affine(&v.transform)],
+ ))
+ });
+ loop {
+ // r2r::log_warn!(
+ // NODE_ID,
+ // "WORLD TO CHILD LOOP."
+ // );
+ println!(
+ "WORLD TO CHILD: {:?}",
+ stack.iter().map(|x| x.0.clone()).collect::>()
+ );
+ match stack.pop() {
+ Some((frame, path, chain)) => match frame == child_frame_id {
+ true => {
+ println!("WORLD TO CHILD PATH: {:?}", path);
+ break Some(chain);
+ }
+ false => {
+ get_frame_children_3(&frame, frames)
+ .iter()
+ .for_each(|(k, v)| {
+ let mut prev_path = path.clone();
+ let mut prev_chain = chain.clone();
+ prev_path.push(k.clone());
+ prev_chain.push(tf_to_affine(&v.transform));
+ stack.insert(0, (k.to_string(), prev_path.clone(), prev_chain.clone()));
+ });
+
+ continue;
+ }
+ },
+ None => break None,
+ };
+ }
+}
+
+// get all children frames of a frame
+// [tested]
+fn get_frame_children(frame: &str, frames: &HashMap) -> Vec {
+ frames
+ .iter()
+ .filter(|(_, v)| frame == v.frame_data.parent_frame_id)
+ .map(|(k, _)| k.clone())
+ .collect()
+}
+
+// get all children frames of a frame as a hashmap
+fn get_frame_children_2(
+ frame: &str,
+ frames: &HashMap,
+) -> HashMap {
+ let mut children = HashMap::::new();
+ frames
+ .iter()
+ .filter(|(_, v)| frame == v.frame_data.parent_frame_id)
+ .for_each(|(k, v)| {
+ children.insert(k.to_string(), v.clone());
+ });
+ children
+}
+
+// get all children frames of a frame
+fn get_frame_children_3(
+ frame: &str,
+ frames: &HashMap,
+) -> Vec<(String, FrameData)> {
+ frames
+ .iter()
+ .filter(|(_, v)| frame == v.frame_data.parent_frame_id)
+ .map(|(k, v)| (k.clone(), v.frame_data.clone()))
+ .collect()
+}
+
+// TODO: should return option. should check also max depth
+// check for cycles in the tree segment starting from this frame
+// [tested]
+fn is_cyclic(frame: &str, frames: &HashMap) -> (bool, String) {
+ let mut stack = vec![];
+ let mut visited = vec![];
+ stack.push(frame.to_string());
+ loop {
+ // r2r::log_warn!(
+ // NODE_ID,
+ // "IS CYCLIC LOOP."
+ // );
+ match stack.pop() {
+ Some(current_frame) => match visited.contains(¤t_frame) {
+ true => break (true, current_frame),
+ false => {
+ visited.push(current_frame.clone());
+ for child in get_frame_children(¤t_frame, frames) {
+ stack.push(child);
+ continue;
+ }
+ }
+ },
+ None => break (false, "".to_string()),
+ }
+ }
+}
+
+// check for all cycles including all frames even if tree is segmented
+// [tested]
+fn is_cyclic_all(frames: &HashMap) -> (bool, String) {
+ for (k, _) in frames {
+ let (cyclic, cause) = is_cyclic(k, frames);
+ match cyclic {
+ true => return (cyclic, cause),
+ false => continue,
+ }
+ }
+ (false, "".to_string())
+}
+
+// check if adding the frame to the tree would produce a cycle
+// [tested]
+fn check_would_produce_cycle(
+ frame: &ExtendedFrameData,
+ frames: &HashMap,
+) -> (bool, String) {
+ let mut frames_local = frames.clone();
+ frames_local.insert(frame.frame_data.child_frame_id.clone(), frame.clone());
+ is_cyclic_all(&frames_local)
+}
diff --git a/scene_manipulation_service/src/tests/buffer.rs b/scene_manipulation_service/src/tests/buffer.rs
new file mode 100644
index 0000000..a9a0508
--- /dev/null
+++ b/scene_manipulation_service/src/tests/buffer.rs
@@ -0,0 +1,278 @@
+#![allow(unused_imports)]
+#![allow(dead_code)]
+use r2r::geometry_msgs::msg::{Quaternion, Transform, Vector3};
+use std::{process::exit, collections::HashMap};
+
+use crate::ExtendedFrameData;
+
+pub static NODE_ID: &'static str = "buffer_test";
+
+// #[tokio::main]
+// async fn main() -> Result<(), Box> {
+// let ctx = r2r::Context::create()?;
+// let mut node = r2r::Node::create(ctx, NODE_ID, "")?;
+
+// let client = node.create_client::("manipulate_scene")?;
+// let waiting = node.is_available(&client)?;
+
+// let handle = std::thread::spawn(move || loop {
+// &node.spin_once(std::time::Duration::from_millis(100));
+// });
+
+// r2r::log_warn!(NODE_ID, "Waiting for Scene Manipulation Service...");
+// waiting.await?;
+// r2r::log_info!(NODE_ID, "Scene Manipulation Service available.");
+// r2r::log_info!(
+// NODE_ID,
+// "Node started."
+// );
+
+// std::thread::sleep(std::time::Duration::from_millis(3000));
+
+// let mut messages = vec![];
+
+// let message_1 = ManipulateScene::Request {
+// command: "update".to_string(),
+// parent_frame: "frame_4".to_string(),
+// child_frame: "frame_2".to_string(),
+// same_position_in_world: true,
+// transform: Transform::default(),
+// };
+// messages.push(message_1);
+
+// let message_2 = ManipulateScene::Request {
+// command: "update".to_string(),
+// parent_frame: "world".to_string(),
+// child_frame: "frame_4".to_string(),
+// same_position_in_world: false,
+// transform: Transform::default(),
+// };
+// messages.push(message_2);
+
+// let message_3 = ManipulateScene::Request {
+// command: "remove".to_string(),
+// parent_frame: "world".to_string(),
+// child_frame: "frame_4".to_string(),
+// same_position_in_world: false,
+// transform: Transform::default(),
+// };
+// messages.push(message_3);
+
+// for message in messages {
+// sms_test(
+// &client,
+// message
+// ).await?;
+// }
+
+// handle.join().unwrap();
+
+// Ok(())
+// }
+
+// async fn sms_test(
+// client: &r2r::Client,
+// message: ManipulateScene::Request,
+// ) -> Result<(), Box> {
+
+// let response = client
+// .request(&message)
+// .expect("Could not send SMS request.")
+// .await
+// .expect("Cancelled.");
+
+// r2r::log_info!("sms_test", "Request to SMS sent.");
+
+// match response.success {
+// true => {
+// r2r::log_info!("sms_test", "Got sms response: {}", response.success);
+// }
+// false => {
+// r2r::log_error!(
+// "sms_test",
+// "Couldn't manipulate scene for command command '{}'.",
+// message.command
+// );
+// }
+// }
+
+// std::thread::sleep(std::time::Duration::from_millis(3000));
+
+// Ok(())
+// }
+
+#[tokio::test]
+async fn test_maintain_buffer() -> Result<(), Box> {
+ let mut buffer = crate::HashMap::::new();
+ let mut clock = r2r::Clock::create(r2r::ClockType::RosTime).unwrap();
+ let now = clock.get_now().unwrap();
+ let current_time = r2r::Clock::to_builtin_time(&now);
+
+ let dummy_active_folder_fd = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_active_folder".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: current_time.clone()
+ };
+
+ buffer.insert(
+ "dummy_active_folder".to_string(),
+ dummy_active_folder_fd.clone()
+ );
+
+ let dummy_static_folder_fd = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_static_folder".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: false,
+ },
+ folder_loaded: true,
+ time_stamp: current_time.clone()
+ };
+
+ buffer.insert(
+ "dummy_static_folder".to_string(),
+ dummy_static_folder_fd.clone()
+ );
+
+ let dummy_active_in_tf_fd = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_active_in_tf".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: false,
+ time_stamp: current_time.clone()
+ };
+
+ buffer.insert(
+ "dummy_active_in_tf".to_string(),
+ dummy_active_in_tf_fd.clone()
+ );
+
+ let dummy_static_in_tf_fd = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_static_in_tf".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: false,
+ },
+ folder_loaded: false,
+ time_stamp: current_time.clone()
+ };
+
+ buffer.insert(
+ "dummy_static_in_tf".to_string(),
+ dummy_static_in_tf_fd.clone()
+ );
+
+ let ctx = r2r::Context::create().unwrap();
+ let mut node = r2r::Node::create(ctx, crate::NODE_ID, "").unwrap();
+
+ let buffer_maintain_timer = node.create_wall_timer(std::time::Duration::from_millis(crate::BUFFER_MAINTAIN_RATE)).unwrap();
+ let buffer_arc = crate::Arc::new(crate::Mutex::new(buffer));
+
+ // spawn a tokio task to maintain the buffer
+ let buffered_frames_clone_1 = buffer_arc.clone();
+ let handle1 = tokio::task::spawn(async move {
+ match crate::maintain_buffer(buffer_maintain_timer, &buffered_frames_clone_1).await {
+ Ok(()) => (),
+ Err(e) => r2r::log_error!(crate::NODE_ID, "Buffer maintainer failed with: '{}'.", e),
+ };
+ });
+
+ // keep the node alive
+ let handle = std::thread::spawn(move || loop {
+ node.spin_once(std::time::Duration::from_millis(1000));
+ });
+
+ let buffered_frames_clone_2 = buffer_arc.clone();
+
+ let mut t: u64 = 0;
+ loop {
+ std::thread::sleep(std::time::Duration::from_millis(100));
+ let mut frames = buffered_frames_clone_2.lock().unwrap().clone();
+
+ if t < 50 { // all frames should be here for 5 more seconds
+ assert_eq!(frames.get("dummy_active_folder"), Some(&dummy_active_folder_fd));
+ assert_eq!(frames.get("dummy_static_folder"), Some(&dummy_static_folder_fd));
+ assert_eq!(frames.get("dummy_active_in_tf"), Some(&dummy_active_in_tf_fd));
+ assert_eq!(frames.get("dummy_static_in_tf"), Some(&dummy_static_in_tf_fd));
+ }
+ if t > 71 { // the dummy_active_in_tf should be removed from the buffer after 5 seconds
+ assert_eq!(frames.get("dummy_active_folder"), Some(&dummy_active_folder_fd));
+ assert_eq!(frames.get("dummy_static_folder"), Some(&dummy_static_folder_fd));
+ assert_eq!(frames.get("dummy_active_in_tf"), None);
+ assert_eq!(frames.get("dummy_static_in_tf"), Some(&dummy_static_in_tf_fd));
+ }
+ if t > 101 { // the dummy_static_in_tf should be removed from the buffer after 10 seconds
+ assert_eq!(frames.get("dummy_active_folder"), Some(&dummy_active_folder_fd));
+ assert_eq!(frames.get("dummy_static_folder"), Some(&dummy_static_folder_fd));
+ assert_eq!(frames.get("dummy_active_in_tf"), None);
+ assert_eq!(frames.get("dummy_static_in_tf"), None);
+ handle1.abort();
+ break
+ }
+ t = t + 1;
+ }
+
+ // r2r::log_info!(NODE_ID, "Node started.");
+
+ handle.join().unwrap();
+
+ Ok(())
+}
\ No newline at end of file
diff --git a/scene_manipulation_service/src/tests/cycles.rs b/scene_manipulation_service/src/tests/cycles.rs
new file mode 100644
index 0000000..91aba3c
--- /dev/null
+++ b/scene_manipulation_service/src/tests/cycles.rs
@@ -0,0 +1,782 @@
+#![allow(unused_imports)]
+#![allow(dead_code)]
+use r2r::geometry_msgs::msg::{Quaternion, Transform, Vector3};
+
+#[tokio::test]
+async fn test_get_frame_children() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+
+ assert_eq!(crate::get_frame_children("world", &buffer), vec!("dummy_1"));
+
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+ // |
+ // d2
+
+ assert_eq!(
+ crate::get_frame_children("dummy_1", &buffer).sort(),
+ vec!("dummy_2").sort()
+ );
+
+ buffer.insert(
+ "dummy_3".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+ // / \
+ // d2 d3
+
+ assert_eq!(
+ crate::get_frame_children("dummy_1", &buffer).sort(),
+ vec!("dummy_2, dummy_3").sort()
+ );
+
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ //
+ // d1
+ // // \
+ // d2 d3
+
+ assert_eq!(
+ crate::get_frame_children("world", &buffer),
+ Vec::::new()
+ );
+}
+
+#[tokio::test]
+async fn test_is_cyclic_1() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // |
+ // d2
+
+ let res = crate::is_cyclic("dummy_1", &buffer);
+ assert_eq!(res.0, false);
+
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // //
+ // d2
+
+ let res = crate::is_cyclic("dummy_1", &buffer);
+ assert_eq!(res.0, true);
+}
+
+#[tokio::test]
+async fn test_is_cyclic_2() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // |
+ // d2
+
+ let res = crate::is_cyclic("dummy_1", &buffer);
+ assert_eq!(res.0, false);
+
+ let res = crate::is_cyclic("dummy_2", &buffer);
+ assert_eq!(res.0, false);
+
+ buffer.insert(
+ "dummy_3".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // /
+ // d2 -- d3
+
+ let res = crate::is_cyclic("dummy_1", &buffer);
+ assert_eq!(res.0, false);
+
+ let res = crate::is_cyclic("dummy_2", &buffer);
+ assert_eq!(res.0, false);
+
+ let res = crate::is_cyclic("dummy_3", &buffer);
+ assert_eq!(res.0, false);
+
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_3".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // / \
+ // d2 -- d3
+
+ let res = crate::is_cyclic("dummy_1", &buffer);
+ assert_eq!(res.0, true);
+
+ let res = crate::is_cyclic("dummy_2", &buffer);
+ assert_eq!(res.0, true);
+
+ let res = crate::is_cyclic("dummy_3", &buffer);
+ assert_eq!(res.0, true);
+}
+
+#[tokio::test]
+async fn test_is_cyclic_all_1() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // |
+ // d2
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_3".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // /
+ // d2 -- d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_3".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // / \
+ // d2 -- d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res.unwrap().0, true);
+}
+
+#[tokio::test]
+async fn test_is_cyclic_all_2() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+ // |
+ // d2
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_3".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w
+ // |
+ // d1
+ // / \
+ // d2 d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_5".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_4".to_string(),
+ child_frame_id: "dummy_5".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w d4
+ // | |
+ // d d5
+ // / \
+ // d2 d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_6".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_5".to_string(),
+ child_frame_id: "dummy_6".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w d4
+ // | |
+ // d d5
+ // / \ |
+ // d2 d3 d6
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+
+ buffer.insert(
+ "dummy_4".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_6".to_string(),
+ child_frame_id: "dummy_4".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w d4
+ // | / \
+ // d d5 -- d6
+ // / \
+ // d2 d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res.unwrap().0, true);
+
+ buffer.insert(
+ "dummy_4".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_4".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // w---------d4
+ // | /
+ // d d5 -- d6
+ // / \
+ // d2 d3
+
+ let res = crate::is_cyclic_all(&buffer);
+ assert_eq!(res, None);
+}
+
+#[tokio::test]
+async fn test_check_would_produce_cycle() {
+ let mut buffer = crate::HashMap::::new();
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ // d1
+ // |
+ // d2
+
+ let frame_3 = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ };
+
+ // d1
+ // /
+ // d2 -- d3
+
+ let res = crate::check_would_produce_cycle( &frame_3,&buffer);
+ assert_eq!(res, None);
+
+ let frame_1 = crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_2".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ };
+
+ // d1
+ // ||
+ // d2
+
+ let res = crate::check_would_produce_cycle( &frame_1,&buffer);
+ assert_eq!(res.unwrap().0, true);
+}
diff --git a/scene_manipulation_service/src/tests/loading.rs b/scene_manipulation_service/src/tests/loading.rs
new file mode 100644
index 0000000..e1913f6
--- /dev/null
+++ b/scene_manipulation_service/src/tests/loading.rs
@@ -0,0 +1,212 @@
+#![allow(unused_imports)]
+#![allow(dead_code)]
+use serde_json::json;
+use std::io::Write;
+
+fn make_frame(name: &str, x: f64, y: f64, z: f64) -> String {
+ json!({
+ "active": true,
+ "child_frame_id": name,
+ "parent_frame_id": "world",
+ "transform": {
+ "translation": {
+ "x": x,
+ "y": y,
+ "z": z
+ },
+ "rotation": {
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0,
+ "w": 1.0
+ }
+ }
+ })
+ .to_string()
+}
+
+#[tokio::test]
+async fn test_list_frames_in_dir() {
+ // create the temp directory to store the dummy files in
+ let dir = match tempfile::tempdir() {
+ Ok(d) => d,
+ Err(e) => {
+ println!(
+ "Failed to generate temporary dummy directory with: '{}'.",
+ e
+ );
+ panic!()
+ }
+ };
+
+ // create the temporary dummy files
+ let dummy_file_1 = dir.path().join("dummy_1.urdf");
+ let dummy_file_2 = dir.path().join("dummy_2.txt");
+ let dummy_file_3 = dir.path().join("dummy_3.json");
+ let pathbufs = vec![
+ dummy_file_1.clone(),
+ dummy_file_2.clone(),
+ dummy_file_3.clone(),
+ ];
+ pathbufs.iter().for_each(|pb| {
+ crate::File::create(pb.clone()).unwrap();
+ });
+
+ let mut list =
+ crate::list_frames_in_dir(&format!("{}", dir.path().as_os_str().to_str().unwrap())).await;
+
+ assert_eq!(
+ list.sort(),
+ vec!(
+ dummy_file_1.as_path().as_os_str().to_str().unwrap(),
+ dummy_file_2.as_path().as_os_str().to_str().unwrap(),
+ dummy_file_3.as_path().as_os_str().to_str().unwrap()
+ )
+ .sort()
+ );
+
+ pathbufs.iter().for_each(|x| drop(x));
+
+ match dir.close() {
+ Ok(()) => (),
+ Err(_) => panic!(),
+ };
+}
+
+#[tokio::test]
+async fn test_load_scenario() {
+ // create the temp directory to store the dummy files in
+ let dir = match tempfile::tempdir() {
+ Ok(d) => d,
+ Err(e) => {
+ println!(
+ "Failed to generate temporary dummy directory with: '{}'.",
+ e
+ );
+ panic!()
+ }
+ };
+
+ // create the temporary dummy files
+ let dummy_file_1 = dir.path().join("dummy_1.json");
+ let dummy_file_2 = dir.path().join("dummy_2.json");
+ let dummy_file_3 = dir.path().join("dummy_3.json");
+
+ let mut file_1 = crate::File::create(dummy_file_1.clone()).unwrap();
+ write!(file_1, "{}", make_frame("dummy_1", 1.0, 0.0, 0.0)).unwrap();
+ let mut file_2 = crate::File::create(dummy_file_2.clone()).unwrap();
+ write!(file_2, "{}", make_frame("dummy_2", 0.0, 1.0, 0.0)).unwrap();
+ let mut file_3 = crate::File::create(dummy_file_3.clone()).unwrap();
+ write!(file_3, "{}", make_frame("dummy_3", 0.0, 0.0, 1.0)).unwrap();
+
+ let mut list =
+ crate::list_frames_in_dir(&format!("{}", dir.path().as_os_str().to_str().unwrap())).await;
+
+ assert_eq!(
+ list.sort(),
+ vec!(
+ dummy_file_1.as_path().as_os_str().to_str().unwrap(),
+ dummy_file_2.as_path().as_os_str().to_str().unwrap(),
+ dummy_file_3.as_path().as_os_str().to_str().unwrap()
+ )
+ .sort()
+ );
+
+ let scenario = crate::load_scenario(&list).await;
+ let dummy_1_timestamp = scenario.get("dummy_1").unwrap().time_stamp.clone();
+ let dummy_2_timestamp = scenario.get("dummy_2").unwrap().time_stamp.clone();
+ let dummy_3_timestamp = scenario.get("dummy_3").unwrap().time_stamp.clone();
+ let mut test_scenario = crate::HashMap::::new();
+
+ test_scenario.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.0,
+ y: 0.0,
+ z: 0.0,
+ w: 1.0,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: dummy_1_timestamp,
+ },
+ );
+
+ test_scenario.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 0.0,
+ y: 1.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.0,
+ y: 0.0,
+ z: 0.0,
+ w: 1.0,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: dummy_2_timestamp,
+ },
+ );
+
+ test_scenario.insert(
+ "dummy_3".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_3".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 0.0,
+ y: 0.0,
+ z: 1.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.0,
+ y: 0.0,
+ z: 0.0,
+ w: 1.0,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: dummy_3_timestamp,
+ },
+ );
+
+ assert_eq!(scenario.get("dummy_1").unwrap().clone(), test_scenario.get("dummy_1").unwrap().clone());
+ assert_eq!(scenario.get("dummy_2").unwrap().clone(), test_scenario.get("dummy_2").unwrap().clone());
+ assert_eq!(scenario.get("dummy_3").unwrap().clone(), test_scenario.get("dummy_3").unwrap().clone());
+
+ drop(dummy_file_1);
+ drop(dummy_file_2);
+ drop(dummy_file_3);
+
+ match dir.close() {
+ Ok(()) => (),
+ Err(_) => panic!(),
+ };
+}
+
diff --git a/scene_manipulation_service/src/tests/lookup.rs b/scene_manipulation_service/src/tests/lookup.rs
new file mode 100644
index 0000000..b1630ef
--- /dev/null
+++ b/scene_manipulation_service/src/tests/lookup.rs
@@ -0,0 +1,82 @@
+#![allow(unused_imports)]
+#![allow(dead_code)]
+use r2r::geometry_msgs::msg::{Quaternion, Transform, Vector3};
+
+#[tokio::test]
+async fn test_lookup() {
+ let mut buffer = crate::HashMap::::new();
+
+ buffer.insert(
+ "dummy_1".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "world".to_string(),
+ child_frame_id: "dummy_1".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 0.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ buffer.insert(
+ "dummy_2".to_string(),
+ crate::ExtendedFrameData {
+ frame_data: crate::FrameData {
+ parent_frame_id: "dummy_1".to_string(),
+ child_frame_id: "dummy_2".to_string(),
+ transform: r2r::geometry_msgs::msg::Transform {
+ translation: r2r::geometry_msgs::msg::Vector3 {
+ x: 0.0,
+ y: 0.0,
+ z: 1.0,
+ },
+ rotation: r2r::geometry_msgs::msg::Quaternion {
+ x: 0.8596911,
+ y: 0.0256624,
+ z: 0.2951178,
+ w: -0.4161468,
+ },
+ },
+ active: true,
+ },
+ folder_loaded: true,
+ time_stamp: crate::Time { sec: 0, nanosec: 0 },
+ },
+ );
+
+ let buffer_arc = crate::Arc::new(crate::Mutex::new(buffer));
+
+ let res = crate::lookup_transform("world", "dummy_2", &buffer_arc)
+ .await
+ .unwrap();
+ assert_eq!(
+ res,
+ Transform {
+ translation: Vector3 {
+ x: 1.0,
+ y: 0.0,
+ z: 1.0
+ },
+ rotation: Quaternion {
+ x: 0.5385984008512792,
+ y: 0.03972973362897914,
+ z: -0.5891401367376571,
+ w: -0.6010384431556353
+ }
+ }
+ );
+}
diff --git a/scene_manipulation_service/src/tests/mod.rs b/scene_manipulation_service/src/tests/mod.rs
new file mode 100644
index 0000000..fa18594
--- /dev/null
+++ b/scene_manipulation_service/src/tests/mod.rs
@@ -0,0 +1,5 @@
+mod loading;
+// mod quaternion;
+mod lookup;
+// mod cycles;
+// mod buffer;
\ No newline at end of file
diff --git a/scene_manipulation_service/src/tests/quaternion.rs b/scene_manipulation_service/src/tests/quaternion.rs
new file mode 100644
index 0000000..8656d97
--- /dev/null
+++ b/scene_manipulation_service/src/tests/quaternion.rs
@@ -0,0 +1,88 @@
+#![allow(unused_imports)]
+#![allow(dead_code)]
+use r2r::geometry_msgs::msg::Quaternion;
+use glam::Quat;
+
+#[tokio::test]
+async fn test_multiply() {
+ let q1 = r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023
+ };
+ let q2 = r2r::geometry_msgs::msg::Quaternion {
+ x: 0.8596911,
+ y: 0.0256624,
+ z: 0.2951178,
+ w: -0.4161468
+ };
+
+ // order reversed here because in the function we reverse the chain
+ let q3 = crate::multiply_quaternion(&q2, &q1);
+ assert_eq!(q3, r2r::geometry_msgs::msg::Quaternion {
+ x: 0.53859841651413,
+ y: 0.03972973478434999,
+ z: -0.5891401538703,
+ w: -0.60103846063429
+ });
+}
+
+#[tokio::test]
+async fn test_glam_mul() {
+ let q1 = Quat::from_xyzw(0.2657277, 0.6643192, 0.4428795, 0.5403023);
+ let q2 = Quat::from_xyzw(0.8596911, 0.0256624, 0.2951178, -0.4161468);
+
+ // order reversed here because in the function we reverse the chain
+ let q3 = q1.mul_quat(q2);
+ println!("{:?}", q3);
+}
+
+#[tokio::test]
+async fn test_glam_mul_and_norm() {
+ let q1 = Quat::from_xyzw(0.2657277, 0.6643192, 0.4428795, 0.5403023);
+ let q2 = Quat::from_xyzw(0.8596911, 0.0256624, 0.2951178, -0.4161468);
+
+ // order reversed here because in the function we reverse the chain
+ let q3 = q1.mul_quat(q2);
+ println!("{:?}", q3);
+ println!("{:?}", q3.normalize());
+}
+
+#[tokio::test]
+async fn test_glam_mul_and_norm_and_inverse() {
+ let q1 = Quat::from_xyzw(0.2657277, 0.6643192, 0.4428795, 0.5403023);
+ let q2 = Quat::from_xyzw(0.8596911, 0.0256624, 0.2951178, -0.4161468);
+
+ // order reversed here because in the function we reverse the chain
+ let q3 = q1.mul_quat(q2);
+ println!("{:?}", q3);
+ println!("{:?}", q3.normalize());
+ println!("{:?}", q3.normalize().inverse());
+}
+
+#[tokio::test]
+async fn test_multiply_and_normalize() {
+ let q1 = r2r::geometry_msgs::msg::Quaternion {
+ x: 0.2657277,
+ y: 0.6643192,
+ z: 0.4428795,
+ w: 0.5403023
+ };
+ let q2 = r2r::geometry_msgs::msg::Quaternion {
+ x: 0.8596911,
+ y: 0.0256624,
+ z: 0.2951178,
+ w: -0.4161468
+ };
+
+ // order reversed here because in the function we reverse the chain
+ let q3 = crate::multiply_and_normalize(&q2, &q1);
+ assert_eq!(q3, r2r::geometry_msgs::msg::Quaternion {
+ x: 0.5385984008512792,
+ y: 0.03972973362897914,
+ z: -0.5891401367376571,
+ w: -0.6010384431556353
+ });
+}
+